embedded code for bounding robot
Fork of bounding by
main.cpp
- Committer:
- calisch
- Date:
- 2014-04-20
- Revision:
- 4:7f9c9bd9da26
- Parent:
- 3:f68eaa68f4ec
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); } }