embedded code for bounding robot
Fork of bounding by
Diff: main.cpp
- Revision:
- 0:fc382eeb78ad
- Child:
- 1:e549754ca234
--- /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