![](/media/cache/img/default_profile.jpg.50x50_q85.jpg)
SMARTASSES 2019
main.cpp@12:b39f69ed20af, 2019-03-22 (annotated)
- Committer:
- OmarMuttawa
- Date:
- Fri Mar 22 22:28:24 2019 +0000
- Revision:
- 12:b39f69ed20af
- Parent:
- 11:aae7c9c290e2
FINALFINAL;
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
OmarMuttawa | 12:b39f69ed20af | 1 | //SMARTASSES |
OmarMuttawa | 12:b39f69ed20af | 2 | |
estott | 0:de4320f74764 | 3 | #include "mbed.h" |
OmarMuttawa | 11:aae7c9c290e2 | 4 | #include "SHA256.h" |
OmarMuttawa | 11:aae7c9c290e2 | 5 | #include <string> |
OmarMuttawa | 11:aae7c9c290e2 | 6 | #include <vector> |
OmarMuttawa | 11:aae7c9c290e2 | 7 | #include <RawSerial.h> |
estott | 0:de4320f74764 | 8 | |
estott | 0:de4320f74764 | 9 | //Photointerrupter input pins |
estott | 10:a4b5723b6c9d | 10 | #define I1pin D3 |
estott | 10:a4b5723b6c9d | 11 | #define I2pin D6 |
estott | 10:a4b5723b6c9d | 12 | #define I3pin D5 |
estott | 2:4e88faab6988 | 13 | |
estott | 2:4e88faab6988 | 14 | //Incremental encoder input pins |
estott | 10:a4b5723b6c9d | 15 | #define CHApin D12 |
estott | 10:a4b5723b6c9d | 16 | #define CHBpin D11 |
estott | 0:de4320f74764 | 17 | |
OmarMuttawa | 11:aae7c9c290e2 | 18 | //Motor Drive output pins //Mask in output byte |
OmarMuttawa | 11:aae7c9c290e2 | 19 | #define L1Lpin D1 //0x01 |
OmarMuttawa | 11:aae7c9c290e2 | 20 | #define L1Hpin A3 //0x02 |
OmarMuttawa | 11:aae7c9c290e2 | 21 | #define L2Lpin D0 //0x04 |
estott | 10:a4b5723b6c9d | 22 | #define L2Hpin A6 //0x08 |
OmarMuttawa | 11:aae7c9c290e2 | 23 | #define L3Lpin D10 //0x10 |
estott | 10:a4b5723b6c9d | 24 | #define L3Hpin D2 //0x20 |
estott | 10:a4b5723b6c9d | 25 | |
estott | 10:a4b5723b6c9d | 26 | #define PWMpin D9 |
estott | 5:08f338b5e4d9 | 27 | |
estott | 5:08f338b5e4d9 | 28 | //Motor current sense |
estott | 5:08f338b5e4d9 | 29 | #define MCSPpin A1 |
estott | 5:08f338b5e4d9 | 30 | #define MCSNpin A0 |
estott | 0:de4320f74764 | 31 | |
OmarMuttawa | 11:aae7c9c290e2 | 32 | #define TESTPIN D4 //USED FOR TIMING ETC |
OmarMuttawa | 11:aae7c9c290e2 | 33 | |
estott | 0:de4320f74764 | 34 | //Mapping from sequential drive states to motor phase outputs |
estott | 0:de4320f74764 | 35 | /* |
estott | 0:de4320f74764 | 36 | State L1 L2 L3 |
estott | 0:de4320f74764 | 37 | 0 H - L |
estott | 0:de4320f74764 | 38 | 1 - H L |
estott | 0:de4320f74764 | 39 | 2 L H - |
estott | 0:de4320f74764 | 40 | 3 L - H |
estott | 0:de4320f74764 | 41 | 4 - L H |
estott | 0:de4320f74764 | 42 | 5 H L - |
estott | 0:de4320f74764 | 43 | 6 - - - |
estott | 0:de4320f74764 | 44 | 7 - - - |
estott | 0:de4320f74764 | 45 | */ |
estott | 0:de4320f74764 | 46 | //Drive state to output table |
estott | 0:de4320f74764 | 47 | const int8_t driveTable[] = {0x12,0x18,0x09,0x21,0x24,0x06,0x00,0x00}; |
estott | 2:4e88faab6988 | 48 | |
estott | 0:de4320f74764 | 49 | //Mapping from interrupter inputs to sequential rotor states. 0x00 and 0x07 are not valid |
estott | 2:4e88faab6988 | 50 | const int8_t stateMap[] = {0x07,0x05,0x03,0x04,0x01,0x00,0x02,0x07}; |
estott | 2:4e88faab6988 | 51 | //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 | 52 | |
estott | 2:4e88faab6988 | 53 | //Phase lead to make motor spin |
OmarMuttawa | 11:aae7c9c290e2 | 54 | volatile int8_t lead = 2; //2 for forwards, -2 for backwards |
estott | 0:de4320f74764 | 55 | |
estott | 0:de4320f74764 | 56 | //Status LED |
estott | 0:de4320f74764 | 57 | DigitalOut led1(LED1); |
OmarMuttawa | 11:aae7c9c290e2 | 58 | DigitalOut testPin(TESTPIN); |
estott | 0:de4320f74764 | 59 | |
estott | 0:de4320f74764 | 60 | //Photointerrupter inputs |
OmarMuttawa | 11:aae7c9c290e2 | 61 | InterruptIn I1(I1pin); |
OmarMuttawa | 11:aae7c9c290e2 | 62 | InterruptIn I2(I2pin); |
OmarMuttawa | 11:aae7c9c290e2 | 63 | InterruptIn I3(I3pin); |
OmarMuttawa | 11:aae7c9c290e2 | 64 | |
OmarMuttawa | 11:aae7c9c290e2 | 65 | //Global varibale to count revolutions up to 6 |
OmarMuttawa | 11:aae7c9c290e2 | 66 | volatile int count_revolution = 0; |
OmarMuttawa | 11:aae7c9c290e2 | 67 | volatile int number_of_notes = 0; |
OmarMuttawa | 11:aae7c9c290e2 | 68 | volatile int last_count_revolution = 0; |
OmarMuttawa | 11:aae7c9c290e2 | 69 | //TUNING PARAMETERS FOR SPEED CONTROLLER |
OmarMuttawa | 11:aae7c9c290e2 | 70 | volatile float K_PS = 0.1; |
OmarMuttawa | 11:aae7c9c290e2 | 71 | volatile float K_IS = 0.18; |
OmarMuttawa | 11:aae7c9c290e2 | 72 | volatile float K_DS = 0.0001; |
OmarMuttawa | 11:aae7c9c290e2 | 73 | volatile float VELOCITY_INTEGRAL_LIMIT = 100; |
OmarMuttawa | 11:aae7c9c290e2 | 74 | //TUNING PARAMETERS FOR REVOLUTION CONTROLLER |
OmarMuttawa | 11:aae7c9c290e2 | 75 | volatile float K_PR= 0.1; |
OmarMuttawa | 11:aae7c9c290e2 | 76 | volatile float K_IR = 0.001; |
OmarMuttawa | 11:aae7c9c290e2 | 77 | volatile float K_DR = 0.05; |
OmarMuttawa | 11:aae7c9c290e2 | 78 | volatile float ROTATION_INTEGRAL_LIMIT = 100; |
OmarMuttawa | 11:aae7c9c290e2 | 79 | |
OmarMuttawa | 11:aae7c9c290e2 | 80 | float timeSinceLastRun; |
OmarMuttawa | 11:aae7c9c290e2 | 81 | float elapsedTime = 0; |
OmarMuttawa | 11:aae7c9c290e2 | 82 | |
OmarMuttawa | 11:aae7c9c290e2 | 83 | string tune; |
OmarMuttawa | 11:aae7c9c290e2 | 84 | char tune_c_str[50]; |
OmarMuttawa | 11:aae7c9c290e2 | 85 | |
OmarMuttawa | 11:aae7c9c290e2 | 86 | char notes [50]; //hold the notes |
OmarMuttawa | 11:aae7c9c290e2 | 87 | float periods[50]; |
OmarMuttawa | 11:aae7c9c290e2 | 88 | int durations[50]; //holds the durations |
OmarMuttawa | 11:aae7c9c290e2 | 89 | volatile bool playTune = false; |
OmarMuttawa | 11:aae7c9c290e2 | 90 | bool playTuneLocal = false; //used to prevent deadlocks and long mutexs |
OmarMuttawa | 11:aae7c9c290e2 | 91 | int noteIndex = 0; |
OmarMuttawa | 11:aae7c9c290e2 | 92 | |
OmarMuttawa | 11:aae7c9c290e2 | 93 | //PWM Class: |
OmarMuttawa | 11:aae7c9c290e2 | 94 | PwmOut PWM_pin(PWMpin); |
estott | 0:de4320f74764 | 95 | |
estott | 0:de4320f74764 | 96 | //Motor Drive outputs |
estott | 0:de4320f74764 | 97 | DigitalOut L1L(L1Lpin); |
estott | 0:de4320f74764 | 98 | DigitalOut L1H(L1Hpin); |
estott | 0:de4320f74764 | 99 | DigitalOut L2L(L2Lpin); |
estott | 0:de4320f74764 | 100 | DigitalOut L2H(L2Hpin); |
estott | 0:de4320f74764 | 101 | DigitalOut L3L(L3Lpin); |
estott | 0:de4320f74764 | 102 | DigitalOut L3H(L3Hpin); |
estott | 0:de4320f74764 | 103 | |
OmarMuttawa | 11:aae7c9c290e2 | 104 | /* Mail */ |
OmarMuttawa | 11:aae7c9c290e2 | 105 | typedef struct { |
OmarMuttawa | 11:aae7c9c290e2 | 106 | int type; //0 means it will be a string, 1 means it will be uint64 |
OmarMuttawa | 11:aae7c9c290e2 | 107 | uint64_t uint64_message; |
OmarMuttawa | 11:aae7c9c290e2 | 108 | char* string_message; |
OmarMuttawa | 11:aae7c9c290e2 | 109 | float float_message; |
OmarMuttawa | 11:aae7c9c290e2 | 110 | } mail_t; |
OmarMuttawa | 11:aae7c9c290e2 | 111 | |
OmarMuttawa | 11:aae7c9c290e2 | 112 | //Declaration of timer global variable |
OmarMuttawa | 11:aae7c9c290e2 | 113 | Timer t; |
OmarMuttawa | 11:aae7c9c290e2 | 114 | |
OmarMuttawa | 11:aae7c9c290e2 | 115 | //Declaration of timer for velocity calculations global variable |
OmarMuttawa | 11:aae7c9c290e2 | 116 | Timer motorCtrlTimer; |
OmarMuttawa | 11:aae7c9c290e2 | 117 | |
OmarMuttawa | 11:aae7c9c290e2 | 118 | //Threads' declaration |
OmarMuttawa | 11:aae7c9c290e2 | 119 | Thread printMsgT; |
OmarMuttawa | 11:aae7c9c290e2 | 120 | Thread readMsgT; |
OmarMuttawa | 11:aae7c9c290e2 | 121 | Thread motorCtrlT(osPriorityHigh,1024); |
OmarMuttawa | 11:aae7c9c290e2 | 122 | |
OmarMuttawa | 11:aae7c9c290e2 | 123 | //Declaration of Mail global object |
OmarMuttawa | 11:aae7c9c290e2 | 124 | Mail<mail_t, 16> mail_box; |
OmarMuttawa | 11:aae7c9c290e2 | 125 | |
OmarMuttawa | 11:aae7c9c290e2 | 126 | //Global to keep to previous position necessary for velocity calculations |
OmarMuttawa | 11:aae7c9c290e2 | 127 | float prevPosition; |
OmarMuttawa | 11:aae7c9c290e2 | 128 | |
OmarMuttawa | 11:aae7c9c290e2 | 129 | //Global for motor's position |
OmarMuttawa | 11:aae7c9c290e2 | 130 | float motor_position = 0; |
OmarMuttawa | 11:aae7c9c290e2 | 131 | float current_position = 0; |
OmarMuttawa | 11:aae7c9c290e2 | 132 | |
OmarMuttawa | 11:aae7c9c290e2 | 133 | //Global variables |
OmarMuttawa | 11:aae7c9c290e2 | 134 | int8_t intState,intStateOld,orState; |
OmarMuttawa | 11:aae7c9c290e2 | 135 | |
OmarMuttawa | 11:aae7c9c290e2 | 136 | RawSerial pc(SERIAL_TX, SERIAL_RX); |
OmarMuttawa | 11:aae7c9c290e2 | 137 | Queue<void, 8> inCharQ; |
OmarMuttawa | 11:aae7c9c290e2 | 138 | uint64_t newKey; |
OmarMuttawa | 11:aae7c9c290e2 | 139 | float Ts,Tr,newTorque; |
OmarMuttawa | 11:aae7c9c290e2 | 140 | volatile float rot_input=0; |
OmarMuttawa | 11:aae7c9c290e2 | 141 | volatile float max_vel = 10; |
OmarMuttawa | 11:aae7c9c290e2 | 142 | volatile int exec_count = 0; |
OmarMuttawa | 11:aae7c9c290e2 | 143 | |
OmarMuttawa | 11:aae7c9c290e2 | 144 | Mutex newKey_mutex; |
OmarMuttawa | 11:aae7c9c290e2 | 145 | Mutex newRotation_mutex; |
OmarMuttawa | 11:aae7c9c290e2 | 146 | Mutex newVelocity_mutex; |
OmarMuttawa | 11:aae7c9c290e2 | 147 | Mutex countRev_mutex; |
OmarMuttawa | 11:aae7c9c290e2 | 148 | Mutex playTune_mutex; |
OmarMuttawa | 11:aae7c9c290e2 | 149 | int hashRate; |
OmarMuttawa | 11:aae7c9c290e2 | 150 | |
OmarMuttawa | 11:aae7c9c290e2 | 151 | volatile float velocityError = 0; |
OmarMuttawa | 11:aae7c9c290e2 | 152 | volatile float prevVelocityError=0; |
OmarMuttawa | 11:aae7c9c290e2 | 153 | volatile float differentialVelocityError = 0; |
OmarMuttawa | 11:aae7c9c290e2 | 154 | volatile float integralVelocityError = 0; |
OmarMuttawa | 11:aae7c9c290e2 | 155 | |
OmarMuttawa | 11:aae7c9c290e2 | 156 | volatile float rotationError = 0; |
OmarMuttawa | 11:aae7c9c290e2 | 157 | volatile float prevRotationError = 0; |
OmarMuttawa | 11:aae7c9c290e2 | 158 | volatile float differentialRotationError = 0; |
OmarMuttawa | 11:aae7c9c290e2 | 159 | volatile float integralRotationError = 0; |
OmarMuttawa | 11:aae7c9c290e2 | 160 | |
OmarMuttawa | 11:aae7c9c290e2 | 161 | float max_t = 0; |
OmarMuttawa | 11:aae7c9c290e2 | 162 | |
OmarMuttawa | 11:aae7c9c290e2 | 163 | int target_position,position_error; |
OmarMuttawa | 11:aae7c9c290e2 | 164 | |
OmarMuttawa | 11:aae7c9c290e2 | 165 | //Global variable for velocity |
OmarMuttawa | 11:aae7c9c290e2 | 166 | volatile float velocity; |
OmarMuttawa | 11:aae7c9c290e2 | 167 | |
estott | 0:de4320f74764 | 168 | //Set a given drive state |
estott | 0:de4320f74764 | 169 | void motorOut(int8_t driveState){ |
estott | 0:de4320f74764 | 170 | |
estott | 2:4e88faab6988 | 171 | //Lookup the output byte from the drive state. |
estott | 2:4e88faab6988 | 172 | int8_t driveOut = driveTable[driveState & 0x07]; |
estott | 2:4e88faab6988 | 173 | |
estott | 2:4e88faab6988 | 174 | //Turn off first |
estott | 2:4e88faab6988 | 175 | if (~driveOut & 0x01) L1L = 0; |
estott | 2:4e88faab6988 | 176 | if (~driveOut & 0x02) L1H = 1; |
estott | 2:4e88faab6988 | 177 | if (~driveOut & 0x04) L2L = 0; |
estott | 2:4e88faab6988 | 178 | if (~driveOut & 0x08) L2H = 1; |
estott | 2:4e88faab6988 | 179 | if (~driveOut & 0x10) L3L = 0; |
estott | 2:4e88faab6988 | 180 | if (~driveOut & 0x20) L3H = 1; |
estott | 2:4e88faab6988 | 181 | |
estott | 2:4e88faab6988 | 182 | //Then turn on |
estott | 2:4e88faab6988 | 183 | if (driveOut & 0x01) L1L = 1; |
estott | 2:4e88faab6988 | 184 | if (driveOut & 0x02) L1H = 0; |
estott | 2:4e88faab6988 | 185 | if (driveOut & 0x04) L2L = 1; |
estott | 2:4e88faab6988 | 186 | if (driveOut & 0x08) L2H = 0; |
estott | 2:4e88faab6988 | 187 | if (driveOut & 0x10) L3L = 1; |
estott | 2:4e88faab6988 | 188 | if (driveOut & 0x20) L3H = 0; |
estott | 0:de4320f74764 | 189 | } |
estott | 0:de4320f74764 | 190 | |
OmarMuttawa | 11:aae7c9c290e2 | 191 | //Convert photointerrupter inputs to a rotor state |
estott | 0:de4320f74764 | 192 | inline int8_t readRotorState(){ |
estott | 2:4e88faab6988 | 193 | return stateMap[I1 + 2*I2 + 4*I3]; |
OmarMuttawa | 11:aae7c9c290e2 | 194 | } |
estott | 0:de4320f74764 | 195 | |
OmarMuttawa | 11:aae7c9c290e2 | 196 | void Rotorcalib(){ |
OmarMuttawa | 11:aae7c9c290e2 | 197 | intState = readRotorState(); |
OmarMuttawa | 11:aae7c9c290e2 | 198 | if (intState != intStateOld) { |
OmarMuttawa | 11:aae7c9c290e2 | 199 | //lead_mutex.lock(); |
OmarMuttawa | 11:aae7c9c290e2 | 200 | motorOut((intState-orState+lead+6)%6); //+6 to make sure the remainder is positive |
OmarMuttawa | 11:aae7c9c290e2 | 201 | //lead_mutex.unlock(); |
OmarMuttawa | 11:aae7c9c290e2 | 202 | //pc.printf("%d\n\r",intState); |
OmarMuttawa | 11:aae7c9c290e2 | 203 | if (intState-intStateOld==-5){ |
OmarMuttawa | 11:aae7c9c290e2 | 204 | //Goes positive direction |
OmarMuttawa | 11:aae7c9c290e2 | 205 | motor_position++; |
OmarMuttawa | 11:aae7c9c290e2 | 206 | }else if (intState-intStateOld==5) { |
OmarMuttawa | 11:aae7c9c290e2 | 207 | //Goes negative direction |
OmarMuttawa | 11:aae7c9c290e2 | 208 | motor_position--; |
OmarMuttawa | 11:aae7c9c290e2 | 209 | }else{ |
OmarMuttawa | 11:aae7c9c290e2 | 210 | motor_position = motor_position + (intState-intStateOld); //every |
OmarMuttawa | 11:aae7c9c290e2 | 211 | } |
OmarMuttawa | 11:aae7c9c290e2 | 212 | //Update state |
OmarMuttawa | 11:aae7c9c290e2 | 213 | intStateOld = intState; |
OmarMuttawa | 11:aae7c9c290e2 | 214 | } |
OmarMuttawa | 11:aae7c9c290e2 | 215 | } |
OmarMuttawa | 11:aae7c9c290e2 | 216 | |
estott | 0:de4320f74764 | 217 | //Basic synchronisation routine |
estott | 2:4e88faab6988 | 218 | int8_t motorHome() { |
OmarMuttawa | 11:aae7c9c290e2 | 219 | |
estott | 0:de4320f74764 | 220 | //Put the motor in drive state 0 and wait for it to stabilise |
estott | 0:de4320f74764 | 221 | motorOut(0); |
estott | 3:569b35e2a602 | 222 | wait(2.0); |
OmarMuttawa | 11:aae7c9c290e2 | 223 | motorOut(1.0); |
estott | 0:de4320f74764 | 224 | |
estott | 0:de4320f74764 | 225 | //Get the rotor state |
estott | 2:4e88faab6988 | 226 | return readRotorState(); |
estott | 0:de4320f74764 | 227 | } |
OmarMuttawa | 11:aae7c9c290e2 | 228 | |
OmarMuttawa | 11:aae7c9c290e2 | 229 | //Print message from Mail_box |
OmarMuttawa | 11:aae7c9c290e2 | 230 | void printMsgFn (void) { |
OmarMuttawa | 11:aae7c9c290e2 | 231 | while(1){ |
OmarMuttawa | 11:aae7c9c290e2 | 232 | |
OmarMuttawa | 11:aae7c9c290e2 | 233 | //Recieve a message and print it |
OmarMuttawa | 11:aae7c9c290e2 | 234 | osEvent evt = mail_box.get(); |
OmarMuttawa | 11:aae7c9c290e2 | 235 | |
OmarMuttawa | 11:aae7c9c290e2 | 236 | if (evt.status == osEventMail) { |
OmarMuttawa | 11:aae7c9c290e2 | 237 | mail_t *mail = (mail_t*)evt.value.p; |
OmarMuttawa | 11:aae7c9c290e2 | 238 | if(mail->type == 1){ //int |
OmarMuttawa | 11:aae7c9c290e2 | 239 | printf("%llu\n\r", mail->uint64_message); |
OmarMuttawa | 11:aae7c9c290e2 | 240 | } |
OmarMuttawa | 11:aae7c9c290e2 | 241 | else if(mail->type == 2){ |
OmarMuttawa | 11:aae7c9c290e2 | 242 | //string! |
OmarMuttawa | 11:aae7c9c290e2 | 243 | printf("%s", mail->string_message); |
OmarMuttawa | 11:aae7c9c290e2 | 244 | }else if (mail->type==3){ |
OmarMuttawa | 11:aae7c9c290e2 | 245 | printf("%f\n\r", mail->float_message); |
OmarMuttawa | 11:aae7c9c290e2 | 246 | } |
OmarMuttawa | 11:aae7c9c290e2 | 247 | mail_box.free(mail); |
OmarMuttawa | 11:aae7c9c290e2 | 248 | } |
OmarMuttawa | 11:aae7c9c290e2 | 249 | } |
OmarMuttawa | 11:aae7c9c290e2 | 250 | } |
OmarMuttawa | 11:aae7c9c290e2 | 251 | |
OmarMuttawa | 11:aae7c9c290e2 | 252 | //Create a int tupe Mail_box object |
OmarMuttawa | 11:aae7c9c290e2 | 253 | void put_msg_int (uint64_t uint64_message) { |
OmarMuttawa | 11:aae7c9c290e2 | 254 | mail_t *mail = mail_box.alloc(); |
OmarMuttawa | 11:aae7c9c290e2 | 255 | mail->type=1; |
OmarMuttawa | 11:aae7c9c290e2 | 256 | mail->uint64_message = uint64_message; |
OmarMuttawa | 11:aae7c9c290e2 | 257 | mail_box.put(mail); |
OmarMuttawa | 11:aae7c9c290e2 | 258 | } |
OmarMuttawa | 11:aae7c9c290e2 | 259 | |
OmarMuttawa | 11:aae7c9c290e2 | 260 | //Create a string tupe Mail_box object |
OmarMuttawa | 11:aae7c9c290e2 | 261 | void put_msg_string (char* message) { |
OmarMuttawa | 11:aae7c9c290e2 | 262 | mail_t *mail = mail_box.alloc(); |
OmarMuttawa | 11:aae7c9c290e2 | 263 | mail->type=2; |
OmarMuttawa | 11:aae7c9c290e2 | 264 | mail->string_message = message; |
OmarMuttawa | 11:aae7c9c290e2 | 265 | mail_box.put(mail); |
OmarMuttawa | 11:aae7c9c290e2 | 266 | } |
OmarMuttawa | 11:aae7c9c290e2 | 267 | |
OmarMuttawa | 11:aae7c9c290e2 | 268 | void put_msg_float (float message) { |
OmarMuttawa | 11:aae7c9c290e2 | 269 | mail_t *mail = mail_box.alloc(); |
OmarMuttawa | 11:aae7c9c290e2 | 270 | mail->type=3; |
OmarMuttawa | 11:aae7c9c290e2 | 271 | mail->float_message = message; |
OmarMuttawa | 11:aae7c9c290e2 | 272 | mail_box.put(mail); |
OmarMuttawa | 11:aae7c9c290e2 | 273 | } |
OmarMuttawa | 11:aae7c9c290e2 | 274 | |
OmarMuttawa | 11:aae7c9c290e2 | 275 | //Serial read |
OmarMuttawa | 11:aae7c9c290e2 | 276 | void serialISR(){ |
OmarMuttawa | 11:aae7c9c290e2 | 277 | //testPin.write(1); |
OmarMuttawa | 11:aae7c9c290e2 | 278 | uint8_t newChar = pc.getc(); |
OmarMuttawa | 11:aae7c9c290e2 | 279 | inCharQ.put((void*)newChar); |
OmarMuttawa | 11:aae7c9c290e2 | 280 | //testPin.write(0); |
OmarMuttawa | 11:aae7c9c290e2 | 281 | } |
OmarMuttawa | 11:aae7c9c290e2 | 282 | |
OmarMuttawa | 11:aae7c9c290e2 | 283 | //The function the ticker calls |
OmarMuttawa | 11:aae7c9c290e2 | 284 | void motorCtrlTick(){ |
OmarMuttawa | 11:aae7c9c290e2 | 285 | motorCtrlT.signal_set(0x1); |
OmarMuttawa | 11:aae7c9c290e2 | 286 | } |
OmarMuttawa | 11:aae7c9c290e2 | 287 | |
OmarMuttawa | 11:aae7c9c290e2 | 288 | void printNotes(void){ |
OmarMuttawa | 11:aae7c9c290e2 | 289 | put_msg_string(notes); |
OmarMuttawa | 11:aae7c9c290e2 | 290 | for(int i = 0; i < number_of_notes; i++){ |
OmarMuttawa | 11:aae7c9c290e2 | 291 | put_msg_float((float)durations[i]); |
OmarMuttawa | 11:aae7c9c290e2 | 292 | } |
OmarMuttawa | 11:aae7c9c290e2 | 293 | for(int i = 0; i < number_of_notes; i++){ |
OmarMuttawa | 11:aae7c9c290e2 | 294 | put_msg_float((float)periods[i]); |
OmarMuttawa | 11:aae7c9c290e2 | 295 | } |
OmarMuttawa | 11:aae7c9c290e2 | 296 | } |
OmarMuttawa | 11:aae7c9c290e2 | 297 | |
OmarMuttawa | 11:aae7c9c290e2 | 298 | void processTuneString(void){ |
OmarMuttawa | 11:aae7c9c290e2 | 299 | playTune_mutex.lock(); |
OmarMuttawa | 11:aae7c9c290e2 | 300 | playTune = false; |
OmarMuttawa | 11:aae7c9c290e2 | 301 | playTune_mutex.unlock(); |
OmarMuttawa | 11:aae7c9c290e2 | 302 | strcpy(tune_c_str, tune.c_str()); //turn tune (string) into c style string |
OmarMuttawa | 11:aae7c9c290e2 | 303 | number_of_notes = 0; |
OmarMuttawa | 11:aae7c9c290e2 | 304 | //reset the arrays |
OmarMuttawa | 11:aae7c9c290e2 | 305 | for(int i =0; i<10; i++){ |
OmarMuttawa | 11:aae7c9c290e2 | 306 | notes[i] = '\0'; |
OmarMuttawa | 11:aae7c9c290e2 | 307 | durations[i] = -1; |
OmarMuttawa | 11:aae7c9c290e2 | 308 | } |
OmarMuttawa | 11:aae7c9c290e2 | 309 | |
OmarMuttawa | 11:aae7c9c290e2 | 310 | char current_char; |
OmarMuttawa | 11:aae7c9c290e2 | 311 | char prev_char; |
OmarMuttawa | 11:aae7c9c290e2 | 312 | int tune_string_length = strlen(tune_c_str); |
OmarMuttawa | 11:aae7c9c290e2 | 313 | int a; |
OmarMuttawa | 11:aae7c9c290e2 | 314 | noteIndex = 0; |
OmarMuttawa | 11:aae7c9c290e2 | 315 | |
OmarMuttawa | 11:aae7c9c290e2 | 316 | for(a = 0; a < tune_string_length; a++){ |
OmarMuttawa | 11:aae7c9c290e2 | 317 | current_char = tune_c_str[a]; |
OmarMuttawa | 11:aae7c9c290e2 | 318 | if((current_char >= 'A') &&(current_char <= 'G')){ |
OmarMuttawa | 11:aae7c9c290e2 | 319 | notes[number_of_notes] = current_char; |
OmarMuttawa | 11:aae7c9c290e2 | 320 | number_of_notes++; |
OmarMuttawa | 11:aae7c9c290e2 | 321 | } |
OmarMuttawa | 11:aae7c9c290e2 | 322 | else if((current_char >= '1') && (current_char <= '9')) |
OmarMuttawa | 11:aae7c9c290e2 | 323 | durations[number_of_notes - 1] = (int)current_char - 48; |
OmarMuttawa | 11:aae7c9c290e2 | 324 | |
OmarMuttawa | 11:aae7c9c290e2 | 325 | else if(current_char == '#'){ |
OmarMuttawa | 11:aae7c9c290e2 | 326 | prev_char = tune_c_str[a-1]; |
OmarMuttawa | 11:aae7c9c290e2 | 327 | switch(prev_char){ |
OmarMuttawa | 11:aae7c9c290e2 | 328 | case 'A': |
OmarMuttawa | 11:aae7c9c290e2 | 329 | notes[number_of_notes - 1] = 'b'; |
OmarMuttawa | 11:aae7c9c290e2 | 330 | break; |
OmarMuttawa | 11:aae7c9c290e2 | 331 | case 'C': |
OmarMuttawa | 11:aae7c9c290e2 | 332 | notes[number_of_notes - 1] = 'd'; |
OmarMuttawa | 11:aae7c9c290e2 | 333 | break; |
OmarMuttawa | 11:aae7c9c290e2 | 334 | case 'D': |
OmarMuttawa | 11:aae7c9c290e2 | 335 | notes[number_of_notes - 1] = 'e'; |
OmarMuttawa | 11:aae7c9c290e2 | 336 | break; |
OmarMuttawa | 11:aae7c9c290e2 | 337 | case 'F': |
OmarMuttawa | 11:aae7c9c290e2 | 338 | notes[number_of_notes - 1] = 'g'; |
OmarMuttawa | 11:aae7c9c290e2 | 339 | break; |
OmarMuttawa | 11:aae7c9c290e2 | 340 | case 'G': |
OmarMuttawa | 11:aae7c9c290e2 | 341 | notes[number_of_notes - 1] = 'a'; |
OmarMuttawa | 11:aae7c9c290e2 | 342 | break; |
OmarMuttawa | 11:aae7c9c290e2 | 343 | } |
OmarMuttawa | 11:aae7c9c290e2 | 344 | } //end of # |
OmarMuttawa | 11:aae7c9c290e2 | 345 | else if(current_char == '^'){ |
OmarMuttawa | 11:aae7c9c290e2 | 346 | prev_char = tune_c_str[a-1]; |
OmarMuttawa | 11:aae7c9c290e2 | 347 | switch(prev_char){ |
OmarMuttawa | 11:aae7c9c290e2 | 348 | case 'B': |
OmarMuttawa | 11:aae7c9c290e2 | 349 | notes[number_of_notes - 1] = 'b'; |
OmarMuttawa | 11:aae7c9c290e2 | 350 | break; |
OmarMuttawa | 11:aae7c9c290e2 | 351 | case 'D': |
OmarMuttawa | 11:aae7c9c290e2 | 352 | notes[number_of_notes - 1] = 'd'; |
OmarMuttawa | 11:aae7c9c290e2 | 353 | break; |
OmarMuttawa | 11:aae7c9c290e2 | 354 | case 'E': |
OmarMuttawa | 11:aae7c9c290e2 | 355 | notes[number_of_notes - 1] = 'e'; |
OmarMuttawa | 11:aae7c9c290e2 | 356 | break; |
OmarMuttawa | 11:aae7c9c290e2 | 357 | case 'G': |
OmarMuttawa | 11:aae7c9c290e2 | 358 | notes[number_of_notes - 1] = 'g'; |
OmarMuttawa | 11:aae7c9c290e2 | 359 | break; |
OmarMuttawa | 11:aae7c9c290e2 | 360 | case 'A': |
OmarMuttawa | 11:aae7c9c290e2 | 361 | notes[number_of_notes - 1] = 'a'; |
OmarMuttawa | 11:aae7c9c290e2 | 362 | break; |
OmarMuttawa | 11:aae7c9c290e2 | 363 | }//end of switch |
OmarMuttawa | 11:aae7c9c290e2 | 364 | } //end of ^ |
OmarMuttawa | 11:aae7c9c290e2 | 365 | |
OmarMuttawa | 11:aae7c9c290e2 | 366 | }//end of the for loop |
OmarMuttawa | 11:aae7c9c290e2 | 367 | for(a = 0; a < number_of_notes; a++){ |
OmarMuttawa | 11:aae7c9c290e2 | 368 | char current_note = notes[a]; |
OmarMuttawa | 11:aae7c9c290e2 | 369 | switch(current_note){ |
OmarMuttawa | 11:aae7c9c290e2 | 370 | case 'A': periods[a] = 1.0/440.0; |
OmarMuttawa | 11:aae7c9c290e2 | 371 | break; |
OmarMuttawa | 11:aae7c9c290e2 | 372 | case 'b': periods[a] = 1.0/0466.16; |
OmarMuttawa | 11:aae7c9c290e2 | 373 | break; |
OmarMuttawa | 11:aae7c9c290e2 | 374 | case 'B': periods[a] = 1.0/493.88; |
OmarMuttawa | 11:aae7c9c290e2 | 375 | break; |
OmarMuttawa | 11:aae7c9c290e2 | 376 | case 'C': periods[a] = 1.0/523.25; |
OmarMuttawa | 11:aae7c9c290e2 | 377 | break; |
OmarMuttawa | 11:aae7c9c290e2 | 378 | case 'd': periods[a] = 1.0/554.37; |
OmarMuttawa | 11:aae7c9c290e2 | 379 | break; |
OmarMuttawa | 11:aae7c9c290e2 | 380 | case 'D': periods[a] = 1.0/587.83; |
OmarMuttawa | 11:aae7c9c290e2 | 381 | break; |
OmarMuttawa | 11:aae7c9c290e2 | 382 | case 'e': periods[a] = 1.0/622.25; |
OmarMuttawa | 11:aae7c9c290e2 | 383 | break; |
OmarMuttawa | 11:aae7c9c290e2 | 384 | case 'E': periods[a] = 1.0/659.25; |
OmarMuttawa | 11:aae7c9c290e2 | 385 | break; |
OmarMuttawa | 11:aae7c9c290e2 | 386 | case 'F': periods[a] = 1.0/698.46; |
OmarMuttawa | 11:aae7c9c290e2 | 387 | break; |
OmarMuttawa | 11:aae7c9c290e2 | 388 | case 'g': periods[a] = 1.0/739.99; |
OmarMuttawa | 11:aae7c9c290e2 | 389 | break; |
OmarMuttawa | 11:aae7c9c290e2 | 390 | case 'G': periods[a] = 1.0/783.99; |
OmarMuttawa | 11:aae7c9c290e2 | 391 | break; |
OmarMuttawa | 11:aae7c9c290e2 | 392 | case 'a': periods[a] = 1.0/830.61; |
OmarMuttawa | 11:aae7c9c290e2 | 393 | break; |
OmarMuttawa | 11:aae7c9c290e2 | 394 | default: periods[a] = 1.0/440.0; |
OmarMuttawa | 11:aae7c9c290e2 | 395 | break; |
OmarMuttawa | 11:aae7c9c290e2 | 396 | }//end of switch |
OmarMuttawa | 11:aae7c9c290e2 | 397 | }//end of for |
OmarMuttawa | 11:aae7c9c290e2 | 398 | playTune_mutex.lock(); |
OmarMuttawa | 11:aae7c9c290e2 | 399 | playTune = true; |
OmarMuttawa | 11:aae7c9c290e2 | 400 | playTune_mutex.unlock(); |
OmarMuttawa | 11:aae7c9c290e2 | 401 | return; |
OmarMuttawa | 11:aae7c9c290e2 | 402 | } |
OmarMuttawa | 11:aae7c9c290e2 | 403 | |
OmarMuttawa | 11:aae7c9c290e2 | 404 | |
OmarMuttawa | 11:aae7c9c290e2 | 405 | //Run in the motorCtrl thread |
OmarMuttawa | 11:aae7c9c290e2 | 406 | void motorCtrlFn(){ |
OmarMuttawa | 11:aae7c9c290e2 | 407 | char msg[50]; |
OmarMuttawa | 11:aae7c9c290e2 | 408 | Ticker motorCtrlTicker; |
OmarMuttawa | 11:aae7c9c290e2 | 409 | motorCtrlTicker.attach_us(&motorCtrlTick,100000); //run every 100ms |
OmarMuttawa | 11:aae7c9c290e2 | 410 | prevPosition=motor_position; |
OmarMuttawa | 11:aae7c9c290e2 | 411 | //Start velocity times |
OmarMuttawa | 11:aae7c9c290e2 | 412 | motorCtrlTimer.start(); |
OmarMuttawa | 11:aae7c9c290e2 | 413 | while(1){ |
OmarMuttawa | 11:aae7c9c290e2 | 414 | //Wait for ticker message |
OmarMuttawa | 11:aae7c9c290e2 | 415 | |
OmarMuttawa | 11:aae7c9c290e2 | 416 | motorCtrlT.signal_wait(0x1); |
OmarMuttawa | 11:aae7c9c290e2 | 417 | testPin.write(1); |
OmarMuttawa | 11:aae7c9c290e2 | 418 | motorCtrlTimer.stop(); |
OmarMuttawa | 11:aae7c9c290e2 | 419 | timeSinceLastRun = motorCtrlTimer.read(); |
OmarMuttawa | 11:aae7c9c290e2 | 420 | motorCtrlTimer.reset(); |
OmarMuttawa | 11:aae7c9c290e2 | 421 | motorCtrlTimer.start(); |
OmarMuttawa | 11:aae7c9c290e2 | 422 | //play tune |
OmarMuttawa | 11:aae7c9c290e2 | 423 | |
OmarMuttawa | 11:aae7c9c290e2 | 424 | playTune_mutex.lock(); |
OmarMuttawa | 11:aae7c9c290e2 | 425 | playTuneLocal = playTune; |
OmarMuttawa | 11:aae7c9c290e2 | 426 | playTune_mutex.unlock(); |
OmarMuttawa | 11:aae7c9c290e2 | 427 | |
OmarMuttawa | 11:aae7c9c290e2 | 428 | if(playTuneLocal == true){ |
OmarMuttawa | 11:aae7c9c290e2 | 429 | elapsedTime = elapsedTime + timeSinceLastRun; //increment the |
OmarMuttawa | 11:aae7c9c290e2 | 430 | float currentDuration = durations[noteIndex]; |
OmarMuttawa | 11:aae7c9c290e2 | 431 | if(elapsedTime >= currentDuration){ |
OmarMuttawa | 11:aae7c9c290e2 | 432 | elapsedTime = 0; |
OmarMuttawa | 11:aae7c9c290e2 | 433 | noteIndex++; |
OmarMuttawa | 11:aae7c9c290e2 | 434 | if(noteIndex == number_of_notes) |
OmarMuttawa | 11:aae7c9c290e2 | 435 | noteIndex = 0; //reset if needed |
OmarMuttawa | 11:aae7c9c290e2 | 436 | PWM_pin.period(periods[noteIndex]); //set the |
OmarMuttawa | 11:aae7c9c290e2 | 437 | } |
OmarMuttawa | 11:aae7c9c290e2 | 438 | } |
OmarMuttawa | 11:aae7c9c290e2 | 439 | else //if playTune == false |
OmarMuttawa | 11:aae7c9c290e2 | 440 | PWM_pin.period(0.002f); |
OmarMuttawa | 11:aae7c9c290e2 | 441 | |
OmarMuttawa | 11:aae7c9c290e2 | 442 | |
OmarMuttawa | 11:aae7c9c290e2 | 443 | //Calculate velocity |
OmarMuttawa | 11:aae7c9c290e2 | 444 | core_util_critical_section_enter(); |
OmarMuttawa | 11:aae7c9c290e2 | 445 | current_position=motor_position; |
OmarMuttawa | 11:aae7c9c290e2 | 446 | core_util_critical_section_exit(); |
OmarMuttawa | 11:aae7c9c290e2 | 447 | |
OmarMuttawa | 11:aae7c9c290e2 | 448 | velocity = (float)((current_position - prevPosition)/(6.0*timeSinceLastRun)); //rps |
OmarMuttawa | 11:aae7c9c290e2 | 449 | prevPosition=current_position; |
OmarMuttawa | 11:aae7c9c290e2 | 450 | |
OmarMuttawa | 11:aae7c9c290e2 | 451 | if((max_vel == 0) && (target_position == 0)){ //no limits |
OmarMuttawa | 11:aae7c9c290e2 | 452 | lead = 2; |
OmarMuttawa | 11:aae7c9c290e2 | 453 | PWM_pin.write(1.0); |
OmarMuttawa | 11:aae7c9c290e2 | 454 | } |
OmarMuttawa | 11:aae7c9c290e2 | 455 | |
OmarMuttawa | 11:aae7c9c290e2 | 456 | //speed limit no rotation limit, run forever at defined speed |
OmarMuttawa | 11:aae7c9c290e2 | 457 | else if ((max_vel != 0) && (target_position == 0)){ |
OmarMuttawa | 11:aae7c9c290e2 | 458 | prevVelocityError = velocityError; |
OmarMuttawa | 11:aae7c9c290e2 | 459 | |
OmarMuttawa | 11:aae7c9c290e2 | 460 | velocityError = max_vel - abs(velocity); //Proportional |
OmarMuttawa | 11:aae7c9c290e2 | 461 | differentialVelocityError = (velocityError - prevVelocityError)/timeSinceLastRun; //Differential |
OmarMuttawa | 11:aae7c9c290e2 | 462 | integralVelocityError += velocityError*timeSinceLastRun; // Integral |
OmarMuttawa | 11:aae7c9c290e2 | 463 | |
OmarMuttawa | 11:aae7c9c290e2 | 464 | if(integralVelocityError > VELOCITY_INTEGRAL_LIMIT) //Limit the integral |
OmarMuttawa | 11:aae7c9c290e2 | 465 | integralVelocityError = VELOCITY_INTEGRAL_LIMIT; |
OmarMuttawa | 11:aae7c9c290e2 | 466 | if(integralVelocityError < -1*VELOCITY_INTEGRAL_LIMIT) |
OmarMuttawa | 11:aae7c9c290e2 | 467 | integralVelocityError =(-1*VELOCITY_INTEGRAL_LIMIT); |
OmarMuttawa | 11:aae7c9c290e2 | 468 | |
OmarMuttawa | 11:aae7c9c290e2 | 469 | Ts = K_PS*velocityError + K_IS*integralVelocityError + K_DS*differentialVelocityError; |
OmarMuttawa | 11:aae7c9c290e2 | 470 | |
OmarMuttawa | 11:aae7c9c290e2 | 471 | lead = (Ts<0) ? -2 : 2; |
OmarMuttawa | 11:aae7c9c290e2 | 472 | |
OmarMuttawa | 11:aae7c9c290e2 | 473 | Ts = abs(Ts); |
OmarMuttawa | 11:aae7c9c290e2 | 474 | if (Ts>1){ |
OmarMuttawa | 11:aae7c9c290e2 | 475 | Ts=1.0; |
OmarMuttawa | 11:aae7c9c290e2 | 476 | } |
OmarMuttawa | 11:aae7c9c290e2 | 477 | PWM_pin.write(Ts); |
OmarMuttawa | 11:aae7c9c290e2 | 478 | |
OmarMuttawa | 11:aae7c9c290e2 | 479 | } |
OmarMuttawa | 11:aae7c9c290e2 | 480 | //no speed limit, but a rotation limit |
OmarMuttawa | 11:aae7c9c290e2 | 481 | else if((max_vel == 0) && (target_position != 0)){ |
OmarMuttawa | 11:aae7c9c290e2 | 482 | prevRotationError = rotationError; |
OmarMuttawa | 11:aae7c9c290e2 | 483 | rotationError = target_position - current_position; //in ISR TICKS |
OmarMuttawa | 11:aae7c9c290e2 | 484 | differentialRotationError = (rotationError - prevRotationError)/timeSinceLastRun; //in ISR ticks /s |
OmarMuttawa | 11:aae7c9c290e2 | 485 | |
OmarMuttawa | 11:aae7c9c290e2 | 486 | |
OmarMuttawa | 11:aae7c9c290e2 | 487 | integralRotationError += rotationError*timeSinceLastRun; |
OmarMuttawa | 11:aae7c9c290e2 | 488 | if(integralRotationError > ROTATION_INTEGRAL_LIMIT) |
OmarMuttawa | 11:aae7c9c290e2 | 489 | integralRotationError = ROTATION_INTEGRAL_LIMIT; |
OmarMuttawa | 11:aae7c9c290e2 | 490 | if(integralRotationError < -1*ROTATION_INTEGRAL_LIMIT) |
OmarMuttawa | 11:aae7c9c290e2 | 491 | integralRotationError = -1*ROTATION_INTEGRAL_LIMIT; |
OmarMuttawa | 11:aae7c9c290e2 | 492 | //put_msg_float(integralRotationError); |
OmarMuttawa | 11:aae7c9c290e2 | 493 | Tr = (K_PR*rotationError) + (K_IR*integralRotationError) + (K_DR*differentialRotationError); // Need to divide by time |
OmarMuttawa | 11:aae7c9c290e2 | 494 | |
OmarMuttawa | 11:aae7c9c290e2 | 495 | // In case the rotations were overshot change the direction back |
OmarMuttawa | 11:aae7c9c290e2 | 496 | |
OmarMuttawa | 11:aae7c9c290e2 | 497 | lead = (Tr > 0) ? 2 : -2; //change direction if needed |
OmarMuttawa | 11:aae7c9c290e2 | 498 | Tr = abs(Tr); |
OmarMuttawa | 11:aae7c9c290e2 | 499 | Tr = (Tr > 1) ? 1 : Tr; //clamp at 1.0 |
OmarMuttawa | 11:aae7c9c290e2 | 500 | |
OmarMuttawa | 11:aae7c9c290e2 | 501 | if(abs(rotationError) == 0){ |
OmarMuttawa | 11:aae7c9c290e2 | 502 | PWM_pin.write(0.0); |
OmarMuttawa | 11:aae7c9c290e2 | 503 | } |
OmarMuttawa | 11:aae7c9c290e2 | 504 | else |
OmarMuttawa | 11:aae7c9c290e2 | 505 | PWM_pin.write(Tr); |
OmarMuttawa | 11:aae7c9c290e2 | 506 | } |
OmarMuttawa | 11:aae7c9c290e2 | 507 | //speed limit and rotation limit |
OmarMuttawa | 11:aae7c9c290e2 | 508 | else{ //((max_vel != 0) && (target_position != 0)){{ |
OmarMuttawa | 11:aae7c9c290e2 | 509 | prevRotationError = rotationError; |
OmarMuttawa | 11:aae7c9c290e2 | 510 | rotationError = target_position - current_position; //in ISR TICKS |
OmarMuttawa | 11:aae7c9c290e2 | 511 | float differentialRotationError = (rotationError - prevRotationError)/timeSinceLastRun; |
OmarMuttawa | 11:aae7c9c290e2 | 512 | integralRotationError += (rotationError*timeSinceLastRun); |
OmarMuttawa | 11:aae7c9c290e2 | 513 | if(integralRotationError > ROTATION_INTEGRAL_LIMIT) |
OmarMuttawa | 11:aae7c9c290e2 | 514 | integralRotationError = ROTATION_INTEGRAL_LIMIT; |
OmarMuttawa | 11:aae7c9c290e2 | 515 | if(integralRotationError < -1*ROTATION_INTEGRAL_LIMIT) |
OmarMuttawa | 11:aae7c9c290e2 | 516 | integralRotationError = -1*ROTATION_INTEGRAL_LIMIT; |
OmarMuttawa | 11:aae7c9c290e2 | 517 | //put_msg_float(integralRotationError); |
OmarMuttawa | 11:aae7c9c290e2 | 518 | Tr = (K_PR*rotationError) + (K_IR*integralRotationError) + (K_DR*differentialRotationError); // Need to divide by time |
OmarMuttawa | 11:aae7c9c290e2 | 519 | |
OmarMuttawa | 11:aae7c9c290e2 | 520 | prevVelocityError = velocityError; |
OmarMuttawa | 11:aae7c9c290e2 | 521 | velocityError = max_vel - abs(velocity); |
OmarMuttawa | 11:aae7c9c290e2 | 522 | float differentialVelocityError = (velocityError - prevVelocityError)/timeSinceLastRun; |
OmarMuttawa | 11:aae7c9c290e2 | 523 | |
OmarMuttawa | 11:aae7c9c290e2 | 524 | // Integral error |
OmarMuttawa | 11:aae7c9c290e2 | 525 | integralVelocityError += velocityError*timeSinceLastRun; |
OmarMuttawa | 11:aae7c9c290e2 | 526 | if(integralVelocityError > VELOCITY_INTEGRAL_LIMIT) |
OmarMuttawa | 11:aae7c9c290e2 | 527 | integralVelocityError = VELOCITY_INTEGRAL_LIMIT; |
OmarMuttawa | 11:aae7c9c290e2 | 528 | if(integralVelocityError < -1*VELOCITY_INTEGRAL_LIMIT) |
OmarMuttawa | 11:aae7c9c290e2 | 529 | integralVelocityError =(-1*VELOCITY_INTEGRAL_LIMIT); |
OmarMuttawa | 11:aae7c9c290e2 | 530 | |
OmarMuttawa | 11:aae7c9c290e2 | 531 | Ts = K_PS*velocityError + K_IS*integralVelocityError + K_DS*differentialVelocityError; |
OmarMuttawa | 11:aae7c9c290e2 | 532 | if(differentialRotationError < 0) |
OmarMuttawa | 11:aae7c9c290e2 | 533 | Ts = -Ts; |
OmarMuttawa | 11:aae7c9c290e2 | 534 | |
OmarMuttawa | 11:aae7c9c290e2 | 535 | |
OmarMuttawa | 11:aae7c9c290e2 | 536 | if(abs(velocity) > max_vel){ |
OmarMuttawa | 11:aae7c9c290e2 | 537 | newTorque = min(abs(Tr), abs(Ts)); |
OmarMuttawa | 11:aae7c9c290e2 | 538 | } |
OmarMuttawa | 11:aae7c9c290e2 | 539 | else |
OmarMuttawa | 11:aae7c9c290e2 | 540 | newTorque = max(abs(Tr), abs(Ts)); |
OmarMuttawa | 11:aae7c9c290e2 | 541 | |
OmarMuttawa | 11:aae7c9c290e2 | 542 | if(abs(rotationError)<10) //if we're close, take Tr |
OmarMuttawa | 11:aae7c9c290e2 | 543 | newTorque = abs(Tr); |
OmarMuttawa | 11:aae7c9c290e2 | 544 | |
OmarMuttawa | 11:aae7c9c290e2 | 545 | lead = (rotationError >= 0) ? 2 : -2; |
OmarMuttawa | 11:aae7c9c290e2 | 546 | |
OmarMuttawa | 11:aae7c9c290e2 | 547 | |
OmarMuttawa | 11:aae7c9c290e2 | 548 | |
OmarMuttawa | 11:aae7c9c290e2 | 549 | newTorque = abs(newTorque); |
OmarMuttawa | 11:aae7c9c290e2 | 550 | |
OmarMuttawa | 11:aae7c9c290e2 | 551 | if(newTorque > 1.0) |
OmarMuttawa | 11:aae7c9c290e2 | 552 | newTorque = 1.0; |
OmarMuttawa | 11:aae7c9c290e2 | 553 | |
OmarMuttawa | 11:aae7c9c290e2 | 554 | if(abs(rotationError) == 0) |
OmarMuttawa | 11:aae7c9c290e2 | 555 | PWM_pin.write(0.0); |
OmarMuttawa | 11:aae7c9c290e2 | 556 | else |
OmarMuttawa | 11:aae7c9c290e2 | 557 | PWM_pin.write(newTorque); |
OmarMuttawa | 11:aae7c9c290e2 | 558 | |
OmarMuttawa | 11:aae7c9c290e2 | 559 | } |
OmarMuttawa | 11:aae7c9c290e2 | 560 | testPin.write(0); |
OmarMuttawa | 11:aae7c9c290e2 | 561 | }//end of while 1 |
OmarMuttawa | 11:aae7c9c290e2 | 562 | }//end of fn |
OmarMuttawa | 11:aae7c9c290e2 | 563 | |
OmarMuttawa | 11:aae7c9c290e2 | 564 | |
OmarMuttawa | 11:aae7c9c290e2 | 565 | //Receiving command serially and decoding it |
OmarMuttawa | 11:aae7c9c290e2 | 566 | void readMsgFn(){ |
OmarMuttawa | 11:aae7c9c290e2 | 567 | string message_string; |
OmarMuttawa | 11:aae7c9c290e2 | 568 | pc.attach(&serialISR); |
OmarMuttawa | 11:aae7c9c290e2 | 569 | char message[18]; |
OmarMuttawa | 11:aae7c9c290e2 | 570 | int i=0; |
OmarMuttawa | 11:aae7c9c290e2 | 571 | while(1){ |
OmarMuttawa | 11:aae7c9c290e2 | 572 | osEvent newEvent = inCharQ.get(); |
OmarMuttawa | 11:aae7c9c290e2 | 573 | //testPin.write(1); |
OmarMuttawa | 11:aae7c9c290e2 | 574 | uint8_t newChar = (uint8_t)newEvent.value.p; |
OmarMuttawa | 11:aae7c9c290e2 | 575 | if(i >= 17) |
OmarMuttawa | 11:aae7c9c290e2 | 576 | i=0; |
OmarMuttawa | 11:aae7c9c290e2 | 577 | else{ |
OmarMuttawa | 11:aae7c9c290e2 | 578 | message[i]=newChar; |
OmarMuttawa | 11:aae7c9c290e2 | 579 | i++; |
OmarMuttawa | 11:aae7c9c290e2 | 580 | } |
OmarMuttawa | 11:aae7c9c290e2 | 581 | if(newChar=='\r' || newChar == '\n'){ |
OmarMuttawa | 11:aae7c9c290e2 | 582 | if(message[0]=='K'){ |
OmarMuttawa | 11:aae7c9c290e2 | 583 | newKey_mutex.lock(); |
OmarMuttawa | 11:aae7c9c290e2 | 584 | put_msg_string("Entering New Key: "); |
OmarMuttawa | 11:aae7c9c290e2 | 585 | sscanf(message,"K%x",&newKey); |
OmarMuttawa | 11:aae7c9c290e2 | 586 | message[i]='\0'; |
OmarMuttawa | 11:aae7c9c290e2 | 587 | put_msg_int(newKey); |
OmarMuttawa | 11:aae7c9c290e2 | 588 | newKey_mutex.unlock(); |
OmarMuttawa | 11:aae7c9c290e2 | 589 | i=0; |
OmarMuttawa | 11:aae7c9c290e2 | 590 | } |
OmarMuttawa | 11:aae7c9c290e2 | 591 | else if(message[0]=='R'){ |
OmarMuttawa | 11:aae7c9c290e2 | 592 | newRotation_mutex.lock(); |
OmarMuttawa | 11:aae7c9c290e2 | 593 | integralRotationError = 0; //reset the sum! |
OmarMuttawa | 11:aae7c9c290e2 | 594 | integralVelocityError = 0; //reset the sum! |
OmarMuttawa | 11:aae7c9c290e2 | 595 | sscanf(message,"R%f",&rot_input); //rot_input in number of revs |
OmarMuttawa | 11:aae7c9c290e2 | 596 | if(rot_input == 0) |
OmarMuttawa | 11:aae7c9c290e2 | 597 | target_position = 0; |
OmarMuttawa | 11:aae7c9c290e2 | 598 | else |
OmarMuttawa | 11:aae7c9c290e2 | 599 | { |
OmarMuttawa | 11:aae7c9c290e2 | 600 | target_position = current_position + (rot_input*6); //in ISR TICKS |
OmarMuttawa | 11:aae7c9c290e2 | 601 | } |
OmarMuttawa | 11:aae7c9c290e2 | 602 | message[i]='\0'; |
OmarMuttawa | 11:aae7c9c290e2 | 603 | newRotation_mutex.unlock(); |
OmarMuttawa | 11:aae7c9c290e2 | 604 | i=0; |
OmarMuttawa | 11:aae7c9c290e2 | 605 | } |
OmarMuttawa | 11:aae7c9c290e2 | 606 | else if(message[0]=='V'){ |
OmarMuttawa | 11:aae7c9c290e2 | 607 | newVelocity_mutex.lock(); |
OmarMuttawa | 11:aae7c9c290e2 | 608 | sscanf(message,"V%f",&max_vel); |
OmarMuttawa | 11:aae7c9c290e2 | 609 | integralVelocityError = 0; //reset the sum! |
OmarMuttawa | 11:aae7c9c290e2 | 610 | integralRotationError = 0; //reset the sum! |
OmarMuttawa | 11:aae7c9c290e2 | 611 | newVelocity_mutex.unlock(); |
OmarMuttawa | 11:aae7c9c290e2 | 612 | //printf("%f",max_vel); |
OmarMuttawa | 11:aae7c9c290e2 | 613 | message[i]='\0'; |
OmarMuttawa | 11:aae7c9c290e2 | 614 | i=0; |
OmarMuttawa | 11:aae7c9c290e2 | 615 | } |
OmarMuttawa | 11:aae7c9c290e2 | 616 | else if(message[0] == 'T'){ |
OmarMuttawa | 11:aae7c9c290e2 | 617 | sscanf(message,"T%s",tune); |
OmarMuttawa | 11:aae7c9c290e2 | 618 | if(message[1] == '0'){ |
OmarMuttawa | 11:aae7c9c290e2 | 619 | playTune_mutex.lock(); |
OmarMuttawa | 11:aae7c9c290e2 | 620 | playTune = false; |
OmarMuttawa | 11:aae7c9c290e2 | 621 | playTune_mutex.unlock(); |
OmarMuttawa | 11:aae7c9c290e2 | 622 | } |
OmarMuttawa | 11:aae7c9c290e2 | 623 | else |
OmarMuttawa | 11:aae7c9c290e2 | 624 | processTuneString(); |
OmarMuttawa | 11:aae7c9c290e2 | 625 | message[i] = '\0'; |
OmarMuttawa | 11:aae7c9c290e2 | 626 | i=0; |
OmarMuttawa | 11:aae7c9c290e2 | 627 | } |
OmarMuttawa | 11:aae7c9c290e2 | 628 | } |
OmarMuttawa | 11:aae7c9c290e2 | 629 | //testPin.write(0); |
OmarMuttawa | 11:aae7c9c290e2 | 630 | } |
OmarMuttawa | 11:aae7c9c290e2 | 631 | } |
OmarMuttawa | 11:aae7c9c290e2 | 632 | |
OmarMuttawa | 11:aae7c9c290e2 | 633 | |
estott | 0:de4320f74764 | 634 | //Main |
estott | 0:de4320f74764 | 635 | int main() { |
OmarMuttawa | 11:aae7c9c290e2 | 636 | string msg="Hello World"; |
OmarMuttawa | 11:aae7c9c290e2 | 637 | SHA256 mySHA256 = SHA256(); |
OmarMuttawa | 11:aae7c9c290e2 | 638 | //put_msg("HELLO"); |
OmarMuttawa | 11:aae7c9c290e2 | 639 | uint8_t sequence[] = {0x45,0x6D,0x62,0x65,0x64,0x64,0x65,0x64, |
OmarMuttawa | 11:aae7c9c290e2 | 640 | 0x20,0x53,0x79,0x73,0x74,0x65,0x6D,0x73, |
OmarMuttawa | 11:aae7c9c290e2 | 641 | 0x20,0x61,0x72,0x65,0x20,0x66,0x75,0x6E, |
OmarMuttawa | 11:aae7c9c290e2 | 642 | 0x20,0x61,0x6E,0x64,0x20,0x64,0x6F,0x20, |
OmarMuttawa | 11:aae7c9c290e2 | 643 | 0x61,0x77,0x65,0x73,0x6F,0x6D,0x65,0x20, |
OmarMuttawa | 11:aae7c9c290e2 | 644 | 0x74,0x68,0x69,0x6E,0x67,0x73,0x21,0x20, |
OmarMuttawa | 11:aae7c9c290e2 | 645 | 0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00, |
OmarMuttawa | 11:aae7c9c290e2 | 646 | 0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00}; |
OmarMuttawa | 11:aae7c9c290e2 | 647 | uint64_t* key = (uint64_t*)((int)sequence + 48); |
OmarMuttawa | 11:aae7c9c290e2 | 648 | uint64_t* nonce = (uint64_t*)((int)sequence + 56); |
estott | 2:4e88faab6988 | 649 | |
OmarMuttawa | 11:aae7c9c290e2 | 650 | uint8_t hash[32]; |
OmarMuttawa | 11:aae7c9c290e2 | 651 | |
OmarMuttawa | 11:aae7c9c290e2 | 652 | orState = 0; //Rotot offset at motor state 0 |
OmarMuttawa | 11:aae7c9c290e2 | 653 | intState = 0; |
OmarMuttawa | 11:aae7c9c290e2 | 654 | intStateOld = 0; |
OmarMuttawa | 11:aae7c9c290e2 | 655 | |
OmarMuttawa | 11:aae7c9c290e2 | 656 | //Initialise PWM and Duty Cycle; |
OmarMuttawa | 11:aae7c9c290e2 | 657 | PWM_pin.period(0.002f); |
OmarMuttawa | 11:aae7c9c290e2 | 658 | PWM_pin.write(1.00f); |
OmarMuttawa | 11:aae7c9c290e2 | 659 | |
estott | 0:de4320f74764 | 660 | pc.printf("Hello\n\r"); |
estott | 0:de4320f74764 | 661 | |
estott | 0:de4320f74764 | 662 | //Run the motor synchronisation |
estott | 2:4e88faab6988 | 663 | orState = motorHome(); |
estott | 2:4e88faab6988 | 664 | pc.printf("Rotor origin: %x\n\r",orState); |
OmarMuttawa | 11:aae7c9c290e2 | 665 | |
estott | 2:4e88faab6988 | 666 | //orState is subtracted from future rotor state inputs to align rotor and motor states |
OmarMuttawa | 11:aae7c9c290e2 | 667 | I1.rise(&Rotorcalib); |
OmarMuttawa | 11:aae7c9c290e2 | 668 | I2.rise(&Rotorcalib); |
OmarMuttawa | 11:aae7c9c290e2 | 669 | I3.rise(&Rotorcalib); |
OmarMuttawa | 11:aae7c9c290e2 | 670 | I1.fall(&Rotorcalib); |
OmarMuttawa | 11:aae7c9c290e2 | 671 | I2.fall(&Rotorcalib); |
OmarMuttawa | 11:aae7c9c290e2 | 672 | I3.fall(&Rotorcalib); |
OmarMuttawa | 11:aae7c9c290e2 | 673 | |
OmarMuttawa | 11:aae7c9c290e2 | 674 | hashRate = 0; |
OmarMuttawa | 11:aae7c9c290e2 | 675 | t.start(); |
OmarMuttawa | 11:aae7c9c290e2 | 676 | |
OmarMuttawa | 11:aae7c9c290e2 | 677 | printMsgT.start(callback(printMsgFn)); //start print msg thread |
OmarMuttawa | 11:aae7c9c290e2 | 678 | readMsgT.start(callback(readMsgFn)); |
OmarMuttawa | 11:aae7c9c290e2 | 679 | motorCtrlT.start(callback(motorCtrlFn)); // start the motorCtrlT thread |
OmarMuttawa | 11:aae7c9c290e2 | 680 | |
estott | 0:de4320f74764 | 681 | |
OmarMuttawa | 11:aae7c9c290e2 | 682 | while (1) { |
OmarMuttawa | 11:aae7c9c290e2 | 683 | //testPin.write(!testPin.read()); |
OmarMuttawa | 11:aae7c9c290e2 | 684 | newKey_mutex.lock(); |
OmarMuttawa | 11:aae7c9c290e2 | 685 | *key = newKey; |
OmarMuttawa | 11:aae7c9c290e2 | 686 | newKey_mutex.unlock(); |
OmarMuttawa | 11:aae7c9c290e2 | 687 | |
OmarMuttawa | 11:aae7c9c290e2 | 688 | mySHA256.computeHash(hash,sequence,sizeof(sequence)); |
OmarMuttawa | 11:aae7c9c290e2 | 689 | hashRate++; |
OmarMuttawa | 11:aae7c9c290e2 | 690 | if(t.read() >= 1){ //if a second has passed |
OmarMuttawa | 11:aae7c9c290e2 | 691 | t.stop(); |
OmarMuttawa | 11:aae7c9c290e2 | 692 | t.reset(); |
OmarMuttawa | 11:aae7c9c290e2 | 693 | t.start(); |
OmarMuttawa | 11:aae7c9c290e2 | 694 | char msg[10]; |
OmarMuttawa | 11:aae7c9c290e2 | 695 | strcpy(msg, "VELOCITY: "); |
OmarMuttawa | 11:aae7c9c290e2 | 696 | put_msg_string(msg); |
OmarMuttawa | 11:aae7c9c290e2 | 697 | put_msg_float(velocity); |
OmarMuttawa | 11:aae7c9c290e2 | 698 | char msg2[10]; |
OmarMuttawa | 11:aae7c9c290e2 | 699 | strcpy(msg2, "HASH RATE: "); |
OmarMuttawa | 11:aae7c9c290e2 | 700 | put_msg_string(msg2); |
OmarMuttawa | 11:aae7c9c290e2 | 701 | put_msg_int(hashRate); //hashes per second |
OmarMuttawa | 11:aae7c9c290e2 | 702 | hashRate = 0; |
estott | 0:de4320f74764 | 703 | } |
OmarMuttawa | 11:aae7c9c290e2 | 704 | if((hash[0] == 0) && (hash[1] == 0)){ |
OmarMuttawa | 11:aae7c9c290e2 | 705 | char msg1[10]; |
OmarMuttawa | 11:aae7c9c290e2 | 706 | strcpy(msg1, "SUCCESS: "); |
OmarMuttawa | 11:aae7c9c290e2 | 707 | put_msg_string(msg1); |
OmarMuttawa | 11:aae7c9c290e2 | 708 | put_msg_int(*nonce); |
OmarMuttawa | 11:aae7c9c290e2 | 709 | } |
OmarMuttawa | 11:aae7c9c290e2 | 710 | *nonce+=1; |
OmarMuttawa | 11:aae7c9c290e2 | 711 | |
estott | 2:4e88faab6988 | 712 | } |
estott | 0:de4320f74764 | 713 | } |
estott | 0:de4320f74764 | 714 |