File content as of revision 4:7f9c9bd9da26:

#include "mbed.h"
#include "QEI.h"
#define CONTROL_PERIOD 0.002 // 500Hz ***
#define SAVE_PERIOD 0.005 // 200HZ

const int n_samples = 165;

// 1000 x 3 array of degree values
const float trajectory[n_samples][3] = {
34,10,0,
34,10,0,
34,10,0,
34,10,0,
34,10,0,
34,10,-1,
34,10,-1,
34,10,-1,
34,10,-2,
34,10,-2,
34,10,-2,
34,10,-3,
34,10,-3,
34,10,-3,
33,11,-3,
33,11,-4,
33,11,-4,
33,11,-4,
33,11,-4,
32,12,-4,
32,12,-5,
32,12,-5,
31,13,-5,
31,13,-5,
31,13,-5,
30,14,-5,
30,14,-6,
30,14,-6,
29,15,-6,
29,15,-6,
28,16,-6,
28,16,-6,
27,17,-6,
27,17,-6,
26,18,-6,
26,18,-6,
25,19,-6,
25,19,-6,
24,20,-6,
24,20,-6,
23,21,-6,
23,21,-6,
22,22,-6,
22,22,-6,
21,23,-6,
20,24,-6,
20,24,-6,
19,25,-6,
19,25,-6,
18,26,-6,
18,26,-6,
17,27,-6,
17,27,-6,
16,28,-6,
16,28,-6,
15,29,-6,
15,29,-6,
15,29,-6,
14,30,-6,
14,30,-5,
13,31,-5,
13,31,-5,
13,31,-5,
12,32,-5,
12,32,-5,
12,32,-4,
12,32,-4,
11,33,-4,
11,33,-4,
11,33,-4,
11,33,-3,
10,34,-3,
10,34,-3,
10,34,-3,
10,34,-2,
10,34,-2,
10,34,-2,
10,34,-1,
10,34,-1,
10,34,-1,
10,34,0,
10,34,0,
10,34,0,
10,34,0,
10,34,0,
10,34,0,
10,34,0,
10,34,1,
10,34,1,
10,34,1,
10,34,2,
10,34,2,
10,34,2,
10,34,2,
10,34,3,
10,34,3,
10,34,3,
11,33,3,
11,33,4,
11,33,4,
11,33,4,
12,32,4,
12,32,5,
12,32,5,
12,32,5,
13,31,5,
13,31,5,
13,31,5,
14,30,6,
14,30,6,
15,29,6,
15,29,6,
15,29,6,
16,28,6,
16,28,6,
17,27,6,
17,27,6,
18,26,6,
18,26,6,
19,25,6,
19,25,6,
20,24,6,
20,24,6,
21,23,6,
22,22,6,
22,22,6,
23,21,6,
23,21,6,
24,20,6,
24,20,6,
25,19,6,
25,19,6,
26,18,6,
26,18,6,
27,17,6,
27,17,6,
28,16,6,
28,16,6,
29,15,6,
29,15,6,
30,14,6,
30,14,6,
30,14,5,
31,13,5,
31,13,5,
31,13,5,
32,12,5,
32,12,5,
32,12,4,
33,11,4,
33,11,4,
33,11,4,
33,11,3,
33,11,3,
34,10,3,
34,10,3,
34,10,2,
34,10,2,
34,10,2,
34,10,2,
34,10,1,
34,10,1,
34,10,1,
34,10,0,
34,10,0
};

//control flow
const int standing_time = 250; //samples at 500Hz, time to stand up
const float standing_step = 1/(float)standing_time; //recipherical of standing_time
const int sitting_time = 1000; //samples at 500Hz, time to sit down
const float sitting_step = 1/(float)sitting_time; //recipherical of sitting time
volatile int getting_up = 1; //are we currently in the process of standing up
volatile int getting_down = 0; //are we currently in the process of sitting down
const float standing_position = 34.0;
const float bent_position = 10.0;
const float spine_start = trajectory[0][2];

volatile int current_sample = 0;
volatile int current_loop = 0;
const int n_loops = 12;

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(p23, p14, NC, 1200, QEI::X4_ENCODING);  // rear leg
QEI front_encoder(p21, p16, NC, 1200, QEI::X4_ENCODING);  // front leg
QEI spine_encoder(p22, p15, NC, 1200, QEI::X4_ENCODING);  // spine

// Specify pinout
DigitalOut  rear_motorA(p7);
DigitalOut  rear_motorB(p6);
PwmOut      rear_motorPWM(p26);
AnalogIn    rear_cs(p18);

DigitalOut  front_motorA(p11);
DigitalOut  front_motorB(p10);
PwmOut      front_motorPWM(p24);
AnalogIn    front_cs(p20);

DigitalOut  spine_motorA(p9);
DigitalOut  spine_motorB(p8);
PwmOut      spine_motorPWM(p25);
AnalogIn    spine_cs(p19);

//LEDs for current safety
DigitalOut rear_led(LED1);
DigitalOut front_led(LED2);
DigitalOut spine_led(LED3);

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

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

volatile int n[3] = {0,0,0}; //encoder values
volatile int last_n[3] = {0,0,0}; //previous encoder values

volatile float avg_current[3] = {0.0,0.0,0.0}; //integration of current in time

// Output Variables
volatile float 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;

