William Langford
bounding robot
Diff: main.cpp
- Revision:
- 0:fc382eeb78ad
--- /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