Sam Calisch / bounding

embedded code for bounding robot

Dependencies:   QEI mbed

Fork of bounding by Sam Calisch

main.cpp@4:7f9c9bd9da26, 2014-04-20 (annotated)

Committer:
calisch
Date:
Sun Apr 20 18:01:02 2014 +0000
Revision:
4:7f9c9bd9da26
Parent:
3:f68eaa68f4ec 
hi there;

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 #define CONTROL_PERIOD 0.002 // 500Hz ***
langfordw 0:fc382eeb78ad 4 #define SAVE_PERIOD 0.005 // 200HZ
langfordw 0:fc382eeb78ad 5
calisch 4:7f9c9bd9da26 6 const int n_samples = 165;
calisch 4:7f9c9bd9da26 7
calisch 4:7f9c9bd9da26 8 // 1000 x 3 array of degree values
calisch 4:7f9c9bd9da26 9 const float trajectory[n_samples][3] = {
calisch 4:7f9c9bd9da26 10 34,10,0,
calisch 4:7f9c9bd9da26 11 34,10,0,
calisch 4:7f9c9bd9da26 12 34,10,0,
calisch 4:7f9c9bd9da26 13 34,10,0,
calisch 4:7f9c9bd9da26 14 34,10,0,
calisch 4:7f9c9bd9da26 15 34,10,-1,
calisch 4:7f9c9bd9da26 16 34,10,-1,
calisch 4:7f9c9bd9da26 17 34,10,-1,
calisch 4:7f9c9bd9da26 18 34,10,-2,
calisch 4:7f9c9bd9da26 19 34,10,-2,
calisch 4:7f9c9bd9da26 20 34,10,-2,
calisch 4:7f9c9bd9da26 21 34,10,-3,
calisch 4:7f9c9bd9da26 22 34,10,-3,
calisch 4:7f9c9bd9da26 23 34,10,-3,
calisch 4:7f9c9bd9da26 24 33,11,-3,
calisch 4:7f9c9bd9da26 25 33,11,-4,
calisch 4:7f9c9bd9da26 26 33,11,-4,
calisch 4:7f9c9bd9da26 27 33,11,-4,
calisch 4:7f9c9bd9da26 28 33,11,-4,
calisch 4:7f9c9bd9da26 29 32,12,-4,
calisch 4:7f9c9bd9da26 30 32,12,-5,
calisch 4:7f9c9bd9da26 31 32,12,-5,
calisch 4:7f9c9bd9da26 32 31,13,-5,
calisch 4:7f9c9bd9da26 33 31,13,-5,
calisch 4:7f9c9bd9da26 34 31,13,-5,
calisch 4:7f9c9bd9da26 35 30,14,-5,
calisch 4:7f9c9bd9da26 36 30,14,-6,
calisch 4:7f9c9bd9da26 37 30,14,-6,
calisch 4:7f9c9bd9da26 38 29,15,-6,
calisch 4:7f9c9bd9da26 39 29,15,-6,
calisch 4:7f9c9bd9da26 40 28,16,-6,
calisch 4:7f9c9bd9da26 41 28,16,-6,
calisch 4:7f9c9bd9da26 42 27,17,-6,
calisch 4:7f9c9bd9da26 43 27,17,-6,
calisch 4:7f9c9bd9da26 44 26,18,-6,
calisch 4:7f9c9bd9da26 45 26,18,-6,
calisch 4:7f9c9bd9da26 46 25,19,-6,
calisch 4:7f9c9bd9da26 47 25,19,-6,
calisch 4:7f9c9bd9da26 48 24,20,-6,
calisch 4:7f9c9bd9da26 49 24,20,-6,
calisch 4:7f9c9bd9da26 50 23,21,-6,
calisch 4:7f9c9bd9da26 51 23,21,-6,
calisch 4:7f9c9bd9da26 52 22,22,-6,
calisch 4:7f9c9bd9da26 53 22,22,-6,
calisch 4:7f9c9bd9da26 54 21,23,-6,
calisch 4:7f9c9bd9da26 55 20,24,-6,
calisch 4:7f9c9bd9da26 56 20,24,-6,
calisch 4:7f9c9bd9da26 57 19,25,-6,
calisch 4:7f9c9bd9da26 58 19,25,-6,
calisch 4:7f9c9bd9da26 59 18,26,-6,
calisch 4:7f9c9bd9da26 60 18,26,-6,
calisch 4:7f9c9bd9da26 61 17,27,-6,
calisch 4:7f9c9bd9da26 62 17,27,-6,
calisch 4:7f9c9bd9da26 63 16,28,-6,
calisch 4:7f9c9bd9da26 64 16,28,-6,
calisch 4:7f9c9bd9da26 65 15,29,-6,
calisch 4:7f9c9bd9da26 66 15,29,-6,
calisch 4:7f9c9bd9da26 67 15,29,-6,
calisch 4:7f9c9bd9da26 68 14,30,-6,
calisch 4:7f9c9bd9da26 69 14,30,-5,
calisch 4:7f9c9bd9da26 70 13,31,-5,
calisch 4:7f9c9bd9da26 71 13,31,-5,
calisch 4:7f9c9bd9da26 72 13,31,-5,
calisch 4:7f9c9bd9da26 73 12,32,-5,
calisch 4:7f9c9bd9da26 74 12,32,-5,
calisch 4:7f9c9bd9da26 75 12,32,-4,
calisch 4:7f9c9bd9da26 76 12,32,-4,
calisch 4:7f9c9bd9da26 77 11,33,-4,
calisch 4:7f9c9bd9da26 78 11,33,-4,
calisch 4:7f9c9bd9da26 79 11,33,-4,
calisch 4:7f9c9bd9da26 80 11,33,-3,
calisch 4:7f9c9bd9da26 81 10,34,-3,
calisch 4:7f9c9bd9da26 82 10,34,-3,
calisch 4:7f9c9bd9da26 83 10,34,-3,
calisch 4:7f9c9bd9da26 84 10,34,-2,
calisch 4:7f9c9bd9da26 85 10,34,-2,
calisch 4:7f9c9bd9da26 86 10,34,-2,
calisch 4:7f9c9bd9da26 87 10,34,-1,
calisch 4:7f9c9bd9da26 88 10,34,-1,
calisch 4:7f9c9bd9da26 89 10,34,-1,
calisch 4:7f9c9bd9da26 90 10,34,0,
calisch 4:7f9c9bd9da26 91 10,34,0,
calisch 4:7f9c9bd9da26 92 10,34,0,
calisch 4:7f9c9bd9da26 93 10,34,0,
calisch 4:7f9c9bd9da26 94 10,34,0,
calisch 4:7f9c9bd9da26 95 10,34,0,
calisch 4:7f9c9bd9da26 96 10,34,0,
calisch 4:7f9c9bd9da26 97 10,34,1,
calisch 4:7f9c9bd9da26 98 10,34,1,
calisch 4:7f9c9bd9da26 99 10,34,1,
calisch 4:7f9c9bd9da26 100 10,34,2,
calisch 4:7f9c9bd9da26 101 10,34,2,
calisch 4:7f9c9bd9da26 102 10,34,2,
calisch 4:7f9c9bd9da26 103 10,34,2,
calisch 4:7f9c9bd9da26 104 10,34,3,
calisch 4:7f9c9bd9da26 105 10,34,3,
calisch 4:7f9c9bd9da26 106 10,34,3,
calisch 4:7f9c9bd9da26 107 11,33,3,
calisch 4:7f9c9bd9da26 108 11,33,4,
calisch 4:7f9c9bd9da26 109 11,33,4,
calisch 4:7f9c9bd9da26 110 11,33,4,
calisch 4:7f9c9bd9da26 111 12,32,4,
calisch 4:7f9c9bd9da26 112 12,32,5,
calisch 4:7f9c9bd9da26 113 12,32,5,
calisch 4:7f9c9bd9da26 114 12,32,5,
calisch 4:7f9c9bd9da26 115 13,31,5,
calisch 4:7f9c9bd9da26 116 13,31,5,
calisch 4:7f9c9bd9da26 117 13,31,5,
calisch 4:7f9c9bd9da26 118 14,30,6,
calisch 4:7f9c9bd9da26 119 14,30,6,
calisch 4:7f9c9bd9da26 120 15,29,6,
calisch 4:7f9c9bd9da26 121 15,29,6,
calisch 4:7f9c9bd9da26 122 15,29,6,
calisch 4:7f9c9bd9da26 123 16,28,6,
calisch 4:7f9c9bd9da26 124 16,28,6,
calisch 4:7f9c9bd9da26 125 17,27,6,
calisch 4:7f9c9bd9da26 126 17,27,6,
calisch 4:7f9c9bd9da26 127 18,26,6,
calisch 4:7f9c9bd9da26 128 18,26,6,
calisch 4:7f9c9bd9da26 129 19,25,6,
calisch 4:7f9c9bd9da26 130 19,25,6,
calisch 4:7f9c9bd9da26 131 20,24,6,
calisch 4:7f9c9bd9da26 132 20,24,6,
calisch 4:7f9c9bd9da26 133 21,23,6,
calisch 4:7f9c9bd9da26 134 22,22,6,
calisch 4:7f9c9bd9da26 135 22,22,6,
calisch 4:7f9c9bd9da26 136 23,21,6,
calisch 4:7f9c9bd9da26 137 23,21,6,
calisch 4:7f9c9bd9da26 138 24,20,6,
calisch 4:7f9c9bd9da26 139 24,20,6,
calisch 4:7f9c9bd9da26 140 25,19,6,
calisch 4:7f9c9bd9da26 141 25,19,6,
calisch 4:7f9c9bd9da26 142 26,18,6,
calisch 4:7f9c9bd9da26 143 26,18,6,
calisch 4:7f9c9bd9da26 144 27,17,6,
calisch 4:7f9c9bd9da26 145 27,17,6,
calisch 4:7f9c9bd9da26 146 28,16,6,
calisch 4:7f9c9bd9da26 147 28,16,6,
calisch 4:7f9c9bd9da26 148 29,15,6,
calisch 4:7f9c9bd9da26 149 29,15,6,
calisch 4:7f9c9bd9da26 150 30,14,6,
calisch 4:7f9c9bd9da26 151 30,14,6,
calisch 4:7f9c9bd9da26 152 30,14,5,
calisch 4:7f9c9bd9da26 153 31,13,5,
calisch 4:7f9c9bd9da26 154 31,13,5,
calisch 4:7f9c9bd9da26 155 31,13,5,
calisch 4:7f9c9bd9da26 156 32,12,5,
calisch 4:7f9c9bd9da26 157 32,12,5,
calisch 4:7f9c9bd9da26 158 32,12,4,
calisch 4:7f9c9bd9da26 159 33,11,4,
calisch 4:7f9c9bd9da26 160 33,11,4,
calisch 4:7f9c9bd9da26 161 33,11,4,
calisch 4:7f9c9bd9da26 162 33,11,3,
calisch 4:7f9c9bd9da26 163 33,11,3,
calisch 4:7f9c9bd9da26 164 34,10,3,
calisch 4:7f9c9bd9da26 165 34,10,3,
calisch 4:7f9c9bd9da26 166 34,10,2,
calisch 4:7f9c9bd9da26 167 34,10,2,
calisch 4:7f9c9bd9da26 168 34,10,2,
calisch 4:7f9c9bd9da26 169 34,10,2,
calisch 4:7f9c9bd9da26 170 34,10,1,
calisch 4:7f9c9bd9da26 171 34,10,1,
calisch 4:7f9c9bd9da26 172 34,10,1,
calisch 4:7f9c9bd9da26 173 34,10,0,
calisch 4:7f9c9bd9da26 174 34,10,0
calisch 3:f68eaa68f4ec 175 };
calisch 1:e549754ca234 176
calisch 2:17379e2a6f7d 177 //control flow
calisch 4:7f9c9bd9da26 178 const int standing_time = 250; //samples at 500Hz, time to stand up
calisch 4:7f9c9bd9da26 179 const float standing_step = 1/(float)standing_time; //recipherical of standing_time
calisch 4:7f9c9bd9da26 180 const int sitting_time = 1000; //samples at 500Hz, time to sit down
calisch 4:7f9c9bd9da26 181 const float sitting_step = 1/(float)sitting_time; //recipherical of sitting time
calisch 3:f68eaa68f4ec 182 volatile int getting_up = 1; //are we currently in the process of standing up
calisch 3:f68eaa68f4ec 183 volatile int getting_down = 0; //are we currently in the process of sitting down
calisch 4:7f9c9bd9da26 184 const float standing_position = 34.0;
calisch 4:7f9c9bd9da26 185 const float bent_position = 10.0;
calisch 4:7f9c9bd9da26 186 const float spine_start = trajectory[0][2];
calisch 3:f68eaa68f4ec 187
calisch 2:17379e2a6f7d 188 volatile int current_sample = 0;
calisch 2:17379e2a6f7d 189 volatile int current_loop = 0;
calisch 4:7f9c9bd9da26 190 const int n_loops = 12;
calisch 2:17379e2a6f7d 191
langfordw 0:fc382eeb78ad 192 Ticker tick;
langfordw 0:fc382eeb78ad 193 Ticker tock;
langfordw 0:fc382eeb78ad 194 Timer t;
langfordw 0:fc382eeb78ad 195
langfordw 0:fc382eeb78ad 196 Serial pc(USBTX, USBRX); // tx, rx
langfordw 0:fc382eeb78ad 197 LocalFileSystem local("data"); // Create the local filesystem under the name "local"
langfordw 0:fc382eeb78ad 198
langfordw 0:fc382eeb78ad 199 // Declare Three Encoders
calisch 4:7f9c9bd9da26 200 QEI rear_encoder(p23, p14, NC, 1200, QEI::X4_ENCODING); // rear leg
calisch 4:7f9c9bd9da26 201 QEI front_encoder(p21, p16, NC, 1200, QEI::X4_ENCODING); // front leg
calisch 4:7f9c9bd9da26 202 QEI spine_encoder(p22, p15, NC, 1200, QEI::X4_ENCODING); // spine
langfordw 0:fc382eeb78ad 203
langfordw 0:fc382eeb78ad 204 // Specify pinout
calisch 4:7f9c9bd9da26 205 DigitalOut rear_motorA(p7);
calisch 4:7f9c9bd9da26 206 DigitalOut rear_motorB(p6);
calisch 4:7f9c9bd9da26 207 PwmOut rear_motorPWM(p26);
calisch 4:7f9c9bd9da26 208 AnalogIn rear_cs(p18);
langfordw 0:fc382eeb78ad 209
calisch 4:7f9c9bd9da26 210 DigitalOut front_motorA(p11);
calisch 4:7f9c9bd9da26 211 DigitalOut front_motorB(p10);
calisch 4:7f9c9bd9da26 212 PwmOut front_motorPWM(p24);
calisch 4:7f9c9bd9da26 213 AnalogIn front_cs(p20);
langfordw 0:fc382eeb78ad 214
calisch 4:7f9c9bd9da26 215 DigitalOut spine_motorA(p9);
calisch 4:7f9c9bd9da26 216 DigitalOut spine_motorB(p8);
calisch 4:7f9c9bd9da26 217 PwmOut spine_motorPWM(p25);
calisch 4:7f9c9bd9da26 218 AnalogIn spine_cs(p19);
langfordw 0:fc382eeb78ad 219
calisch 2:17379e2a6f7d 220 //LEDs for current safety
calisch 2:17379e2a6f7d 221 DigitalOut rear_led(LED1);
calisch 2:17379e2a6f7d 222 DigitalOut front_led(LED2);
calisch 2:17379e2a6f7d 223 DigitalOut spine_led(LED3);
calisch 2:17379e2a6f7d 224
langfordw 0:fc382eeb78ad 225 //number domains for abstraction
calisch 1:e549754ca234 226 const int rear = 0;
calisch 1:e549754ca234 227 const int front = 1;
calisch 1:e549754ca234 228 const int spine = 2;
langfordw 0:fc382eeb78ad 229
langfordw 0:fc382eeb78ad 230 // Sensing Variables
langfordw 0:fc382eeb78ad 231 volatile int i = 0;
langfordw 0:fc382eeb78ad 232 volatile float w = 0;
langfordw 0:fc382eeb78ad 233 volatile int id = 4000;
langfordw 0:fc382eeb78ad 234 volatile int sign = 0;
langfordw 0:fc382eeb78ad 235
calisch 1:e549754ca234 236 volatile int n[3] = {0,0,0}; //encoder values
calisch 1:e549754ca234 237 volatile int last_n[3] = {0,0,0}; //previous encoder values
calisch 1:e549754ca234 238
calisch 4:7f9c9bd9da26 239 volatile float avg_current[3] = {0.0,0.0,0.0}; //integration of current in time
langfordw 0:fc382eeb78ad 240
langfordw 0:fc382eeb78ad 241 // Output Variables
langfordw 0:fc382eeb78ad 242 volatile float pwm = 0;
langfordw 0:fc382eeb78ad 243
langfordw 0:fc382eeb78ad 244 // Control Constants
langfordw 0:fc382eeb78ad 245 const float R = 2.3/1000.0; // [kohm]
langfordw 0:fc382eeb78ad 246 const float Kv = 0.1682;
langfordw 0:fc382eeb78ad 247 const int Vs = 18; // [V]
langfordw 0:fc382eeb78ad 248 const float n2d = 3.3333;
langfordw 0:fc382eeb78ad 249
calisch 2:17379e2a6f7d 250 const float integ_alpha = .05; //peristence of current integration. 0->all past, 1->all present
calisch 4:7f9c9bd9da26 251 const float stall_current = 8000; //mA
calisch 3:f68eaa68f4ec 252
calisch 4:7f9c9bd9da26 253 const float time_out_current = 5000; //mA if avg_current is above, increment the timeout count
calisch 4:7f9c9bd9da26 254 const int time_out_steps = 400; //max steps after which kill the test
calisch 3:f68eaa68f4ec 255 volatile int time_out_count[3] = {0,0,0}; //counter for time out
calisch 3:f68eaa68f4ec 256 int kill_test = 0; //kill switch
calisch 1:e549754ca234 257
langfordw 0:fc382eeb78ad 258 // Control Parameters
langfordw 0:fc382eeb78ad 259 float rear_Kp = 0.001;
calisch 4:7f9c9bd9da26 260 float rear_Ks_p = 220.0;
calisch 4:7f9c9bd9da26 261 float rear_Ks_d = 500.0;
langfordw 0:fc382eeb78ad 262
langfordw 0:fc382eeb78ad 263 float front_Kp = 0.001;
calisch 4:7f9c9bd9da26 264 float front_Ks_p = 220.0;
calisch 4:7f9c9bd9da26 265 float front_Ks_d = 500.0;
langfordw 0:fc382eeb78ad 266
langfordw 0:fc382eeb78ad 267 float spine_Kp = 0.001;
calisch 4:7f9c9bd9da26 268 float spine_Ks_p = 300.0;
calisch 4:7f9c9bd9da26 269 float spine_Ks_d = 400.0;
langfordw 0:fc382eeb78ad 270
langfordw 0:fc382eeb78ad 271 float rear_n_d = 0.0*n2d;
langfordw 0:fc382eeb78ad 272 float front_n_d = 0.0*n2d;
langfordw 0:fc382eeb78ad 273 float spine_n_d = 0.0*n2d;
langfordw 0:fc382eeb78ad 274 float rear_w_d = 0;
langfordw 0:fc382eeb78ad 275 float front_w_d = 0;
langfordw 0:fc382eeb78ad 276 float spine_w_d = 0;
langfordw 0:fc382eeb78ad 277
calisch 1:e549754ca234 278
langfordw 0:fc382eeb78ad 279 FILE *fp = fopen("/data/out.txt", "w"); // Open "out.txt" on the local file system for writing
langfordw 0:fc382eeb78ad 280
langfordw 0:fc382eeb78ad 281 int read_current(int which_domain) {
langfordw 0:fc382eeb78ad 282 int current = 0;
calisch 1:e549754ca234 283 if (which_domain == rear) { // rear
langfordw 0:fc382eeb78ad 284 current = rear_cs.read()*23570;
calisch 1:e549754ca234 285 } else if (which_domain == front) { // front
langfordw 0:fc382eeb78ad 286 current = front_cs.read()*23570;
calisch 1:e549754ca234 287 } else if (which_domain == spine){ // spine
langfordw 0:fc382eeb78ad 288 current = spine_cs.read()*23570;
langfordw 0:fc382eeb78ad 289 }
calisch 1:e549754ca234 290 avg_current[which_domain] = (1-integ_alpha)*avg_current[which_domain] + integ_alpha*current; //integrate
langfordw 0:fc382eeb78ad 291 return current; //mA
langfordw 0:fc382eeb78ad 292 }
langfordw 0:fc382eeb78ad 293
langfordw 0:fc382eeb78ad 294 void updateMotor(int which_motor, float power) {
langfordw 0:fc382eeb78ad 295 int dir = 0;
langfordw 0:fc382eeb78ad 296
langfordw 0:fc382eeb78ad 297 if (power < 0) {
langfordw 0:fc382eeb78ad 298 power = -power;
langfordw 0:fc382eeb78ad 299 dir = 0;
langfordw 0:fc382eeb78ad 300 } else {
langfordw 0:fc382eeb78ad 301 dir = 1;
langfordw 0:fc382eeb78ad 302 }
langfordw 0:fc382eeb78ad 303 if (power > 1) { power = 1; }
langfordw 0:fc382eeb78ad 304 if (power < 0) { power = 0; }
langfordw 0:fc382eeb78ad 305
calisch 2:17379e2a6f7d 306 if (which_motor == rear) { // rear
langfordw 0:fc382eeb78ad 307 if (dir == 1) {
langfordw 0:fc382eeb78ad 308 rear_motorA = 0;
langfordw 0:fc382eeb78ad 309 rear_motorB = 1;
langfordw 0:fc382eeb78ad 310 } else {
langfordw 0:fc382eeb78ad 311 rear_motorA = 1;
langfordw 0:fc382eeb78ad 312 rear_motorB = 0;
langfordw 0:fc382eeb78ad 313 }
langfordw 0:fc382eeb78ad 314 rear_motorPWM.write(power);
calisch 2:17379e2a6f7d 315 } else if (which_motor == front) { // front
langfordw 0:fc382eeb78ad 316 if (dir == 1) {
langfordw 0:fc382eeb78ad 317 front_motorA = 0;
langfordw 0:fc382eeb78ad 318 front_motorB = 1;
langfordw 0:fc382eeb78ad 319 } else {
langfordw 0:fc382eeb78ad 320 front_motorA = 1;
langfordw 0:fc382eeb78ad 321 front_motorB = 0;
langfordw 0:fc382eeb78ad 322 }
langfordw 0:fc382eeb78ad 323 front_motorPWM.write(power);
calisch 2:17379e2a6f7d 324 } else if (which_motor == spine) { // spine
langfordw 0:fc382eeb78ad 325 if (dir == 1) {
langfordw 0:fc382eeb78ad 326 spine_motorA = 0;
langfordw 0:fc382eeb78ad 327 spine_motorB = 1;
langfordw 0:fc382eeb78ad 328 } else {
langfordw 0:fc382eeb78ad 329 spine_motorA = 1;
langfordw 0:fc382eeb78ad 330 spine_motorB = 0;
langfordw 0:fc382eeb78ad 331 }
langfordw 0:fc382eeb78ad 332 spine_motorPWM.write(power);
langfordw 0:fc382eeb78ad 333 }
langfordw 0:fc382eeb78ad 334 }
langfordw 0:fc382eeb78ad 335
calisch 1:e549754ca234 336 float updateEncoder(int which_encoder) {
calisch 1:e549754ca234 337 last_n[which_encoder] = n[which_encoder];
calisch 1:e549754ca234 338 if(which_encoder == rear){
calisch 1:e549754ca234 339 n[which_encoder] = rear_encoder.getPulses();
calisch 1:e549754ca234 340 }
calisch 1:e549754ca234 341 else if(which_encoder == front){
calisch 1:e549754ca234 342 n[which_encoder] = front_encoder.getPulses();
calisch 1:e549754ca234 343 }
calisch 1:e549754ca234 344 else if(which_encoder == spine){
calisch 1:e549754ca234 345 n[which_encoder] = spine_encoder.getPulses();
calisch 1:e549754ca234 346 }
calisch 1:e549754ca234 347 w = (n[which_encoder]-last_n[which_encoder])*1.047; //steps/s --> rad/s
calisch 1:e549754ca234 348 return w;
calisch 1:e549754ca234 349 }
langfordw 0:fc382eeb78ad 350
calisch 4:7f9c9bd9da26 351 void shutdown(){
calisch 4:7f9c9bd9da26 352 tick.detach();
calisch 4:7f9c9bd9da26 353 tock.detach();
calisch 4:7f9c9bd9da26 354 updateMotor(rear, 0);
calisch 4:7f9c9bd9da26 355 updateMotor(front, 0);
calisch 4:7f9c9bd9da26 356 updateMotor(spine, 0);
calisch 4:7f9c9bd9da26 357 fclose(fp);
calisch 4:7f9c9bd9da26 358 kill_test = 1;
calisch 4:7f9c9bd9da26 359 }
calisch 4:7f9c9bd9da26 360
langfordw 0:fc382eeb78ad 361 void control() {
calisch 3:f68eaa68f4ec 362 if(getting_up){
calisch 4:7f9c9bd9da26 363 rear_n_d = -standing_position*(standing_step*current_sample)*n2d; //linear ramp up over 500 samples
calisch 4:7f9c9bd9da26 364 front_n_d = -bent_position*(standing_step*current_sample)*n2d; //linear ramp up over 500 samples
calisch 4:7f9c9bd9da26 365 spine_n_d = -spine_start*(standing_step*current_sample)*n2d;
calisch 3:f68eaa68f4ec 366 }
calisch 3:f68eaa68f4ec 367 else if(getting_down){
calisch 4:7f9c9bd9da26 368 rear_n_d = -standing_position*(1-sitting_step*current_sample)*n2d; //linear ramp up over 500 samples
calisch 4:7f9c9bd9da26 369 front_n_d = -bent_position*(1-sitting_step*current_sample)*n2d; //linear ramp up over 500 samples
calisch 4:7f9c9bd9da26 370 spine_n_d = -spine_start*(1-sitting_step*current_sample)*n2d;;
calisch 3:f68eaa68f4ec 371 }
calisch 3:f68eaa68f4ec 372 else{
calisch 3:f68eaa68f4ec 373 rear_n_d = -trajectory[current_sample][rear]*n2d;
calisch 3:f68eaa68f4ec 374 front_n_d = -trajectory[current_sample][front]*n2d;
calisch 3:f68eaa68f4ec 375 spine_n_d = -trajectory[current_sample][spine]*n2d;
calisch 3:f68eaa68f4ec 376 }
calisch 3:f68eaa68f4ec 377
langfordw 0:fc382eeb78ad 378 // rear
langfordw 0:fc382eeb78ad 379 i = read_current(rear);
calisch 1:e549754ca234 380 w = updateEncoder(rear);
calisch 1:e549754ca234 381 id = rear_Ks_p*(rear_n_d-n[rear]) + rear_Ks_d*(rear_w_d-w);
langfordw 0:fc382eeb78ad 382 sign = abs(id)/id;
langfordw 0:fc382eeb78ad 383 id = abs(id);
langfordw 0:fc382eeb78ad 384 pwm = sign*(id*R-sign*Kv*w+rear_Kp*(id-i))/Vs;
calisch 4:7f9c9bd9da26 385 if (avg_current[rear] > stall_current){pwm = 0;}
langfordw 0:fc382eeb78ad 386 updateMotor(rear,pwm);
langfordw 0:fc382eeb78ad 387
langfordw 0:fc382eeb78ad 388 // front
langfordw 0:fc382eeb78ad 389 i = read_current(front);
calisch 1:e549754ca234 390 w = updateEncoder(front);
calisch 1:e549754ca234 391 id = front_Ks_p*(front_n_d-n[front]) + front_Ks_d*(front_w_d-w);
langfordw 0:fc382eeb78ad 392 sign = abs(id)/id;
langfordw 0:fc382eeb78ad 393 id = abs(id);
langfordw 0:fc382eeb78ad 394 pwm = sign*(id*R-sign*Kv*w+front_Kp*(id-i))/Vs;
calisch 4:7f9c9bd9da26 395 if (avg_current[front] > stall_current){pwm = 0;}
langfordw 0:fc382eeb78ad 396 updateMotor(front,pwm);
langfordw 0:fc382eeb78ad 397
langfordw 0:fc382eeb78ad 398 // spine
langfordw 0:fc382eeb78ad 399 i = read_current(spine);
calisch 1:e549754ca234 400 w = updateEncoder(spine);
calisch 1:e549754ca234 401 id = spine_Ks_p*(spine_n_d-n[spine]) + spine_Ks_d*(spine_w_d-w);
langfordw 0:fc382eeb78ad 402 sign = abs(id)/id;
langfordw 0:fc382eeb78ad 403 id = abs(id);
langfordw 0:fc382eeb78ad 404 pwm = sign*(id*R-sign*Kv*w+spine_Kp*(id-i))/Vs;
calisch 4:7f9c9bd9da26 405 if (avg_current[spine] > stall_current){pwm = 0;}
langfordw 0:fc382eeb78ad 406 updateMotor(spine,pwm);
calisch 1:e549754ca234 407
calisch 3:f68eaa68f4ec 408 //timeout for motor safety
calisch 3:f68eaa68f4ec 409 if( avg_current[rear] > time_out_current){ time_out_count[rear]++;}
calisch 3:f68eaa68f4ec 410 if( avg_current[front] > time_out_current){ time_out_count[front]++;}
calisch 3:f68eaa68f4ec 411 if( avg_current[spine] > time_out_current){ time_out_count[spine]++;}
calisch 4:7f9c9bd9da26 412 if( time_out_count[rear]>time_out_steps){ rear_led=1;shutdown();}
calisch 4:7f9c9bd9da26 413 if( time_out_count[front]>time_out_steps){ front_led=1;shutdown();}
calisch 4:7f9c9bd9da26 414 if( time_out_count[spine]>time_out_steps){ spine_led=1;shutdown();}
calisch 3:f68eaa68f4ec 415
calisch 3:f68eaa68f4ec 416
calisch 3:f68eaa68f4ec 417
calisch 1:e549754ca234 418 //step to next control point
calisch 3:f68eaa68f4ec 419 if (getting_up){
calisch 4:7f9c9bd9da26 420 if(current_sample == standing_time){ getting_up = 0; current_sample = 0;} //we're up
calisch 3:f68eaa68f4ec 421 else{ current_sample++;} //still getting up
calisch 3:f68eaa68f4ec 422 }
calisch 3:f68eaa68f4ec 423 else if(getting_down){
calisch 4:7f9c9bd9da26 424 if(current_sample == sitting_time){ //we're down
calisch 4:7f9c9bd9da26 425 shutdown();
calisch 1:e549754ca234 426 }
calisch 3:f68eaa68f4ec 427 else{current_sample++;} //still getting down
calisch 3:f68eaa68f4ec 428 }
calisch 4:7f9c9bd9da26 429 else if (current_sample == n_samples-1){ //normal operation
calisch 4:7f9c9bd9da26 430 if (current_loop == n_loops-1){ getting_down = 1; current_sample=0;} //ready to sit
calisch 1:e549754ca234 431 else{ //end of loop, ready for next
calisch 1:e549754ca234 432 current_sample = 0;
calisch 1:e549754ca234 433 current_loop++;
calisch 1:e549754ca234 434 }
calisch 1:e549754ca234 435 }
calisch 3:f68eaa68f4ec 436 else{ current_sample++;} //middle of running loop
calisch 1:e549754ca234 437 }
langfordw 0:fc382eeb78ad 438
langfordw 0:fc382eeb78ad 439 void save() {
calisch 4:7f9c9bd9da26 440 pc.printf("%i %i %i %i %f %f %f %i\n", t.read_ms(), n[rear], n[front], n[spine], avg_current[0], avg_current[1], avg_current[2], current_sample);
calisch 4:7f9c9bd9da26 441 pc.printf("%i",rear_encoder.getPulses());
langfordw 0:fc382eeb78ad 442 }
langfordw 0:fc382eeb78ad 443
langfordw 0:fc382eeb78ad 444 int main() {
langfordw 0:fc382eeb78ad 445 rear_motorPWM.period(0.00005); //20kHz
langfordw 0:fc382eeb78ad 446 front_motorPWM.period(0.00005); //20kHz
langfordw 0:fc382eeb78ad 447 spine_motorPWM.period(0.00005); //20kHz
langfordw 0:fc382eeb78ad 448 tick.attach(&control,CONTROL_PERIOD);
langfordw 0:fc382eeb78ad 449 tock.attach(&save,SAVE_PERIOD);
calisch 1:e549754ca234 450 t.start();
langfordw 0:fc382eeb78ad 451
calisch 3:f68eaa68f4ec 452 while(~kill_test) {
langfordw 0:fc382eeb78ad 453 //DEBUG
calisch 4:7f9c9bd9da26 454 save();
calisch 4:7f9c9bd9da26 455 //pc.printf("%i %f %i %f %i %i\n", t.read_ms(), pwm, n, w, id, i);
langfordw 0:fc382eeb78ad 456 }
langfordw 0:fc382eeb78ad 457 }