embedded code for bounding robot
Fork of bounding by
Diff: main.cpp
- Revision:
- 1:e549754ca234
- Parent:
- 0:fc382eeb78ad
- Child:
- 2:17379e2a6f7d
--- a/main.cpp Sat Nov 23 18:33:47 2013 +0000 +++ b/main.cpp Sat Nov 23 21:16:12 2013 +0000 @@ -1,9 +1,514 @@ #include "mbed.h" #include "QEI.h" - #define CONTROL_PERIOD 0.002 // 500Hz *** #define SAVE_PERIOD 0.005 // 200HZ +//think about start up and shut down sequences + +// 500 x 3 array of degree values +const float trajectory[500][3] = { +1,1,0, +1,1,0, +1,1,0, +1,1,0, +1,1,0, +1,1,0, +2,2,0, +2,2,0, +3,3,0, +3,3,0, +3,3,0, +4,4,0, +4,4,0, +5,5,0, +5,5,0, +6,6,0, +6,6,0, +7,7,0, +7,7,0, +7,7,0, +8,8,0, +8,8,0, +9,9,0, +9,9,0, +10,10,0, +10,10,0, +11,11,0, +11,11,0, +11,11,0, +12,12,0, +12,12,0, +13,13,0, +13,13,0, +14,14,0, +14,14,0, +14,14,0, +15,15,0, +15,15,0, +16,16,0, +16,16,0, +17,17,0, +17,17,0, +18,18,0, +18,18,0, +18,18,0, +19,19,0, +19,19,0, +20,20,0, +20,20,0, +21,21,0, +21,21,0, +22,22,0, +22,22,0, +22,22,0, +23,23,0, +23,23,0, +24,24,0, +24,24,0, +25,25,0, +25,25,0, +25,25,0, +26,26,0, +26,26,0, +27,27,0, +27,27,0, +28,28,0, +28,28,0, +29,29,0, +29,29,0, +29,29,0, +30,30,0, +30,30,0, +31,31,0, +31,31,0, +32,32,0, +32,32,0, +33,33,0, +33,33,0, +33,33,0, +34,34,0, +34,34,0, +35,35,0, +35,35,0, +36,36,0, +36,36,0, +36,36,0, +37,37,0, +37,37,0, +38,38,0, +38,38,0, +39,39,0, +39,39,0, +40,40,0, +40,40,0, +40,40,0, +41,41,0, +41,41,0, +42,42,0, +42,42,0, +43,43,0, +43,43,0, +44,44,0, +44,44,0, +44,44,0, +45,45,0, +45,45,0, +46,46,0, +46,46,0, +47,47,0, +47,47,0, +47,47,0, +48,48,0, +48,48,0, +49,49,0, +49,49,0, +50,50,0, +50,50,0, +51,51,0, +51,51,0, +51,51,0, +52,52,0, +52,52,0, +53,53,0, +53,53,0, +53,53,0, +54,54,0, +54,54,0, +54,54,0, +54,54,0, +54,54,0, +54,54,0, +54,54,0, +54,54,0, +54,54,0, +54,54,0, +55,55,0, +55,55,0, +55,55,0, +55,55,0, +55,55,0, +55,55,0, +55,55,0, +55,55,0, +55,55,0, +55,55,0, +55,55,0, +55,55,0, +55,55,0, +55,55,0, +55,55,0, +55,55,0, +55,55,0, +55,55,0, +55,55,0, +55,55,0, +55,55,0, +55,55,0, +55,55,0, +55,55,0, +55,55,0, +55,55,0, +55,55,0, +55,55,0, +55,55,0, +55,55,0, +55,55,0, +55,55,0, +55,55,0, +55,55,0, +55,55,0, +55,55,0, +55,55,0, +55,55,0, +55,55,0, +55,55,0, +55,55,0, +55,55,0, +55,55,0, +55,55,0, +55,55,0, +55,55,0, +55,55,0, +55,55,0, +55,55,0, +55,55,0, +55,55,0, +55,55,0, +55,55,0, +55,55,0, +55,55,0, +55,55,0, +55,55,0, +55,55,0, +55,55,0, +55,55,0, +55,55,0, +55,55,0, +55,55,0, +55,55,0, +55,55,0, +55,55,0, +55,55,0, +55,55,0, +55,55,0, +55,55,0, +55,55,0, +55,55,0, +55,55,0, +55,55,0, +55,55,0, +55,55,0, +55,55,0, +55,55,0, +55,55,0, +55,55,0, +55,55,0, +55,55,0, +55,55,0, +55,55,0, +55,55,0, +55,55,0, +55,55,0, +55,55,0, +55,55,0, +55,55,0, +55,55,0, +55,55,0, +55,55,0, +55,55,0, +55,55,0, +55,55,0, +55,55,0, +55,55,0, +55,55,0, +55,55,0, +55,55,0, +55,55,0, +55,55,0, +55,55,0, +55,55,0, +55,55,0, +55,55,0, +55,55,0, +55,55,0, +55,55,0, +55,55,0, +55,55,0, +55,55,0, +55,55,0, +55,55,0, +55,55,0, +55,55,0, +55,55,0, +55,55,0, +55,55,0, +55,55,0, +55,55,0, +55,55,0, +55,55,0, +55,55,0, +55,55,0, +55,55,0, +55,55,0, +55,55,0, +55,55,0, +55,55,0, +55,55,0, +55,55,0, +55,55,0, +55,55,0, +55,55,0, +55,55,0, +55,55,0, +55,55,0, +55,55,0, +55,55,0, +55,55,0, +55,55,0, +55,55,0, +55,55,0, +55,55,0, +55,55,0, +55,55,0, +55,55,0, +55,55,0, +55,55,0, +55,55,0, +55,55,0, +55,55,0, +55,55,0, +55,55,0, +55,55,0, +55,55,0, +55,55,0, +55,55,0, +55,55,0, +55,55,0, +55,55,0, +55,55,0, +55,55,0, +55,55,0, +55,55,0, +55,55,0, +55,55,0, +55,55,0, +55,55,0, +55,55,0, +55,55,0, +55,55,0, +55,55,0, +55,55,0, +55,55,0, +55,55,0, +55,55,0, +55,55,0, +55,55,0, +55,55,0, +55,55,0, +55,55,0, +55,55,0, +55,55,0, +55,55,0, +55,55,0, +55,55,0, +55,55,0, +55,55,0, +55,55,0, +55,55,0, +55,55,0, +55,55,0, +55,55,0, +55,55,0, +55,55,0, +55,55,0, +55,55,0, +55,55,0, +55,55,0, +55,55,0, +55,55,0, +55,55,0, +55,55,0, +55,55,0, +55,55,0, +55,55,0, +55,55,0, +55,55,0, +55,55,0, +55,55,0, +55,55,0, +55,55,0, +55,55,0, +55,55,0, +55,55,0, +55,55,0, +55,55,0, +55,55,0, +55,55,0, +55,55,0, +55,55,0, +55,55,0, +55,55,0, +55,55,0, +55,55,0, +55,55,0, +55,55,0, +55,55,0, +55,55,0, +55,55,0, +54,54,0, +54,54,0, +54,54,0, +54,54,0, +54,54,0, +54,54,0, +54,54,0, +54,54,0, +54,54,0, +54,54,0, +53,53,0, +53,53,0, +53,53,0, +52,52,0, +52,52,0, +51,51,0, +51,51,0, +51,51,0, +50,50,0, +50,50,0, +49,49,0, +49,49,0, +48,48,0, +48,48,0, +47,47,0, +47,47,0, +47,47,0, +46,46,0, +46,46,0, +45,45,0, +45,45,0, +44,44,0, +44,44,0, +43,43,0, +43,43,0, +43,43,0, +42,42,0, +42,42,0, +41,41,0, +41,41,0, +40,40,0, +40,40,0, +40,40,0, +39,39,0, +39,39,0, +38,38,0, +38,38,0, +37,37,0, +37,37,0, +36,36,0, +36,36,0, +36,36,0, +35,35,0, +35,35,0, +34,34,0, +34,34,0, +33,33,0, +33,33,0, +33,33,0, +32,32,0, +32,32,0, +31,31,0, +31,31,0, +30,30,0, +30,30,0, +29,29,0, +29,29,0, +29,29,0, +28,28,0, +28,28,0, +27,27,0, +27,27,0, +26,26,0, +26,26,0, +25,25,0, +25,25,0, +25,25,0, +24,24,0, +24,24,0, +23,23,0, +23,23,0, +22,22,0, +22,22,0, +21,21,0, +21,21,0, +21,21,0, +20,20,0, +20,20,0, +19,19,0, +19,19,0, +18,18,0, +18,18,0, +18,18,0, +17,17,0, +17,17,0, +16,16,0, +16,16,0, +15,15,0, +15,15,0, +14,14,0, +14,14,0, +14,14,0, +13,13,0, +13,13,0, +12,12,0, +12,12,0, +11,11,0, +11,11,0, +10,10,0, +10,10,0, +10,10,0, +9,9,0, +9,9,0, +8,8,0, +8,8,0, +7,7,0, +7,7,0, +7,7,0, +6,6,0, +6,6,0, +5,5,0, +5,5,0, +4,4,0, +4,4,0, +3,3,0, +3,3,0, +3,3,0, +2,2,0, +2,2,0, +1,1,0, +1,1,0, +1,1,0 +}; + Ticker tick; Ticker tock; Timer t; @@ -33,9 +538,9 @@ AnalogIn spine_cs(p18); //number domains for abstraction -const int rear = 1; -const int front = 2; -const int spine = 3; +const int rear = 0; +const int front = 1; +const int spine = 2; // Sensing Variables volatile int i = 0; @@ -43,23 +548,13 @@ 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; +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}; //integration of current in time // 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] @@ -67,10 +562,10 @@ const int Vs = 18; // [V] const float n2d = 3.3333; +const float integ_alpha = .5; //peristence of current integration. 0->all past, 1->all present +const float stall_current = 5; //Amps + // 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; @@ -90,17 +585,24 @@ float front_w_d = 0; float spine_w_d = 0; +//control flow +volatile int current_sample = 0; +volatile int current_loop = 0; +const int n_samples = 500; +const int n_loops = 1; + 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 + if (which_domain == rear) { // rear current = rear_cs.read()*23570; - } else if (which_domain == 2) { // front + } else if (which_domain == front) { // front current = front_cs.read()*23570; - } else if (which_domain == 3){ // spine + } 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 } @@ -146,144 +648,88 @@ } } -//void updateEncoder(int which_encoder) { -// last_n = n; -// n = encoder.getPulses(); -// w = (n-last_n)*1.047; //steps/s --> rad/s - +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 control() { + 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); - 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); + 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); - 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); + 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[rear] > stall_current){pwm = 0;} 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); + 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[rear] > stall_current){pwm = 0;} 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; -} + + //step to next control point + if (current_sample == n_samples){ + if (current_loop == n_loops){ //done + fclose(fp); + pwm = 0; + updateMotor(rear,pwm); + updateMotor(front,pwm); + } + else{ //end of loop, ready for next + current_sample = 0; + current_loop++; + } + } + else{ //middle of loop + current_sample++; + } + +} void save() { - fprintf(fp, "%i %i %i %i\n", t.read_ms(), rear_n, front_n, spine_n); + fprintf(fp, "%i %i %i %i\n", t.read_ms(), n[rear], n[front], n[spine]); } 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(); - 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); }