File content as of revision 3:3c00f1332d9b:

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

#define NUM_INPUTS 10
#define NUM_OUTPUTS 9

//leg 50 deg
//tail 45 deg

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 = 20;

// 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

// 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;


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();
        
        // 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;         
        }
    }

int main (void) {
    // Link the terminal with our server and start it up
    server.attachTerminal(pc);
    server.init();

    // 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];
            
            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{
                    taild = tailCmd;
                    legd = legCmd;
                }
                
                // 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;
                    server.sendData(output_data,NUM_OUTPUTS);
                    new_data = false;
                }
            }     
            end = true;  
            // Cleanup after experiment
            server.setExperimentComplete();
            tailPWM.write(0);
            legPWM.write(0);
        } // end if
    } // end while
} // end main