Omar Muttawa / Mbed OS SMARTASSES_FINAL_CW2

SMARTASSES 2019

Dependencies:   mbed Crypto

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?

UserRevisionLine numberNew 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