embedded code for bounding robot
Fork of bounding by
main.cpp@0:fc382eeb78ad, 2013-11-23 (annotated)
- Committer:
- langfordw
- Date:
- Sat Nov 23 18:33:47 2013 +0000
- Revision:
- 0:fc382eeb78ad
- Child:
- 1:e549754ca234
initial commit;
Who changed what in which revision?
User | Revision | Line number | New 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 | } |