Luccas Eagles / Mbed OS CONTROL_MOTOR_MELODY

added melody to old motorcontol

main.cpp@26:7f88907d7c54, 2020-04-11 (annotated)

Committer:
le1917
Date:
Sat Apr 11 13:44:39 2020 +0000
Revision:
26:7f88907d7c54
Parent:
25:4b893287d750
Child:
27:f2e43189b0f4
bugs fixed;

Who changed what in which revision?

UserRevisionLine numberNew contents of line
estott 0:de4320f74764 1 #include "mbed.h"
kubitz 13:58ada64bb664 2 #include "rtos.h"
le1917 25:4b893287d750 3 #include <string>
le1917 25:4b893287d750 4
estott 0:de4320f74764 5 //Photointerrupter input pins
estott 10:a4b5723b6c9d 6 #define I1pin D3
estott 10:a4b5723b6c9d 7 #define I2pin D6
estott 10:a4b5723b6c9d 8 #define I3pin D5
estott 2:4e88faab6988 9
estott 2:4e88faab6988 10 //Incremental encoder input pins
kubitz 24:6d85dda245e1 11 #define CHApin D12
kubitz 24:6d85dda245e1 12 #define CHBpin D11
estott 0:de4320f74764 13
estott 0:de4320f74764 14 //Motor Drive output pins //Mask in output byte
kubitz 24:6d85dda245e1 15 #define L1Lpin D1 //0x01
kubitz 24:6d85dda245e1 16 #define L1Hpin A3 //0x02
kubitz 24:6d85dda245e1 17 #define L2Lpin D0 //0x04
kubitz 24:6d85dda245e1 18 #define L2Hpin A6 //0x08
kubitz 24:6d85dda245e1 19 #define L3Lpin D10 //0x10
kubitz 24:6d85dda245e1 20 #define L3Hpin D2 //0x20
estott 10:a4b5723b6c9d 21
estott 10:a4b5723b6c9d 22 #define PWMpin D9
estott 5:08f338b5e4d9 23
estott 5:08f338b5e4d9 24 //Motor current sense
kubitz 24:6d85dda245e1 25 #define MCSPpin A1
kubitz 24:6d85dda245e1 26 #define MCSNpin A0
estott 0:de4320f74764 27
estott 11:5ff18183764a 28 //Test outputs
estott 11:5ff18183764a 29 #define TP0pin D4
estott 11:5ff18183764a 30 #define TP1pin D13
estott 11:5ff18183764a 31 #define TP2pin A2
estott 11:5ff18183764a 32
kubitz 14:0146695df1a5 33 Serial pc(SERIAL_TX, SERIAL_RX);
kubitz 14:0146695df1a5 34
le1917 25:4b893287d750 35 // Melody Variables
le1917 25:4b893287d750 36 int melody_length = 0;
le1917 25:4b893287d750 37 int melody_index = 0;
le1917 25:4b893287d750 38 int note_count = 1;
le1917 25:4b893287d750 39 float notes[16];
le1917 25:4b893287d750 40 int note_durations[16];
le1917 25:4b893287d750 41
le1917 25:4b893287d750 42
kubitz 22:af49b9e79d9c 43 // Controller variables
kubitz 14:0146695df1a5 44 Timer timer_velocity;
kubitz 14:0146695df1a5 45 uint32_t last_time_MtrCtlr;
kubitz 14:0146695df1a5 46 int i = 0;
kubitz 18:01977903e972 47
kubitz 22:af49b9e79d9c 48 // Velocity Controller Variables
le1917 25:4b893287d750 49 float target_velocity = 40;
kubitz 24:6d85dda245e1 50 float integral_vel = 0;
kubitz 24:6d85dda245e1 51 float derivative_vel = 0;
kubitz 24:6d85dda245e1 52 float last_vel_error = 0;
kubitz 22:af49b9e79d9c 53
kubitz 22:af49b9e79d9c 54 int previous_position = 0;
kubitz 24:6d85dda245e1 55 int current_position = 0;
kubitz 22:af49b9e79d9c 56
kubitz 22:af49b9e79d9c 57 float kp_vel = 20;
kubitz 24:6d85dda245e1 58 float ki_vel = 0;
kubitz 24:6d85dda245e1 59 float kd_vel = 0;
kubitz 22:af49b9e79d9c 60
kubitz 22:af49b9e79d9c 61 // Position Controller Variables
kubitz 22:af49b9e79d9c 62 float target_position = 500;
kubitz 24:6d85dda245e1 63 float integral_pos = 0;
kubitz 24:6d85dda245e1 64 float derivative_pos = 0;
kubitz 24:6d85dda245e1 65 float last_pos_error = 0;
kubitz 22:af49b9e79d9c 66
kubitz 22:af49b9e79d9c 67 float kp_pos = 20;
kubitz 24:6d85dda245e1 68 float ki_pos = 0;
kubitz 24:6d85dda245e1 69 float kd_pos = 10;
kubitz 22:af49b9e79d9c 70
estott 0:de4320f74764 71 //Mapping from sequential drive states to motor phase outputs
estott 0:de4320f74764 72 /*
estott 0:de4320f74764 73 State L1 L2 L3
estott 0:de4320f74764 74 0 H - L
estott 0:de4320f74764 75 1 - H L
estott 0:de4320f74764 76 2 L H -
estott 0:de4320f74764 77 3 L - H
estott 0:de4320f74764 78 4 - L H
estott 0:de4320f74764 79 5 H L -
estott 0:de4320f74764 80 6 - - -
estott 0:de4320f74764 81 7 - - -
estott 0:de4320f74764 82 */
estott 0:de4320f74764 83 //Drive state to output table
kubitz 24:6d85dda245e1 84 const int8_t driveTable[] = {0x12, 0x18, 0x09, 0x21, 0x24, 0x06, 0x00, 0x00};
estott 2:4e88faab6988 85
estott 0:de4320f74764 86 //Mapping from interrupter inputs to sequential rotor states. 0x00 and 0x07 are not valid
kubitz 24:6d85dda245e1 87 const int8_t stateMap[] = {0x07, 0x05, 0x03, 0x04, 0x01, 0x00, 0x02, 0x07};
estott 2:4e88faab6988 88 //const int8_t stateMap[] = {0x07,0x01,0x03,0x02,0x05,0x00,0x04,0x07}; //Alternative if phase order of input or drive is reversed
estott 2:4e88faab6988 89
kubitz 13:58ada64bb664 90
kubitz 13:58ada64bb664 91 // SHARED GLOBAL VARIABLES //
kubitz 13:58ada64bb664 92 Semaphore pos_semaphore(0);
kubitz 22:af49b9e79d9c 93 float position = 0;
kubitz 13:58ada64bb664 94 bool direction = 1;
le1917 25:4b893287d750 95 float pwm_period = 0.25f;
kubitz 13:58ada64bb664 96 int lead = 2;
kubitz 13:58ada64bb664 97
kubitz 13:58ada64bb664 98 // NON-SHARED GLOBAL VARIABLES //
kubitz 13:58ada64bb664 99 int lead_old;
estott 0:de4320f74764 100
le1917 25:4b893287d750 101
estott 0:de4320f74764 102 //Status LED
estott 0:de4320f74764 103 DigitalOut led1(LED1);
estott 0:de4320f74764 104
estott 0:de4320f74764 105 //Photointerrupter inputs
kubitz 13:58ada64bb664 106 InterruptIn I1(I1pin);
kubitz 13:58ada64bb664 107 InterruptIn I2(I2pin);
kubitz 13:58ada64bb664 108 InterruptIn I3(I3pin);
estott 0:de4320f74764 109
estott 0:de4320f74764 110 //Motor Drive outputs
estott 0:de4320f74764 111 DigitalOut L1L(L1Lpin);
estott 0:de4320f74764 112 DigitalOut L1H(L1Hpin);
estott 0:de4320f74764 113 DigitalOut L2L(L2Lpin);
estott 0:de4320f74764 114 DigitalOut L2H(L2Hpin);
estott 0:de4320f74764 115 DigitalOut L3L(L3Lpin);
estott 0:de4320f74764 116 DigitalOut L3H(L3Hpin);
estott 0:de4320f74764 117
estott 11:5ff18183764a 118 DigitalOut TP1(TP1pin);
estott 11:5ff18183764a 119 PwmOut MotorPWM(PWMpin);
estott 11:5ff18183764a 120
kubitz 13:58ada64bb664 121 Ticker motorCtrlTicker;
kubitz 24:6d85dda245e1 122 Thread thread_motorCtrl(osPriorityNormal, 1024);
kubitz 13:58ada64bb664 123
kubitz 24:6d85dda245e1 124 volatile int8_t orState = 0; //Rotot offset at motor state 0
kubitz 13:58ada64bb664 125 volatile int8_t intState = 0;
kubitz 13:58ada64bb664 126 volatile int8_t intStateOld = 0;
kubitz 13:58ada64bb664 127
estott 0:de4320f74764 128 //Set a given drive state
kubitz 13:58ada64bb664 129 void motorOut(int8_t driveState)
kubitz 13:58ada64bb664 130 {
kubitz 13:58ada64bb664 131
estott 2:4e88faab6988 132 //Lookup the output byte from the drive state.
estott 2:4e88faab6988 133 int8_t driveOut = driveTable[driveState & 0x07];
kubitz 13:58ada64bb664 134
estott 2:4e88faab6988 135 //Turn off first
kubitz 24:6d85dda245e1 136 if (~driveOut & 0x01)
kubitz 24:6d85dda245e1 137 L1L = 0;
kubitz 24:6d85dda245e1 138 if (~driveOut & 0x02)
kubitz 24:6d85dda245e1 139 L1H = 1;
kubitz 24:6d85dda245e1 140 if (~driveOut & 0x04)
kubitz 24:6d85dda245e1 141 L2L = 0;
kubitz 24:6d85dda245e1 142 if (~driveOut & 0x08)
kubitz 24:6d85dda245e1 143 L2H = 1;
kubitz 24:6d85dda245e1 144 if (~driveOut & 0x10)
kubitz 24:6d85dda245e1 145 L3L = 0;
kubitz 24:6d85dda245e1 146 if (~driveOut & 0x20)
kubitz 24:6d85dda245e1 147 L3H = 1;
kubitz 13:58ada64bb664 148
estott 2:4e88faab6988 149 //Then turn on
kubitz 24:6d85dda245e1 150 if (driveOut & 0x01)
kubitz 24:6d85dda245e1 151 L1L = 1;
kubitz 24:6d85dda245e1 152 if (driveOut & 0x02)
kubitz 24:6d85dda245e1 153 L1H = 0;
kubitz 24:6d85dda245e1 154 if (driveOut & 0x04)
kubitz 24:6d85dda245e1 155 L2L = 1;
kubitz 24:6d85dda245e1 156 if (driveOut & 0x08)
kubitz 24:6d85dda245e1 157 L2H = 0;
kubitz 24:6d85dda245e1 158 if (driveOut & 0x10)
kubitz 24:6d85dda245e1 159 L3L = 1;
kubitz 24:6d85dda245e1 160 if (driveOut & 0x20)
kubitz 24:6d85dda245e1 161 L3H = 0;
kubitz 13:58ada64bb664 162 }
kubitz 13:58ada64bb664 163
kubitz 13:58ada64bb664 164 //Convert photointerrupter inputs to a rotor state
kubitz 13:58ada64bb664 165 inline int8_t readRotorState()
kubitz 13:58ada64bb664 166 {
kubitz 24:6d85dda245e1 167 return stateMap[I1 + 2 * I2 + 4 * I3];
kubitz 13:58ada64bb664 168 }
kubitz 13:58ada64bb664 169
kubitz 13:58ada64bb664 170 void move()
kubitz 13:58ada64bb664 171 {
kubitz 13:58ada64bb664 172 intState = readRotorState();
kubitz 13:58ada64bb664 173
kubitz 13:58ada64bb664 174 // Updates direction only if statechanges by 1
kubitz 13:58ada64bb664 175 // If state chance is missed then interrupt is unchanged
kubitz 24:6d85dda245e1 176 if (intState == 0 && intStateOld == 5)
kubitz 24:6d85dda245e1 177 {
kubitz 13:58ada64bb664 178 direction = 1;
kubitz 13:58ada64bb664 179 position++;
kubitz 24:6d85dda245e1 180 }
kubitz 24:6d85dda245e1 181 else if (intState == 5 && intStateOld == 0)
kubitz 24:6d85dda245e1 182 {
kubitz 13:58ada64bb664 183 direction = 0;
kubitz 13:58ada64bb664 184 position--;
kubitz 24:6d85dda245e1 185 }
kubitz 24:6d85dda245e1 186 else if (intState == intStateOld + 1)
kubitz 24:6d85dda245e1 187 {
kubitz 13:58ada64bb664 188 direction = 1;
kubitz 13:58ada64bb664 189 position++;
kubitz 24:6d85dda245e1 190 }
kubitz 24:6d85dda245e1 191 else if (intState == intStateOld - 1)
kubitz 24:6d85dda245e1 192 {
kubitz 13:58ada64bb664 193 direction = 0;
kubitz 13:58ada64bb664 194 position--;
estott 0:de4320f74764 195 }
kubitz 13:58ada64bb664 196
kubitz 13:58ada64bb664 197 pos_semaphore.release();
kubitz 13:58ada64bb664 198
kubitz 13:58ada64bb664 199 intStateOld = intState;
kubitz 13:58ada64bb664 200
kubitz 24:6d85dda245e1 201 motorOut((intState - orState + lead + 6) % 6); //+6 to make sure the remainder is positive
kubitz 24:6d85dda245e1 202 }
kubitz 13:58ada64bb664 203
kubitz 13:58ada64bb664 204 void motorCtrlTick()
kubitz 13:58ada64bb664 205 {
kubitz 14:0146695df1a5 206 thread_motorCtrl.signal_set(0x1);
kubitz 14:0146695df1a5 207 //osSignalSet(thread_motorCtrl, 0x1);
kubitz 13:58ada64bb664 208 }
kubitz 13:58ada64bb664 209
kubitz 22:af49b9e79d9c 210 float get_vel_control_out(float velocity_error)
kubitz 24:6d85dda245e1 211 {
kubitz 24:6d85dda245e1 212 integral_vel += velocity_error;
kubitz 24:6d85dda245e1 213 derivative_vel = velocity_error - last_vel_error;
kubitz 24:6d85dda245e1 214 float velocity_out = (kp_vel * velocity_error) + (ki_vel * integral_vel) + (kd_vel * derivative_vel);
kubitz 24:6d85dda245e1 215 last_vel_error = velocity_error;
kubitz 18:01977903e972 216 return velocity_out;
kubitz 13:58ada64bb664 217 }
kubitz 13:58ada64bb664 218
kubitz 24:6d85dda245e1 219 float get_pos_control_out(float position_error)
kubitz 24:6d85dda245e1 220 {
kubitz 24:6d85dda245e1 221 integral_pos += position_error;
kubitz 24:6d85dda245e1 222 derivative_pos = position_error - last_pos_error;
kubitz 24:6d85dda245e1 223 float velocity_out = (kp_pos * position_error) + (ki_pos * integral_pos) + (kd_pos * derivative_pos);
kubitz 24:6d85dda245e1 224 last_pos_error = position_error;
kubitz 24:6d85dda245e1 225 return velocity_out;
kubitz 13:58ada64bb664 226 }
kubitz 13:58ada64bb664 227
kubitz 17:baffdedf9590 228 float get_motor_out(float velocity_out)
kubitz 13:58ada64bb664 229 {
kubitz 13:58ada64bb664 230 float motor_out;
kubitz 24:6d85dda245e1 231 if (velocity_out < 0)
kubitz 24:6d85dda245e1 232 {
kubitz 24:6d85dda245e1 233 motor_out = velocity_out * -1;
kubitz 24:6d85dda245e1 234 }
kubitz 24:6d85dda245e1 235 else
kubitz 24:6d85dda245e1 236 {
kubitz 17:baffdedf9590 237 motor_out = velocity_out;
kubitz 13:58ada64bb664 238 }
kubitz 24:6d85dda245e1 239 if (velocity_out > 1 || velocity_out < -1)
kubitz 24:6d85dda245e1 240 {
kubitz 13:58ada64bb664 241 motor_out = 1;
kubitz 13:58ada64bb664 242 }
kubitz 13:58ada64bb664 243 return motor_out;
estott 0:de4320f74764 244 }
kubitz 13:58ada64bb664 245
kubitz 24:6d85dda245e1 246 int get_current_position()
kubitz 24:6d85dda245e1 247 {
kubitz 13:58ada64bb664 248 pos_semaphore.wait();
kubitz 13:58ada64bb664 249 return position;
kubitz 13:58ada64bb664 250 }
kubitz 13:58ada64bb664 251
kubitz 24:6d85dda245e1 252 void attach_ISR()
kubitz 24:6d85dda245e1 253 {
kubitz 13:58ada64bb664 254 I1.rise(&move);
kubitz 13:58ada64bb664 255 I1.fall(&move);
kubitz 13:58ada64bb664 256 I2.rise(&move);
kubitz 13:58ada64bb664 257 I2.fall(&move);
kubitz 13:58ada64bb664 258 I3.rise(&move);
kubitz 13:58ada64bb664 259 I3.fall(&move);
kubitz 13:58ada64bb664 260 }
kubitz 13:58ada64bb664 261
kubitz 22:af49b9e79d9c 262 float combine_control_out(float position_control_out, float velocity_control_out, float current_velocity)
kubitz 13:58ada64bb664 263 {
kubitz 24:6d85dda245e1 264 float velocity_out = 0;
kubitz 24:6d85dda245e1 265 if (current_velocity <= 0)
kubitz 24:6d85dda245e1 266 {
kubitz 24:6d85dda245e1 267 velocity_out = std::max(position_control_out, velocity_control_out);
kubitz 24:6d85dda245e1 268 }
kubitz 24:6d85dda245e1 269 else
kubitz 24:6d85dda245e1 270 {
kubitz 24:6d85dda245e1 271 velocity_out = std::min(position_control_out, velocity_control_out);
estott 2:4e88faab6988 272 }
kubitz 22:af49b9e79d9c 273 return velocity_out;
kubitz 13:58ada64bb664 274 }
kubitz 13:58ada64bb664 275
kubitz 24:6d85dda245e1 276 float get_current_velocity(float current_position)
kubitz 24:6d85dda245e1 277 {
kubitz 24:6d85dda245e1 278 float velocity_factor = (1000 / (timer_velocity.read_ms() - last_time_MtrCtlr));
kubitz 24:6d85dda245e1 279 float velocity = ((current_position - previous_position) / 6) * velocity_factor;
kubitz 24:6d85dda245e1 280 last_time_MtrCtlr = timer_velocity.read_ms();
kubitz 24:6d85dda245e1 281 previous_position = current_position;
kubitz 24:6d85dda245e1 282 return velocity;
kubitz 22:af49b9e79d9c 283 }
kubitz 13:58ada64bb664 284
kubitz 24:6d85dda245e1 285 void update_lead(float velocity_out)
kubitz 24:6d85dda245e1 286 {
kubitz 13:58ada64bb664 287 // No functionality for breaking
kubitz 24:6d85dda245e1 288 if (velocity_out >= 0)
kubitz 24:6d85dda245e1 289 {
kubitz 13:58ada64bb664 290 lead = 2;
kubitz 13:58ada64bb664 291 }
kubitz 24:6d85dda245e1 292 else
kubitz 24:6d85dda245e1 293 {
kubitz 13:58ada64bb664 294 lead = -2;
kubitz 13:58ada64bb664 295 }
estott 0:de4320f74764 296 }
le1917 25:4b893287d750 297
le1917 25:4b893287d750 298 void process_melody(std::string input){
le1917 25:4b893287d750 299
le1917 26:7f88907d7c54 300 melody_length = 0;
le1917 25:4b893287d750 301
le1917 25:4b893287d750 302 for(int i = 0; i < 16; i++){
le1917 25:4b893287d750 303 notes[i] = 0;
le1917 25:4b893287d750 304 note_durations[i] = 0;
le1917 25:4b893287d750 305 }
le1917 25:4b893287d750 306
le1917 25:4b893287d750 307 int note_index = 0;
le1917 25:4b893287d750 308 for(int i = 0; i < input.length(); i++){
le1917 25:4b893287d750 309 switch(input[i]){
le1917 25:4b893287d750 310 case 'C':
le1917 25:4b893287d750 311 if(input[i+1] == '#'){
le1917 25:4b893287d750 312 notes[note_index] = 0.003607764;
le1917 25:4b893287d750 313 i++;
le1917 25:4b893287d750 314 }
le1917 25:4b893287d750 315 else{
le1917 25:4b893287d750 316 notes[note_index] = 0.003822192;
le1917 25:4b893287d750 317 }
le1917 25:4b893287d750 318 break;
le1917 25:4b893287d750 319 case 'D':
le1917 25:4b893287d750 320 if(input[i+1] == '^'){
le1917 25:4b893287d750 321 notes[note_index] = 0.003607764;
le1917 25:4b893287d750 322 i++;
le1917 25:4b893287d750 323 }
le1917 25:4b893287d750 324 else if(input[i+1] == '#'){
le1917 25:4b893287d750 325 notes[note_index] = 0.003214091;
le1917 25:4b893287d750 326 i++;
le1917 25:4b893287d750 327 }
le1917 25:4b893287d750 328 else{
le1917 25:4b893287d750 329 notes[note_index] = 0.003405299;
le1917 25:4b893287d750 330 }
le1917 25:4b893287d750 331 break;
le1917 25:4b893287d750 332 case 'E':
le1917 25:4b893287d750 333 if(input[i+1] == '^'){
le1917 25:4b893287d750 334 notes[note_index] =0.003214091;
le1917 25:4b893287d750 335 i++;
le1917 25:4b893287d750 336 }
le1917 25:4b893287d750 337 else{
le1917 25:4b893287d750 338 notes[note_index] =0.003033704;
le1917 25:4b893287d750 339 }
le1917 25:4b893287d750 340 break;
le1917 25:4b893287d750 341 case 'F':
le1917 25:4b893287d750 342 if(input[i+1] == '#'){
le1917 25:4b893287d750 343 notes[note_index] =0.002702776;
le1917 25:4b893287d750 344 i++;
le1917 25:4b893287d750 345 }
le1917 25:4b893287d750 346 else{
le1917 25:4b893287d750 347 notes[note_index] = 0.002863442;
le1917 25:4b893287d750 348 }
le1917 25:4b893287d750 349 break;
le1917 25:4b893287d750 350 case 'G':
le1917 25:4b893287d750 351 if(input[i+1] == '^'){
le1917 25:4b893287d750 352 notes[note_index] = 0.002702776;
le1917 25:4b893287d750 353 i++;
le1917 25:4b893287d750 354 }
le1917 25:4b893287d750 355 else if(input[i+1] == '#'){
le1917 25:4b893287d750 356 notes[note_index] = 0.002407898;
le1917 25:4b893287d750 357 i++;
le1917 25:4b893287d750 358 }
le1917 25:4b893287d750 359 else{
le1917 25:4b893287d750 360 notes[note_index] =0.00255102;
le1917 25:4b893287d750 361 }
le1917 25:4b893287d750 362 break;
le1917 25:4b893287d750 363 case 'A':
le1917 25:4b893287d750 364 if(input[i+1] == '^'){
le1917 25:4b893287d750 365 notes[note_index] =0.002407898;
le1917 25:4b893287d750 366 i++;
le1917 25:4b893287d750 367 }
le1917 25:4b893287d750 368 else if(input[i+1] == '#'){
le1917 25:4b893287d750 369 notes[note_index] = 0.002145186;
le1917 25:4b893287d750 370 i++;
le1917 25:4b893287d750 371 }
le1917 25:4b893287d750 372 else{
le1917 25:4b893287d750 373 notes[note_index] =0.002272727;
le1917 25:4b893287d750 374 }
le1917 25:4b893287d750 375 break;
le1917 25:4b893287d750 376 case 'B':
le1917 25:4b893287d750 377 if(input[i+1] == '^'){
le1917 25:4b893287d750 378 notes[note_index] = 0.002145186;
le1917 25:4b893287d750 379 i++;
le1917 25:4b893287d750 380 }
le1917 25:4b893287d750 381 else{
le1917 25:4b893287d750 382 notes[note_index] = 0.002024783;
le1917 25:4b893287d750 383 }
le1917 25:4b893287d750 384 break;
le1917 25:4b893287d750 385 }
le1917 25:4b893287d750 386
le1917 25:4b893287d750 387
le1917 25:4b893287d750 388 i++;
le1917 25:4b893287d750 389 // convert -'0' to convert char to int
le1917 25:4b893287d750 390 note_durations[note_index] = input[i] - '0';
le1917 25:4b893287d750 391
le1917 25:4b893287d750 392 note_index++;
le1917 25:4b893287d750 393 melody_length ++;
le1917 25:4b893287d750 394
le1917 25:4b893287d750 395 }
le1917 25:4b893287d750 396
le1917 25:4b893287d750 397 }
le1917 25:4b893287d750 398
le1917 25:4b893287d750 399
le1917 25:4b893287d750 400
le1917 25:4b893287d750 401 float get_period(){
le1917 25:4b893287d750 402
le1917 26:7f88907d7c54 403
le1917 25:4b893287d750 404 int curr_note_length = note_durations[melody_index];
le1917 25:4b893287d750 405 float curr_note_period = notes[melody_index];
le1917 26:7f88907d7c54 406
le1917 25:4b893287d750 407
le1917 25:4b893287d750 408 if( note_count >= curr_note_length ){
le1917 25:4b893287d750 409
le1917 25:4b893287d750 410 melody_index = (melody_index + 1)%melody_length;
le1917 25:4b893287d750 411 note_count = 1;
le1917 25:4b893287d750 412 }
le1917 25:4b893287d750 413 else{
le1917 25:4b893287d750 414 note_count = note_count + 1;
le1917 25:4b893287d750 415 }
le1917 26:7f88907d7c54 416
le1917 26:7f88907d7c54 417
le1917 26:7f88907d7c54 418 pc.printf("Period = %f, melody_index = %d, note_count = %d\r\n", curr_note_period, melody_index, note_count);
le1917 26:7f88907d7c54 419
le1917 25:4b893287d750 420 return curr_note_period;
le1917 25:4b893287d750 421 }
le1917 25:4b893287d750 422
le1917 25:4b893287d750 423
kubitz 14:0146695df1a5 424 void motorInitSequence()
kubitz 13:58ada64bb664 425 {
le1917 25:4b893287d750 426 motorCtrlTicker.attach_us(&motorCtrlTick, 125000);
kubitz 22:af49b9e79d9c 427 last_pos_error = target_position;
kubitz 15:2c9b3251c383 428 last_time_MtrCtlr = 0;
kubitz 24:6d85dda245e1 429
kubitz 13:58ada64bb664 430 MotorPWM.write(1);
kubitz 13:58ada64bb664 431 MotorPWM.period(pwm_period);
kubitz 13:58ada64bb664 432
kubitz 13:58ada64bb664 433 motorOut(0);
kubitz 13:58ada64bb664 434 wait(3.0);
kubitz 13:58ada64bb664 435 orState = readRotorState();
kubitz 19:e95f6004f365 436 attach_ISR();
kubitz 19:e95f6004f365 437
kubitz 24:6d85dda245e1 438 if (target_velocity > 0)
kubitz 24:6d85dda245e1 439 {
kubitz 13:58ada64bb664 440 lead = 2;
kubitz 24:6d85dda245e1 441 for (int i = 1; i < 4; i++)
kubitz 24:6d85dda245e1 442 {
kubitz 23:e5a2e8cef243 443 motorOut(i);
kubitz 24:6d85dda245e1 444 wait(0.2);
kubitz 23:e5a2e8cef243 445 }
kubitz 13:58ada64bb664 446 }
kubitz 24:6d85dda245e1 447 else
kubitz 24:6d85dda245e1 448 {
kubitz 13:58ada64bb664 449 lead = -2;
kubitz 24:6d85dda245e1 450 for (int i = 5; i > 2; i--)
kubitz 24:6d85dda245e1 451 {
kubitz 23:e5a2e8cef243 452 motorOut(i);
kubitz 24:6d85dda245e1 453 wait(0.2);
kubitz 23:e5a2e8cef243 454 }
kubitz 13:58ada64bb664 455 }
kubitz 17:baffdedf9590 456 current_position = get_current_position();
kubitz 17:baffdedf9590 457 previous_position = current_position;
kubitz 15:2c9b3251c383 458 timer_velocity.start();
kubitz 14:0146695df1a5 459 }
kubitz 14:0146695df1a5 460
le1917 25:4b893287d750 461
kubitz 14:0146695df1a5 462 void motorCtrlFn()
kubitz 24:6d85dda245e1 463 {
kubitz 24:6d85dda245e1 464 while (1)
kubitz 24:6d85dda245e1 465 {
kubitz 16:53d3445dcf6b 466 thread_motorCtrl.signal_wait(0x1);
kubitz 22:af49b9e79d9c 467
kubitz 17:baffdedf9590 468 current_position = get_current_position();
le1917 26:7f88907d7c54 469 pwm_period = get_period();
kubitz 24:6d85dda245e1 470 float current_velocity = get_current_velocity(current_position);
kubitz 20:4371ed979fbf 471
kubitz 24:6d85dda245e1 472 float velocity_error = target_velocity - current_velocity;
kubitz 24:6d85dda245e1 473 float velocity_control_out = get_vel_control_out(velocity_error);
kubitz 24:6d85dda245e1 474
kubitz 24:6d85dda245e1 475 float position_error = target_position - (current_position / 6);
kubitz 22:af49b9e79d9c 476 float position_control_out = get_pos_control_out(position_error);
kubitz 21:302d9043cb4b 477
kubitz 24:6d85dda245e1 478 float velocity_out = combine_control_out(position_control_out, velocity_control_out, current_velocity);
kubitz 24:6d85dda245e1 479 float motor_out = get_motor_out(position_control_out);
le1917 25:4b893287d750 480
kubitz 22:af49b9e79d9c 481 update_lead(velocity_out);
kubitz 13:58ada64bb664 482 MotorPWM.period(pwm_period);
le1917 26:7f88907d7c54 483 MotorPWM.write(0.6f);
le1917 25:4b893287d750 484
le1917 26:7f88907d7c54 485
kubitz 13:58ada64bb664 486 }
kubitz 24:6d85dda245e1 487 }
kubitz 13:58ada64bb664 488
kubitz 13:58ada64bb664 489 //Main
kubitz 13:58ada64bb664 490 int main()
kubitz 13:58ada64bb664 491 {
le1917 25:4b893287d750 492 process_melody("A4C8G4F#8");
le1917 25:4b893287d750 493
le1917 25:4b893287d750 494 /*
le1917 25:4b893287d750 495 pc.printf("A = %f, C = %f, G = %f, F# = %f\r\n", notes[0], notes[1], notes[2], notes[3]);
le1917 25:4b893287d750 496 pc.printf("length A = %d, length C = %d, length G = %d, length F# = %d\r\n", note_durations[0], note_durations[1], note_durations[2], note_durations[3]);
le1917 25:4b893287d750 497 */
le1917 25:4b893287d750 498
kubitz 14:0146695df1a5 499 motorInitSequence();
kubitz 14:0146695df1a5 500 thread_motorCtrl.start(motorCtrlFn);
kubitz 13:58ada64bb664 501 }