File content as of revision 3:646dc3b2a681:

#include "mbed.h"
#include "rtos.h"
#include "EthernetInterface.h"
#include "ExperimentServer.h"
#include "QEI.h"
#include "FXOS8700Q.h"

#define NUM_INPUTS 10
#define NUM_OUTPUTS 11

// States
#define INIT 0
#define STANCE 1
#define FLIGHT 2

//Debug
//DigitalOut led1(LED1);

// Initialize accelerometer
FXOS8700Q_acc acc( PTE25, PTE24, FXOS8700CQ_SLAVE_ADDR1);
MotionSensorDataUnits acc_data;

Ticker control;
Serial pc(USBTX, USBRX);    // USB Serial Terminal
ExperimentServer server;    // Object that lets us communicate with MATLAB

// Tail Motor Control Pins
PwmOut tailPWM(D5);        // Tail motor PWM output
DigitalOut tailFwd(D6);    // Tail motor forward enable
DigitalOut tailRev(D7);    // Tail motor backward enable

// Leg Motor Control Pins
PwmOut legPWM(D11);         // Leg motor PWM output
DigitalOut legFwd(D10);    // Leg motor forward enable
DigitalOut legRev(D9);     // Leg motor backward enable

Timer t;                   // Timer to measure elapsed time of experiment
float time_ = 0.0;               // time variable
float tfinal = 0.0;

// Motor current sensors
AnalogIn tailCurrent(A0);  // Tail current sensor
AnalogIn legCurrent(A1);   // Leg current sensor 

// Motor Encoders
QEI tailEncoder(D1 ,D2 ,NC ,1200, QEI::X4_ENCODING); // Tail encoder 
QEI legEncoder(D3 ,D4 ,NC ,1200, QEI::X4_ENCODING);  // Leg encoder

// Physical motor parameters
float R = 2.79;                     // Estimated motor resistance from lab
float Kb = .1603;                   // Estimated motor torque constant measured from lab
float Kcurrent = 1.5;                 // Proportional gain of the current control 
float v = 0.01;                     // BackEMF constant


// Matlab inputs/Control variables
float tailCmd = 0.0;                  // Commanded angle (Rads) for the tail
float legCmd = 0.0;                   // Commanded angle (Rads) for the leg
float tailt0 = 0.0;                   // Initial angle (Rads)
float legt0 = 0.0;                    // Initial angle (Rads)
float Kptail = 0.0;                   // Tail proportional gain of position control
float Kpleg = 0.0;                    // Leg proportional gain of position control
float Kdtail = 0.0;                   // Tail derivative gain of position control
float Kdleg = 0.0;                    // Leg derivative gain for position control

// State varibles/sensed parameters
float idtail = 0.0;                   // Tail desired current
float idleg = 0.0;                    // Leg desired current
float taild = 0.0;                    // Desired angle (Rads) for the tail
float legd = 0.0;                     // Desired angle (Rads) for the leg
float wtail = 0.0;                    // Measured angular velocity of the tail
float wleg = 0.0;                     // Measured angular velocity of the leg
float ttail = 0.0;                    // Measured angular position of the tail
float tleg = 0.0;                     // Measured angular position of the leg
float itail = 0.0;                    // Current sensed in the tail motor
float ileg = 0.0;                     // Current sensed in the leg motor
float vctail = 0.0;                   // Tail voltage command
float vcleg = 0.0;                    // Leg voltage command
float faX, faY, faZ;                  // Accelerometer data
float mg = 0.0;                       // Acceleration Magnitude
float mgprev = 0.0;                   // Previous Acceleration Magnitude
float impact = 0.0;                   // Impact event detection variable

// Error terms
float etail = 0.0;                    // Error term for tail
float eleg = 0.0;                     // Error term for leg
float eptail = 0.0;                   // Previous error term for tail
float epleg = 0.0;                    // Previous error term for leg
float edtail = 0.0;                   // Derivative error term for tail
float edleg = 0.0;                    // Derivative error term for leg

float max_cmd = 1.0;

bool new_data = false;
bool end = false;

int state = INIT;
uint32_t tdebounce = 0;
uint32_t ttouchdown = 0;

