Omar Muttawa / Mbed OS SMARTASSES_FINAL_CW2

SMARTASSES 2019

Dependencies:   mbed Crypto

main.cpp@11:aae7c9c290e2, 2019-03-22 (annotated)

Committer:
OmarMuttawa
Date:
Fri Mar 22 22:27:48 2019 +0000
Revision:
11:aae7c9c290e2
Parent:
10:a4b5723b6c9d
Child:
12:b39f69ed20af
Final

Who changed what in which revision?

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