William Langford / bounding

bounding robot

Dependencies:   QEI mbed

main.cpp@0:fc382eeb78ad, 2013-11-23 (annotated)

Committer:
langfordw
Date:
Sat Nov 23 18:33:47 2013 +0000
Revision:
0:fc382eeb78ad
initial commit;

Who changed what in which revision?

UserRevisionLine numberNew contents of line
langfordw 0:fc382eeb78ad 1 #include "mbed.h"
langfordw 0:fc382eeb78ad 2 #include "QEI.h"
langfordw 0:fc382eeb78ad 3
langfordw 0:fc382eeb78ad 4 #define CONTROL_PERIOD 0.002 // 500Hz ***
langfordw 0:fc382eeb78ad 5 #define SAVE_PERIOD 0.005 // 200HZ
langfordw 0:fc382eeb78ad 6
langfordw 0:fc382eeb78ad 7 Ticker tick;
langfordw 0:fc382eeb78ad 8 Ticker tock;
langfordw 0:fc382eeb78ad 9 Timer t;
langfordw 0:fc382eeb78ad 10
langfordw 0:fc382eeb78ad 11 Serial pc(USBTX, USBRX); // tx, rx
langfordw 0:fc382eeb78ad 12 LocalFileSystem local("data"); // Create the local filesystem under the name "local"
langfordw 0:fc382eeb78ad 13
langfordw 0:fc382eeb78ad 14 // Declare Three Encoders
langfordw 0:fc382eeb78ad 15 QEI rear_encoder(p22, p23, NC, 1200, QEI::X4_ENCODING); // rear leg
langfordw 0:fc382eeb78ad 16 QEI front_encoder(p5, p6, NC, 1200, QEI::X4_ENCODING); // front leg
langfordw 0:fc382eeb78ad 17 QEI spine_encoder(p9, p10, NC, 1200, QEI::X4_ENCODING); // spine
langfordw 0:fc382eeb78ad 18
langfordw 0:fc382eeb78ad 19 // Specify pinout
langfordw 0:fc382eeb78ad 20 DigitalOut rear_motorA(p15);
langfordw 0:fc382eeb78ad 21 DigitalOut rear_motorB(p16);
langfordw 0:fc382eeb78ad 22 PwmOut rear_motorPWM(p24);
langfordw 0:fc382eeb78ad 23 AnalogIn rear_cs(p20);
langfordw 0:fc382eeb78ad 24
langfordw 0:fc382eeb78ad 25 DigitalOut front_motorA(p7);
langfordw 0:fc382eeb78ad 26 DigitalOut front_motorB(p8);
langfordw 0:fc382eeb78ad 27 PwmOut front_motorPWM(p25);
langfordw 0:fc382eeb78ad 28 AnalogIn front_cs(p19);
langfordw 0:fc382eeb78ad 29
langfordw 0:fc382eeb78ad 30 DigitalOut spine_motorA(p11);
langfordw 0:fc382eeb78ad 31 DigitalOut spine_motorB(p12);
langfordw 0:fc382eeb78ad 32 PwmOut spine_motorPWM(p26);
langfordw 0:fc382eeb78ad 33 AnalogIn spine_cs(p18);
langfordw 0:fc382eeb78ad 34
langfordw 0:fc382eeb78ad 35 //number domains for abstraction
langfordw 0:fc382eeb78ad 36 const int rear = 1;
langfordw 0:fc382eeb78ad 37 const int front = 2;
langfordw 0:fc382eeb78ad 38 const int spine = 3;
langfordw 0:fc382eeb78ad 39
langfordw 0:fc382eeb78ad 40 // Sensing Variables
langfordw 0:fc382eeb78ad 41 volatile int i = 0;
langfordw 0:fc382eeb78ad 42 volatile float w = 0;
langfordw 0:fc382eeb78ad 43 volatile int id = 4000;
langfordw 0:fc382eeb78ad 44 volatile int sign = 0;
langfordw 0:fc382eeb78ad 45
langfordw 0:fc382eeb78ad 46 volatile int rear_n = 0;
langfordw 0:fc382eeb78ad 47 volatile int rear_last_n = 0;
langfordw 0:fc382eeb78ad 48 //volatile int rear_i = 0;
langfordw 0:fc382eeb78ad 49 //volatile float rear_w = 0;
langfordw 0:fc382eeb78ad 50 volatile int front_n = 0;
langfordw 0:fc382eeb78ad 51 volatile int front_last_n = 0;
langfordw 0:fc382eeb78ad 52 //volatile int front_i = 0;
langfordw 0:fc382eeb78ad 53 //volatile float front_w = 0;
langfordw 0:fc382eeb78ad 54 volatile int spine_n = 0;
langfordw 0:fc382eeb78ad 55 volatile int spine_last_n = 0;
langfordw 0:fc382eeb78ad 56 //volatile int spine_i = 0;
langfordw 0:fc382eeb78ad 57 //volatile float spine_w = 0;
langfordw 0:fc382eeb78ad 58
langfordw 0:fc382eeb78ad 59 // Output Variables
langfordw 0:fc382eeb78ad 60 volatile float pwm = 0;
langfordw 0:fc382eeb78ad 61 //volatile float rear_pwm = 0;
langfordw 0:fc382eeb78ad 62 //volatile float front_pwm = 0;
langfordw 0:fc382eeb78ad 63
langfordw 0:fc382eeb78ad 64 // Control Constants
langfordw 0:fc382eeb78ad 65 const float R = 2.3/1000.0; // [kohm]
langfordw 0:fc382eeb78ad 66 const float Kv = 0.1682;
langfordw 0:fc382eeb78ad 67 const int Vs = 18; // [V]
langfordw 0:fc382eeb78ad 68 const float n2d = 3.3333;
langfordw 0:fc382eeb78ad 69
langfordw 0:fc382eeb78ad 70 // Control Parameters
langfordw 0:fc382eeb78ad 71 //volatile int rear_id = 4000; // [mA]
langfordw 0:fc382eeb78ad 72 //volatile int front_id = 4000;
langfordw 0:fc382eeb78ad 73 //volatile int spine_id = 4000;
langfordw 0:fc382eeb78ad 74 float rear_Kp = 0.001;
langfordw 0:fc382eeb78ad 75 float rear_Ks_p = 250.0;
langfordw 0:fc382eeb78ad 76 float rear_Ks_d = 25.0;
langfordw 0:fc382eeb78ad 77
langfordw 0:fc382eeb78ad 78 float front_Kp = 0.001;
langfordw 0:fc382eeb78ad 79 float front_Ks_p = 250.0;
langfordw 0:fc382eeb78ad 80 float front_Ks_d = 25.0;
langfordw 0:fc382eeb78ad 81
langfordw 0:fc382eeb78ad 82 float spine_Kp = 0.001;
langfordw 0:fc382eeb78ad 83 float spine_Ks_p = 200.0;
langfordw 0:fc382eeb78ad 84 float spine_Ks_d = 20.0;
langfordw 0:fc382eeb78ad 85
langfordw 0:fc382eeb78ad 86 float rear_n_d = 0.0*n2d;
langfordw 0:fc382eeb78ad 87 float front_n_d = 0.0*n2d;
langfordw 0:fc382eeb78ad 88 float spine_n_d = 0.0*n2d;
langfordw 0:fc382eeb78ad 89 float rear_w_d = 0;
langfordw 0:fc382eeb78ad 90 float front_w_d = 0;
langfordw 0:fc382eeb78ad 91 float spine_w_d = 0;
langfordw 0:fc382eeb78ad 92
langfordw 0:fc382eeb78ad 93 FILE *fp = fopen("/data/out.txt", "w"); // Open "out.txt" on the local file system for writing
langfordw 0:fc382eeb78ad 94
langfordw 0:fc382eeb78ad 95 int read_current(int which_domain) {
langfordw 0:fc382eeb78ad 96 int current = 0;
langfordw 0:fc382eeb78ad 97 if (which_domain == 1) { // rear
langfordw 0:fc382eeb78ad 98 current = rear_cs.read()*23570;
langfordw 0:fc382eeb78ad 99 } else if (which_domain == 2) { // front
langfordw 0:fc382eeb78ad 100 current = front_cs.read()*23570;
langfordw 0:fc382eeb78ad 101 } else if (which_domain == 3){ // spine
langfordw 0:fc382eeb78ad 102 current = spine_cs.read()*23570;
langfordw 0:fc382eeb78ad 103 }
langfordw 0:fc382eeb78ad 104 return current; //mA
langfordw 0:fc382eeb78ad 105 }
langfordw 0:fc382eeb78ad 106
langfordw 0:fc382eeb78ad 107 void updateMotor(int which_motor, float power) {
langfordw 0:fc382eeb78ad 108 int dir = 0;
langfordw 0:fc382eeb78ad 109
langfordw 0:fc382eeb78ad 110 if (power < 0) {
langfordw 0:fc382eeb78ad 111 power = -power;
langfordw 0:fc382eeb78ad 112 dir = 0;
langfordw 0:fc382eeb78ad 113 } else {
langfordw 0:fc382eeb78ad 114 dir = 1;
langfordw 0:fc382eeb78ad 115 }
langfordw 0:fc382eeb78ad 116 if (power > 1) { power = 1; }
langfordw 0:fc382eeb78ad 117 if (power < 0) { power = 0; }
langfordw 0:fc382eeb78ad 118
langfordw 0:fc382eeb78ad 119 if (which_motor == 1) { // rear
langfordw 0:fc382eeb78ad 120 if (dir == 1) {
langfordw 0:fc382eeb78ad 121 rear_motorA = 0;
langfordw 0:fc382eeb78ad 122 rear_motorB = 1;
langfordw 0:fc382eeb78ad 123 } else {
langfordw 0:fc382eeb78ad 124 rear_motorA = 1;
langfordw 0:fc382eeb78ad 125 rear_motorB = 0;
langfordw 0:fc382eeb78ad 126 }
langfordw 0:fc382eeb78ad 127 rear_motorPWM.write(power);
langfordw 0:fc382eeb78ad 128 } else if (which_motor == 2) { // front
langfordw 0:fc382eeb78ad 129 if (dir == 1) {
langfordw 0:fc382eeb78ad 130 front_motorA = 0;
langfordw 0:fc382eeb78ad 131 front_motorB = 1;
langfordw 0:fc382eeb78ad 132 } else {
langfordw 0:fc382eeb78ad 133 front_motorA = 1;
langfordw 0:fc382eeb78ad 134 front_motorB = 0;
langfordw 0:fc382eeb78ad 135 }
langfordw 0:fc382eeb78ad 136 front_motorPWM.write(power);
langfordw 0:fc382eeb78ad 137 } else if (which_motor == 3) { // spine
langfordw 0:fc382eeb78ad 138 if (dir == 1) {
langfordw 0:fc382eeb78ad 139 spine_motorA = 0;
langfordw 0:fc382eeb78ad 140 spine_motorB = 1;
langfordw 0:fc382eeb78ad 141 } else {
langfordw 0:fc382eeb78ad 142 spine_motorA = 1;
langfordw 0:fc382eeb78ad 143 spine_motorB = 0;
langfordw 0:fc382eeb78ad 144 }
langfordw 0:fc382eeb78ad 145 spine_motorPWM.write(power);
langfordw 0:fc382eeb78ad 146 }
langfordw 0:fc382eeb78ad 147 }
langfordw 0:fc382eeb78ad 148
langfordw 0:fc382eeb78ad 149 //void updateEncoder(int which_encoder) {
langfordw 0:fc382eeb78ad 150 // last_n = n;
langfordw 0:fc382eeb78ad 151 // n = encoder.getPulses();
langfordw 0:fc382eeb78ad 152 // w = (n-last_n)*1.047; //steps/s --> rad/s
langfordw 0:fc382eeb78ad 153
langfordw 0:fc382eeb78ad 154
langfordw 0:fc382eeb78ad 155 void control() {
langfordw 0:fc382eeb78ad 156 // rear
langfordw 0:fc382eeb78ad 157 i = read_current(rear);
langfordw 0:fc382eeb78ad 158 rear_last_n = rear_n;
langfordw 0:fc382eeb78ad 159 rear_n = rear_encoder.getPulses();
langfordw 0:fc382eeb78ad 160 w = (rear_n-rear_last_n)*1.047; //steps/s --> rad/s
langfordw 0:fc382eeb78ad 161 id = rear_Ks_p*(rear_n_d-rear_n) + rear_Ks_d*(rear_w_d-w);
langfordw 0:fc382eeb78ad 162 sign = abs(id)/id;
langfordw 0:fc382eeb78ad 163 id = abs(id);
langfordw 0:fc382eeb78ad 164 pwm = sign*(id*R-sign*Kv*w+rear_Kp*(id-i))/Vs;
langfordw 0:fc382eeb78ad 165 updateMotor(rear,pwm);
langfordw 0:fc382eeb78ad 166
langfordw 0:fc382eeb78ad 167 // front
langfordw 0:fc382eeb78ad 168 i = read_current(front);
langfordw 0:fc382eeb78ad 169 front_last_n = front_n;
langfordw 0:fc382eeb78ad 170 front_n = front_encoder.getPulses();
langfordw 0:fc382eeb78ad 171 w = (front_n-front_last_n)*1.047; //steps/s --> rad/s
langfordw 0:fc382eeb78ad 172 id = front_Ks_p*(front_n_d-front_n) + front_Ks_d*(front_w_d-w);
langfordw 0:fc382eeb78ad 173 sign = abs(id)/id;
langfordw 0:fc382eeb78ad 174 id = abs(id);
langfordw 0:fc382eeb78ad 175 pwm = sign*(id*R-sign*Kv*w+front_Kp*(id-i))/Vs;
langfordw 0:fc382eeb78ad 176 updateMotor(front,pwm);
langfordw 0:fc382eeb78ad 177
langfordw 0:fc382eeb78ad 178 // spine
langfordw 0:fc382eeb78ad 179 i = read_current(spine);
langfordw 0:fc382eeb78ad 180 spine_last_n = spine_n;
langfordw 0:fc382eeb78ad 181 spine_n = spine_encoder.getPulses();
langfordw 0:fc382eeb78ad 182 w = (spine_n-spine_last_n)*1.047; //steps/s --> rad/s
langfordw 0:fc382eeb78ad 183 id = spine_Ks_p*(spine_n_d-spine_n) + spine_Ks_d*(spine_w_d-w);
langfordw 0:fc382eeb78ad 184 sign = abs(id)/id;
langfordw 0:fc382eeb78ad 185 id = abs(id);
langfordw 0:fc382eeb78ad 186 pwm = sign*(id*R-sign*Kv*w+spine_Kp*(id-i))/Vs;
langfordw 0:fc382eeb78ad 187 updateMotor(spine,pwm);
langfordw 0:fc382eeb78ad 188
langfordw 0:fc382eeb78ad 189 // Position CURRENT CONTROL
langfordw 0:fc382eeb78ad 190 // if (t.read() < (0.2+0.25*(num_jumps+1))) {
langfordw 0:fc382eeb78ad 191 // n_d = -60.0/360.0*1200.0;
langfordw 0:fc382eeb78ad 192 // id = Ks_p*(n_d-n) + Ks_d*(w_d-w);
langfordw 0:fc382eeb78ad 193 // int sign = abs(id)/id;
langfordw 0:fc382eeb78ad 194 // id = abs(id);
langfordw 0:fc382eeb78ad 195 // pwm = sign*(id*R-sign*Kv*w+Kp*(id-i))/Vs;
langfordw 0:fc382eeb78ad 196 // } else if (t.read() < 0.25*(num_jumps+1)) {
langfordw 0:fc382eeb78ad 197 // n_d = -20.0/360.0*1200.0;
langfordw 0:fc382eeb78ad 198 // id = Ks_p*(n_d-n) + Ks_d*(w_d-w);
langfordw 0:fc382eeb78ad 199 // int sign = abs(id)/id;
langfordw 0:fc382eeb78ad 200 // id = abs(id);
langfordw 0:fc382eeb78ad 201 // pwm = sign*(id*R-sign*Kv*w+Kp*(id-i))/Vs;
langfordw 0:fc382eeb78ad 202 // num_jumps++;
langfordw 0:fc382eeb78ad 203 // }
langfordw 0:fc382eeb78ad 204
langfordw 0:fc382eeb78ad 205 // IMPULSE RESPONSE CODE
langfordw 0:fc382eeb78ad 206 // if (t.read() < 0.2) {
langfordw 0:fc382eeb78ad 207 // id = -10000;
langfordw 0:fc382eeb78ad 208 // int sign = abs(id)/id;
langfordw 0:fc382eeb78ad 209 // id = abs(id);
langfordw 0:fc382eeb78ad 210 // pwm = sign*(id*R-sign*Kv*w+Kp*(id-i))/Vs;
langfordw 0:fc382eeb78ad 211 // } else {
langfordw 0:fc382eeb78ad 212 // pwm = 0;
langfordw 0:fc382eeb78ad 213 // }
langfordw 0:fc382eeb78ad 214
langfordw 0:fc382eeb78ad 215 // PD CURRENT CONTROL
langfordw 0:fc382eeb78ad 216 // id = Ks_p*(n_d-n) + Ks_d*(w_d-w);
langfordw 0:fc382eeb78ad 217 // int sign = abs(id)/id;
langfordw 0:fc382eeb78ad 218 // id = abs(id);
langfordw 0:fc382eeb78ad 219 // pwm = sign*(id*R-sign*Kv*w+Kp*(id-i))/Vs;
langfordw 0:fc382eeb78ad 220
langfordw 0:fc382eeb78ad 221 // CURRENT CONTROL
langfordw 0:fc382eeb78ad 222 // pwm = (id*R+Kv*w+Kp*(id-i))/Vs;
langfordw 0:fc382eeb78ad 223 }
langfordw 0:fc382eeb78ad 224
langfordw 0:fc382eeb78ad 225 void save() {
langfordw 0:fc382eeb78ad 226 fprintf(fp, "%i %i %i %i\n", t.read_ms(), rear_n, front_n, spine_n);
langfordw 0:fc382eeb78ad 227 }
langfordw 0:fc382eeb78ad 228
langfordw 0:fc382eeb78ad 229 int main() {
langfordw 0:fc382eeb78ad 230 rear_motorPWM.period(0.00005); //20kHz
langfordw 0:fc382eeb78ad 231 front_motorPWM.period(0.00005); //20kHz
langfordw 0:fc382eeb78ad 232 spine_motorPWM.period(0.00005); //20kHz
langfordw 0:fc382eeb78ad 233
langfordw 0:fc382eeb78ad 234 tick.attach(&control,CONTROL_PERIOD);
langfordw 0:fc382eeb78ad 235 tock.attach(&save,SAVE_PERIOD);
langfordw 0:fc382eeb78ad 236
langfordw 0:fc382eeb78ad 237 t.start();
langfordw 0:fc382eeb78ad 238 while(1) {
langfordw 0:fc382eeb78ad 239 //spine_n_d = 0.0;
langfordw 0:fc382eeb78ad 240 // rear_n_d = -15.0*n2d*(t.read_ms()/1000.0);
langfordw 0:fc382eeb78ad 241 // front_n_d = -15.0*n2d*(t.read_ms()/1000.0);
langfordw 0:fc382eeb78ad 242 if (t.read() < 5) {
langfordw 0:fc382eeb78ad 243 //stand up
langfordw 0:fc382eeb78ad 244 spine_n_d = 0.0;
langfordw 0:fc382eeb78ad 245 rear_n_d = -25.0*n2d*(t.read_ms()/5000.0);
langfordw 0:fc382eeb78ad 246 front_n_d = -40.0*n2d*(t.read_ms()/5000.0);
langfordw 0:fc382eeb78ad 247 } else if (t.read() < 10) {
langfordw 0:fc382eeb78ad 248 spine_n_d = 15.0*sin((t.read_ms()-10000)*12.57/1000.0)*n2d;
langfordw 0:fc382eeb78ad 249 rear_n_d = (-40.0+15.0*cos((t.read_ms()-10000)*12.57/1000.0))*n2d;
langfordw 0:fc382eeb78ad 250 front_n_d = (-40.0+15.0*sin((t.read_ms()-10000)*12.57/1000.0))*n2d;
langfordw 0:fc382eeb78ad 251 // spine_n_d = 15.0*sin((t.read_ms()-10000)*12.57/1000.0)*n2d;
langfordw 0:fc382eeb78ad 252 // //jump
langfordw 0:fc382eeb78ad 253 // front_Ks_p = 100.0;
langfordw 0:fc382eeb78ad 254 // rear_Ks_p = 100.0;
langfordw 0:fc382eeb78ad 255 // spine_n_d = 0.0;
langfordw 0:fc382eeb78ad 256 // rear_n_d = -15.0*n2d;
langfordw 0:fc382eeb78ad 257 // front_n_d = -15.0*n2d;
langfordw 0:fc382eeb78ad 258 // wait(1.0);
langfordw 0:fc382eeb78ad 259 // front_Ks_p = 250.0;
langfordw 0:fc382eeb78ad 260 // rear_Ks_p = 250.0;
langfordw 0:fc382eeb78ad 261 // rear_n_d = -30.0*n2d;
langfordw 0:fc382eeb78ad 262 // front_n_d = -30.0*n2d;
langfordw 0:fc382eeb78ad 263 // wait(0.2);
langfordw 0:fc382eeb78ad 264 // arch spine back and forth
langfordw 0:fc382eeb78ad 265 // spine_n_d = -20.0*n2d*((t.read_ms()-5000)/5000.0);
langfordw 0:fc382eeb78ad 266 // rear_n_d = -40.0*n2d;
langfordw 0:fc382eeb78ad 267 // front_n_d = -40.0*n2d;
langfordw 0:fc382eeb78ad 268 // } else if (t.read() < 15) {
langfordw 0:fc382eeb78ad 269 // spine_n_d = -15.0*n2d*((15000-t.read_ms())/5000.0);
langfordw 0:fc382eeb78ad 270 // rear_n_d = -40.0*n2d;
langfordw 0:fc382eeb78ad 271 // front_n_d = -40.0*n2d;
langfordw 0:fc382eeb78ad 272 // } else if (t.read() < 20) {
langfordw 0:fc382eeb78ad 273 // spine_n_d = -15.0*n2d*((20000-t.read_ms())/5000.0);
langfordw 0:fc382eeb78ad 274 // rear_n_d = -40.0*n2d;
langfordw 0:fc382eeb78ad 275 // front_n_d = -40.0*n2d;
langfordw 0:fc382eeb78ad 276 } else if (t.read() < 15) {
langfordw 0:fc382eeb78ad 277 spine_n_d = 0.0;//20.0*n2d*((25000-t.read_ms())/5000.0);
langfordw 0:fc382eeb78ad 278 rear_n_d = -25.0*n2d*((15000-t.read_ms())/5000.0);
langfordw 0:fc382eeb78ad 279 front_n_d = -40.0*n2d*((15000-t.read_ms())/5000.0);
langfordw 0:fc382eeb78ad 280 } else {
langfordw 0:fc382eeb78ad 281 fclose(fp);
langfordw 0:fc382eeb78ad 282 pwm = 0;
langfordw 0:fc382eeb78ad 283 updateMotor(rear,pwm);
langfordw 0:fc382eeb78ad 284 updateMotor(front,pwm);
langfordw 0:fc382eeb78ad 285 }
langfordw 0:fc382eeb78ad 286
langfordw 0:fc382eeb78ad 287 //DEBUG
langfordw 0:fc382eeb78ad 288 // pc.printf("%i %f %i %f %i %i\n", t.read_ms(), pwm, n, w, id, i);
langfordw 0:fc382eeb78ad 289 }
langfordw 0:fc382eeb78ad 290 }