File content as of revision 0:fc382eeb78ad:

#include "mbed.h"
#include "QEI.h"

#define CONTROL_PERIOD 0.002 // 500Hz ***
#define SAVE_PERIOD 0.005 // 200HZ

Ticker tick;
Ticker tock;
Timer t;

Serial pc(USBTX, USBRX); // tx, rx
LocalFileSystem local("data");               // Create the local filesystem under the name "local"

// Declare Three Encoders
QEI rear_encoder(p22, p23, NC, 1200, QEI::X4_ENCODING);  // rear leg
QEI front_encoder(p5, p6, NC, 1200, QEI::X4_ENCODING);  // front leg
QEI spine_encoder(p9, p10, NC, 1200, QEI::X4_ENCODING);  // spine

// Specify pinout
DigitalOut  rear_motorA(p15);
DigitalOut  rear_motorB(p16);
PwmOut      rear_motorPWM(p24);
AnalogIn    rear_cs(p20);

DigitalOut  front_motorA(p7);
DigitalOut  front_motorB(p8);
PwmOut      front_motorPWM(p25);
AnalogIn    front_cs(p19);

DigitalOut  spine_motorA(p11);
DigitalOut  spine_motorB(p12);
PwmOut      spine_motorPWM(p26);
AnalogIn    spine_cs(p18);

//number domains for abstraction
const int rear = 1;
const int front = 2;
const int spine = 3;

// Sensing Variables
volatile int i = 0;
volatile float w = 0;
volatile int id = 4000;
volatile int sign = 0;

volatile int rear_n = 0;
volatile int rear_last_n = 0;
//volatile int rear_i = 0;
//volatile float rear_w = 0;
volatile int front_n = 0;
volatile int front_last_n = 0;
//volatile int front_i = 0;
//volatile float front_w = 0;
volatile int spine_n = 0;
volatile int spine_last_n = 0;
//volatile int spine_i = 0;
//volatile float spine_w = 0;

// Output Variables
volatile float pwm = 0;
//volatile float rear_pwm = 0;
//volatile float front_pwm = 0;

// Control Constants
const float R = 2.3/1000.0; // [kohm]
const float Kv = 0.1682;
const int Vs = 18; // [V]
const float n2d = 3.3333;

// Control Parameters
//volatile int rear_id = 4000; // [mA]
//volatile int front_id = 4000;
//volatile int spine_id = 4000;
float rear_Kp = 0.001;
float rear_Ks_p = 250.0;
float rear_Ks_d = 25.0;

float front_Kp = 0.001;
float front_Ks_p = 250.0;
float front_Ks_d = 25.0;

float spine_Kp = 0.001;
float spine_Ks_p = 200.0;
float spine_Ks_d = 20.0;

float rear_n_d = 0.0*n2d;
float front_n_d = 0.0*n2d;
float spine_n_d = 0.0*n2d;
float rear_w_d = 0;
float front_w_d = 0;
float spine_w_d = 0;

FILE *fp = fopen("/data/out.txt", "w");  // Open "out.txt" on the local file system for writing

int read_current(int which_domain) {
    int current = 0;
    if (which_domain == 1) {                // rear
        current = rear_cs.read()*23570;
    } else if (which_domain == 2) {         // front
        current = front_cs.read()*23570;
    } else if (which_domain == 3){          // spine
        current = spine_cs.read()*23570;
    }
    return current; //mA
}

void updateMotor(int which_motor, float power) {
    int dir = 0;
    
    if (power < 0) { 
        power = -power; 
        dir = 0;
    } else {
        dir = 1;
    }
    if (power > 1) { power = 1; }
    if (power < 0) { power = 0; }
    
    if (which_motor == 1) {                 // rear
        if (dir == 1) {
            rear_motorA = 0;
            rear_motorB = 1;
        } else {
            rear_motorA = 1;
            rear_motorB = 0;
        }
        rear_motorPWM.write(power);
    } else if (which_motor == 2) {          // front
        if (dir == 1) {
            front_motorA = 0;
            front_motorB = 1;
        } else {
            front_motorA = 1;
            front_motorB = 0;
        }
        front_motorPWM.write(power);
    } else if (which_motor == 3) {          // spine
        if (dir == 1) {
            spine_motorA = 0;
            spine_motorB = 1;
        } else {
            spine_motorA = 1;
            spine_motorB = 0;
        }
        spine_motorPWM.write(power);
    }
}

//void updateEncoder(int which_encoder) {
//    last_n = n;
//    n = encoder.getPulses();
//    w = (n-last_n)*1.047;       //steps/s --> rad/s
    