const float integ_alpha = .05; //peristence of current integration. 0->all past, 1->all present
const float stall_current = 8000; //mA

const float time_out_current = 5000; //mA if avg_current is above, increment the timeout count
const int time_out_steps = 400; //max steps after which kill the test
volatile int time_out_count[3] = {0,0,0};  //counter for time out
int kill_test = 0; //kill switch

// Control Parameters
float rear_Kp = 0.001;
float rear_Ks_p = 220.0;
float rear_Ks_d = 500.0;

float front_Kp = 0.001;
float front_Ks_p = 220.0;
float front_Ks_d = 500.0;

float spine_Kp = 0.001;
float spine_Ks_p = 300.0;
float spine_Ks_d = 400.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 == rear) {                // rear
        current = rear_cs.read()*23570;
    } else if (which_domain == front) {         // front
        current = front_cs.read()*23570;
    } else if (which_domain == spine){          // spine
        current = spine_cs.read()*23570;
    }
    avg_current[which_domain] = (1-integ_alpha)*avg_current[which_domain] + integ_alpha*current;  //integrate
    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 == rear) {                 // 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 == front) {          // 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 == spine) {          // spine
        if (dir == 1) {
            spine_motorA = 0;
            spine_motorB = 1;
        } else {
            spine_motorA = 1;
            spine_motorB = 0;
        }
        spine_motorPWM.write(power);
    }
}

float updateEncoder(int which_encoder) {
    last_n[which_encoder] = n[which_encoder];
    if(which_encoder == rear){
        n[which_encoder] = rear_encoder.getPulses();
    }
    else if(which_encoder == front){
        n[which_encoder] = front_encoder.getPulses();
    }
    else if(which_encoder == spine){
        n[which_encoder] = spine_encoder.getPulses();
    }
    w = (n[which_encoder]-last_n[which_encoder])*1.047;       //steps/s --> rad/s
    return w;
 }

void shutdown(){
    tick.detach();
    tock.detach();
    updateMotor(rear, 0);
    updateMotor(front, 0);
    updateMotor(spine, 0);
    fclose(fp);
    kill_test = 1;
}

void control() {
    if(getting_up){
        rear_n_d = -standing_position*(standing_step*current_sample)*n2d; //linear ramp up over 500 samples
        front_n_d = -bent_position*(standing_step*current_sample)*n2d; //linear ramp up over 500 samples
        spine_n_d = -spine_start*(standing_step*current_sample)*n2d; 
    }
    else if(getting_down){
        rear_n_d = -standing_position*(1-sitting_step*current_sample)*n2d; //linear ramp up over 500 samples
        front_n_d = -bent_position*(1-sitting_step*current_sample)*n2d; //linear ramp up over 500 samples
        spine_n_d = -spine_start*(1-sitting_step*current_sample)*n2d;; 
    }
    else{
        rear_n_d = -trajectory[current_sample][rear]*n2d;
        front_n_d = -trajectory[current_sample][front]*n2d;
        spine_n_d = -trajectory[current_sample][spine]*n2d;
    }
    
    // rear
    i = read_current(rear);
    w = updateEncoder(rear);      
    id = rear_Ks_p*(rear_n_d-n[rear]) + 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;
    if (avg_current[rear] > stall_current){pwm = 0;}
    updateMotor(rear,pwm); 
    
    // front
    i = read_current(front);
    w = updateEncoder(front);      
    id = front_Ks_p*(front_n_d-n[front]) + 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;
    if (avg_current[front] > stall_current){pwm = 0;}
    updateMotor(front,pwm); 
    
    // spine
    i = read_current(spine);
    w = updateEncoder(spine);
    id = spine_Ks_p*(spine_n_d-n[spine]) + 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;
    if (avg_current[spine] > stall_current){pwm = 0;}
    updateMotor(spine,pwm); 

    //timeout for motor safety
    if( avg_current[rear] > time_out_current){ time_out_count[rear]++;}
    if( avg_current[front] > time_out_current){ time_out_count[front]++;}
    if( avg_current[spine] > time_out_current){ time_out_count[spine]++;}
    if( time_out_count[rear]>time_out_steps){ rear_led=1;shutdown();}
    if( time_out_count[front]>time_out_steps){ front_led=1;shutdown();}
    if( time_out_count[spine]>time_out_steps){ spine_led=1;shutdown();}



    //step to next control point
    if (getting_up){
        if(current_sample == standing_time){ getting_up = 0; current_sample = 0;} //we're up
        else{  current_sample++;} //still getting up
    }
    else if(getting_down){
        if(current_sample == sitting_time){ //we're down
            shutdown();
        }
        else{current_sample++;}    //still getting down
    }
    else if (current_sample == n_samples-1){ //normal operation
        if (current_loop == n_loops-1){ getting_down = 1; current_sample=0;} //ready to sit
        else{ //end of loop, ready for next
            current_sample = 0;
            current_loop++;
        }
    }
    else{ current_sample++;} //middle of running loop
} 

void save() {
    pc.printf("%i %i %i %i %f %f %f %i\n", t.read_ms(), n[rear], n[front], n[spine], avg_current[0], avg_current[1], avg_current[2], current_sample);
    pc.printf("%i",rear_encoder.getPulses());
}

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(~kill_test) {
        //DEBUG
        save();
        //pc.printf("%i %f %i %f %i %i\n", t.read_ms(), pwm, n, w, id, i);
    }
}