void control_loop() {
    if (end){
        tailPWM.write(0);
        legPWM.write(0);
        }    
    else{
        //Sensing
        itail = (tailCurrent.read()-.5)*36.7;                                 // Tail current
        ileg = (legCurrent.read()-.5)*36.7;                                   // Leg current
        ttail = tailEncoder.getPulses()/1200.0*2*3.14159 + tailt0;            // Leg angle
        tleg = legEncoder.getPulses()/1200.0*2*3.14159 + legt0;               // Leg angle
        wtail = tailEncoder.getVelocity()*2*3.14159/1200.0;                   // Tail angular velocity
        wleg = legEncoder.getVelocity()*2*3.14159/1200.0;                     // Leg angular velocity
        time_ = t.read();                                                     // Time(sec)
        acc.getAxis(acc_data);                                                // Accelerometer Data
        acc.getX(&faX);
        acc.getY(&faY);
        acc.getZ(&faZ);
        
        // Processing the accelerometer data
        mg = sqrt(faX*faX+faY*faY+faZ*faZ);                                     // Magnitude Data                           
        impact = (mg - mgprev)/.001;                                            // Change in magnitude
        mgprev = mg;                                                            // Update previous magnitude
        
        // Check for state change
        if(state == FLIGHT && impact > 500 && t.read_ms()-tdebounce > 100){
            state = STANCE;
            tdebounce = t.read_ms();
            ttouchdown = t.read_ms(); 
        }else if(state == STANCE && mg < .7 && t.read_ms()-tdebounce > 100){
            state = FLIGHT;
            tdebounce = t.read_ms();
        }
        
        if(state == INIT){
            // Error update
            etail = taild - ttail;                                  // Error in tail's angle position
            eleg = legd - tleg;                                     // Error in leg's angle position
            edtail = (etail - eptail)/.001;                         // Rate of change in tail's error
            edleg = (eleg - epleg)/.001;                            // Rate of change in leg's error
            eptail = etail;                                         // Update previous tail error
            epleg = eleg;                                           // Update previous leg error
            
            // Perform control loop logic
            idtail = (Kptail*etail + Kdtail*edtail + v*wtail) / Kb;         // Tail position control
            idleg = (Kpleg*eleg + Kdleg*edleg + v*wleg) / Kb;              // Leg position control
            
            vctail = (R*idtail + Kcurrent * (idtail - itail) - Kb * wtail) / 24.0;  // Tail current control
            vcleg = (R*idleg + Kcurrent * (idleg - ileg) - Kb * wleg) / 24.0;      // Leg current control
            
            // Tail set command
            if (vctail < 0){
                tailFwd = 1;
                tailRev = 0;
                if (abs(vctail) > 1){
                    vctail = -1;
                }
                tailPWM.write(max_cmd*abs(vctail));
            }else if (vctail > 0){
                tailFwd = 0;
                tailRev = 1;
                if (abs(vctail) > 1){
                    vctail = 1;
                }
                tailPWM.write(max_cmd*abs(vctail));
            } else {
                tailPWM.write(0);
            }      
            
            // Leg set command
            if (vcleg < 0){
                legFwd = 1;
                legRev = 0;
                if (abs(vcleg) > 1){
                    vcleg = -1;
                }
                legPWM.write(max_cmd*abs(vcleg));
            }else if (vcleg > 0){
                legFwd = 0;
                legRev = 1;
                if (abs(vcleg) > 1){
                    vcleg = 1;
                }
                legPWM.write(max_cmd*abs(vcleg));
            } else {
                legPWM.write(0);
            }   
            new_data = true;
            
        }else if (state == STANCE){
            //led1 = false;
            
            legd = 3.14159/80;      //Set the leg angle for SLIP
            taild = -3.14159/12*cos(2*3.14159*(t.read_ms()-ttouchdown)/1000.0);  //Set the tail angle based on cos
            
            // Error update
            etail = taild - ttail;                                  // Error in tail's angle position
            eleg = legd - tleg;                                     // Error in leg's angle position
            edtail = (etail - eptail)/.001;                         // Rate of change in tail's error
            edleg = (eleg - epleg)/.001;                            // Rate of change in leg's error
            eptail = etail;                                         // Update previous tail error
            epleg = eleg;                                           // Update previous leg error
            
            // Tail Control
            idtail = (Kptail*etail + Kdtail*edtail + v*wtail)/ Kb;
            vctail = (R*idtail + Kcurrent * (idtail - itail) - Kb * wtail) / 24.0;
            
            // Leg Control
            idleg = (Kpleg*eleg + Kdleg*edleg + v*wleg) / Kb;
            vcleg = (R*idleg + Kcurrent * (idleg - ileg) - Kb * wleg) / 24.0;
            
            // Tail set command
            if (vctail < 0){
                tailFwd = 1;
                tailRev = 0;
                if (abs(vctail) > 1){
                    vctail = -1;
                }
                tailPWM.write(max_cmd*abs(vctail));
            }else if (vctail > 0){
                tailFwd = 0;
                tailRev = 1;
                if (abs(vctail) > 1){
                    vctail = 1;
                }
                tailPWM.write(max_cmd*abs(vctail));
            } else {
                tailPWM.write(0);
            }      
            
            // Leg set command
            if (vcleg < 0){
                legFwd = 1;
                legRev = 0;
                if (abs(vcleg) > 1){
                    vcleg = -1;
                }
                legPWM.write(max_cmd*abs(vcleg));
            }else if (vcleg > 0){
                legFwd = 0;
                legRev = 1;
                if (abs(vcleg) > 1){
                    vcleg = 1;
                }
                legPWM.write(max_cmd*abs(vcleg));
            } else {
                legPWM.write(0);
            }   
            new_data = true;
        }else if (state == FLIGHT){
          //  led1 = true;
            // Set desired angles
            legd = -3.14159/50.0;
            taild = -3.14159/16.0;
            
            // Error update
            etail = taild - ttail;                                  // Error in tail's angle position
            eleg = legd - tleg;                                     // Error in leg's angle position
            edtail = (etail - eptail)/.001;                         // Rate of change in tail's error
            edleg = (eleg - epleg)/.001;                            // Rate of change in leg's error
            eptail = etail;                                         // Update previous tail error
            epleg = eleg;                                           // Update previous leg error
            
            // Tail Control
            idtail = (Kptail*etail + Kdtail*edtail + v*wtail) / Kb;
            vctail = (R*idtail + Kcurrent * (idtail - itail) - Kb * wtail) / 24.0;
            // Leg Control
            idleg = (Kpleg*eleg + Kdleg*edleg + v*wleg) / Kb;
            vcleg = (R*idleg + Kcurrent * (idleg - ileg) - Kb * wleg) / 24.0;
            
            // Tail set command
            if (vctail < 0){
                tailFwd = 1;
                tailRev = 0;
                if (abs(vctail) > 1){
                    vctail = -1;
                }
                tailPWM.write(max_cmd*abs(vctail));
            }else if (vctail > 0){
                tailFwd = 0;
                tailRev = 1;
                if (abs(vctail) > 1){
                    vctail = 1;
                }
                tailPWM.write(max_cmd*abs(vctail));
            } else {
                tailPWM.write(0);
            }      
            
            // Leg set command
            if (vcleg < 0){
                legFwd = 1;
                legRev = 0;
                if (abs(vcleg) > 1){
                    vcleg = -1;
                }
                legPWM.write(max_cmd*abs(vcleg));
            }else if (vcleg > 0){
                legFwd = 0;
                legRev = 1;
                if (abs(vcleg) > 1){
                    vcleg = 1;
                }
                legPWM.write(max_cmd*abs(vcleg));
            } else {
                legPWM.write(0);
            }   
            new_data = true;
        }
    }
}

