Sam Calisch / bounding

embedded code for bounding robot

Dependencies:   QEI mbed

Fork of bounding by Sam Calisch

Files at this revision

API Documentation at this revision

Revision 1:e549754ca234, committed 2013-11-23

Comitter:
calisch
Date:
Sat Nov 23 21:16:12 2013 +0000
Parent:
0:fc382eeb78ad
Child:
2:17379e2a6f7d
Commit message:
finished trajectory from array, current integral limiting

Changed in this revision

main.cpp Show annotated file Show diff for this revision Revisions of this file 
--- 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);
     }