embedded code for bounding robot
Fork of bounding by
main.cpp
- Committer:
- langfordw
- Date:
- 2013-11-23
- Revision:
- 0:fc382eeb78ad
- Child:
- 1:e549754ca234
File content as of revision 0:fc382eeb78ad:
#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); } }