int main (void) {
    //led1 = false;
    
    // Link the terminal with our server and start it up
    server.attachTerminal(pc);
    server.init();
    
    // Accelerometer
    acc.enable();
    
    // PWM period should nominally be a multiple of our control loop
    tailPWM.period_us(500);
    legPWM.period_us(500);
    
    // Continually get input from MATLAB and run experiments
    float input_params[NUM_INPUTS];
    control.attach(&control_loop, .001);
    
    
    while(1) {
        if (server.getParams(input_params,NUM_INPUTS)) {
            tailCmd = input_params[0];                   // Desired angle (Rads) for the tail
            legCmd = input_params[1];                    // Desired angle (Rads) for the leg
            tailt0 = input_params[2];                    // Initial angle (Rads)
            legt0 = input_params[3];                     // Initial angle (Rads)
            Kptail = input_params[4];                    // Tail proportional gain of position control
            Kpleg = input_params[5];                     // Leg proportional gain of position control
            Kdtail = input_params[6];                    // Tail derivative gain of position control
            Kdleg = input_params[7];                     // Leg derivative gain for position control
            max_cmd = input_params[8];                   // cmd thresholder
            tfinal = input_params[9];
            
            state = INIT;
            
            if (max_cmd > 1.0 || max_cmd < 0){
                max_cmd = 1.0;
                }
            // Setup experiment
            t.reset();
            t.start();
            end = false;
            
            // Reset the encoders
            tailEncoder.reset();
            legEncoder.reset();
            
            // Default forward rotation
            tailFwd = 1;
            tailRev = 0;
            legFwd = 1;
            legRev = 0;
            
            // Stop the motors
            tailPWM.write(0);
            legPWM.write(0);
            
            // Run experiment
            while( t.read() < tfinal) { 
                // Set desired parameters at specific times
                if(t.read()<5){
                    taild = tailt0;
                    legd = legt0;
                }else if(t.read() < 10 && state == INIT){
                    taild = tailCmd;
                    legd = legCmd;
                } else{
                 state = STANCE;    
                }
               // if(state = STANCE){
//                
//                }
                
                // Send data to MATLAB
                if(new_data) {
                    // Form output to send to MATLAB    
                    float output_data[NUM_OUTPUTS];
                    output_data[0] = time_;
                    output_data[1] = vctail;
                    output_data[2] = vcleg;
                    output_data[3] = itail;
                    output_data[4] = ileg;
                    output_data[5] = ttail;
                    output_data[6] = tleg;
                    output_data[7] = wtail;
                    output_data[8] = wleg;
                    output_data[9] = impact;
                    output_data[10] = mg;
                    server.sendData(output_data,NUM_OUTPUTS);
                    new_data = false;
                }
            }     
            end = true;  
            // Cleanup after experiment
            server.setExperimentComplete();
            tailPWM.write(0);
            legPWM.write(0);
            state = INIT;
        } // end if
    } // end while
} // end main