embedded code for bounding robot
Fork of bounding by
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?
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 | #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 | } |