Diff: main.cpp

Revision:

0:fc382eeb78ad

diff -r 000000000000 -r fc382eeb78ad main.cpp
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp	Sat Nov 23 18:33:47 2013 +0000
@@ -0,0 +1,290 @@
+#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);
+    }
+}
\ No newline at end of file