void control() {
    // rear
    i = read_current(rear);
    rear_last_n = rear_n;
    rear_n = rear_encoder.getPulses();
    w = (rear_n-rear_last_n)*1.047;       //steps/s --> rad/s
    id = rear_Ks_p*(rear_n_d-rear_n) + rear_Ks_d*(rear_w_d-w);
    sign = abs(id)/id;
    id = abs(id);
    pwm = sign*(id*R-sign*Kv*w+rear_Kp*(id-i))/Vs;
    updateMotor(rear,pwm); 
    
    // front
    i = read_current(front);
    front_last_n = front_n;
    front_n = front_encoder.getPulses();
    w = (front_n-front_last_n)*1.047;       //steps/s --> rad/s
    id = front_Ks_p*(front_n_d-front_n) + front_Ks_d*(front_w_d-w);
    sign = abs(id)/id;
    id = abs(id);
    pwm = sign*(id*R-sign*Kv*w+front_Kp*(id-i))/Vs;
    updateMotor(front,pwm); 
    
    // spine
    i = read_current(spine);
    spine_last_n = spine_n;
    spine_n = spine_encoder.getPulses();
    w = (spine_n-spine_last_n)*1.047;       //steps/s --> rad/s
    id = spine_Ks_p*(spine_n_d-spine_n) + spine_Ks_d*(spine_w_d-w);
    sign = abs(id)/id;
    id = abs(id);
    pwm = sign*(id*R-sign*Kv*w+spine_Kp*(id-i))/Vs;
    updateMotor(spine,pwm); 
     
// Position CURRENT CONTROL
//    if (t.read() < (0.2+0.25*(num_jumps+1))) {
//        n_d = -60.0/360.0*1200.0;
//        id = Ks_p*(n_d-n) + Ks_d*(w_d-w);
//        int sign = abs(id)/id;
//        id = abs(id);
//        pwm = sign*(id*R-sign*Kv*w+Kp*(id-i))/Vs;  
//    } else if (t.read() < 0.25*(num_jumps+1)) {
//        n_d = -20.0/360.0*1200.0;
//        id = Ks_p*(n_d-n) + Ks_d*(w_d-w);
//        int sign = abs(id)/id;
//        id = abs(id);
//        pwm = sign*(id*R-sign*Kv*w+Kp*(id-i))/Vs;  
//        num_jumps++;
//    }
  
  // IMPULSE RESPONSE CODE  
//      if (t.read() < 0.2) {
//        id = -10000;
//        int sign = abs(id)/id;
//        id = abs(id);
//        pwm = sign*(id*R-sign*Kv*w+Kp*(id-i))/Vs;
//      } else {
//        pwm = 0;
//      }
  
// PD CURRENT CONTROL
//    id = Ks_p*(n_d-n) + Ks_d*(w_d-w);
//    int sign = abs(id)/id;
//    id = abs(id);
//    pwm = sign*(id*R-sign*Kv*w+Kp*(id-i))/Vs;
    
//  CURRENT CONTROL
//    pwm = (id*R+Kv*w+Kp*(id-i))/Vs;
}

void save() {
    fprintf(fp, "%i %i %i %i\n", t.read_ms(), rear_n, front_n, spine_n);
}

int main() {
    rear_motorPWM.period(0.00005);   //20kHz
    front_motorPWM.period(0.00005);   //20kHz
    spine_motorPWM.period(0.00005);   //20kHz
    
    tick.attach(&control,CONTROL_PERIOD);
    tock.attach(&save,SAVE_PERIOD);
    
    t.start();
    while(1) { 
        //spine_n_d = 0.0;
//        rear_n_d = -15.0*n2d*(t.read_ms()/1000.0);
//        front_n_d = -15.0*n2d*(t.read_ms()/1000.0);
        if (t.read() < 5) {
            //stand up
            spine_n_d = 0.0;
            rear_n_d = -25.0*n2d*(t.read_ms()/5000.0);
            front_n_d = -40.0*n2d*(t.read_ms()/5000.0);
        } else if (t.read() < 10) {
            spine_n_d = 15.0*sin((t.read_ms()-10000)*12.57/1000.0)*n2d;
            rear_n_d = (-40.0+15.0*cos((t.read_ms()-10000)*12.57/1000.0))*n2d;
            front_n_d = (-40.0+15.0*sin((t.read_ms()-10000)*12.57/1000.0))*n2d;
//            spine_n_d = 15.0*sin((t.read_ms()-10000)*12.57/1000.0)*n2d;
//            //jump
//            front_Ks_p = 100.0;
//            rear_Ks_p = 100.0;
//            spine_n_d = 0.0;
//            rear_n_d = -15.0*n2d;
//            front_n_d = -15.0*n2d;
//            wait(1.0);
//            front_Ks_p = 250.0;
//            rear_Ks_p = 250.0;
//            rear_n_d = -30.0*n2d;
//            front_n_d = -30.0*n2d;
//            wait(0.2);
        // arch spine back and forth
        //    spine_n_d = -20.0*n2d*((t.read_ms()-5000)/5000.0);
//            rear_n_d = -40.0*n2d;
//            front_n_d = -40.0*n2d;
//        } else if (t.read() < 15) {
//            spine_n_d = -15.0*n2d*((15000-t.read_ms())/5000.0);
//            rear_n_d = -40.0*n2d;
//            front_n_d = -40.0*n2d;
//        } else if (t.read() < 20) {
//            spine_n_d = -15.0*n2d*((20000-t.read_ms())/5000.0);
//            rear_n_d = -40.0*n2d;
//            front_n_d = -40.0*n2d;
        } else if (t.read() < 15) {
            spine_n_d = 0.0;//20.0*n2d*((25000-t.read_ms())/5000.0);
            rear_n_d = -25.0*n2d*((15000-t.read_ms())/5000.0);
            front_n_d = -40.0*n2d*((15000-t.read_ms())/5000.0);
        } else { 
            fclose(fp);
            pwm = 0;
            updateMotor(rear,pwm); 
            updateMotor(front,pwm);
        }
        
        //DEBUG
//        pc.printf("%i %f %i %f %i %i\n", t.read_ms(), pwm, n, w, id, i);
    }
}