Skad00sh / Mbed OS ES_CW2_Dev

Callum and Adel's changes on 12/02/19

Dependencies:   Crypto

main.cpp@26:fb6151e5907d, 2019-03-16 (annotated)

Committer:
adehadd
Date:
Sat Mar 16 18:19:08 2019 +0000
Revision:
26:fb6151e5907d
Parent:
25:995865498aee
Child:
27:ce05fed3c1ea
proposed PID controls added.

Who changed what in which revision?

UserRevisionLine numberNew contents of line
adehadd 26:fb6151e5907d 1 //~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~INCLUDES~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
estott 0:de4320f74764 2 #include "mbed.h"
adehadd 26:fb6151e5907d 3 #include "Crypto.h" // Library used for Bitcoin mining.
adehadd 26:fb6151e5907d 4 #include "rtos.h" // Real time operating system library for threads etc.
adehadd 26:fb6151e5907d 5
adehadd 26:fb6151e5907d 6
adehadd 26:fb6151e5907d 7
adehadd 26:fb6151e5907d 8
adehadd 26:fb6151e5907d 9 //~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~DEFINITIONS~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
CallumAlder 14:4e312fb83330 10
adehadd 26:fb6151e5907d 11 //~~~~~~~~~~~~~~Photointerrupter pins~~~~~~~~~~~~~~
adehadd 26:fb6151e5907d 12 #define I1pin D2
adehadd 26:fb6151e5907d 13 #define I2pin D11
adehadd 26:fb6151e5907d 14 #define I3pin D12
estott 0:de4320f74764 15
adehadd 26:fb6151e5907d 16
adehadd 26:fb6151e5907d 17 ////~~~~~~~~~~Incremental encoder pins//~~~~~~~~~~~
adehadd 26:fb6151e5907d 18 #define CHA D7
adehadd 26:fb6151e5907d 19 #define CHB D8
adehadd 26:fb6151e5907d 20
estott 2:4e88faab6988 21
adehadd 26:fb6151e5907d 22 //~~Motor Drive output pins~/Mask in output byte~~~
adehadd 26:fb6151e5907d 23 #define L1Lpin D4 //0x01
adehadd 26:fb6151e5907d 24 #define L1Hpin D5 //0x02
adehadd 26:fb6151e5907d 25 #define L2Lpin D3 //0x04
adehadd 26:fb6151e5907d 26 #define L2Hpin D6 //0x08
adehadd 26:fb6151e5907d 27 #define L3Lpin D9 //0x10
adehadd 26:fb6151e5907d 28 #define L3Hpin D10 //0x20
adehadd 26:fb6151e5907d 29
adehadd 26:fb6151e5907d 30
adehadd 26:fb6151e5907d 31 //~~~~~~~~Maximum command length accepted~~~~~~~~~~~
adehadd 26:fb6151e5907d 32 #define MAXCMDLENGTH 18
adehadd 26:fb6151e5907d 33
estott 0:de4320f74764 34
adehadd 26:fb6151e5907d 35 //~~~~~~~~Maximum PWM allowed due to 50% restriction
adehadd 26:fb6151e5907d 36 #define MAXPWM 1000
adehadd 26:fb6151e5907d 37
adehadd 26:fb6151e5907d 38
adehadd 26:fb6151e5907d 39 //~~~~~~~Enumeration of message identifiers~~~~~~~~~
adehadd 26:fb6151e5907d 40 enum MsgCode {Msg_motorState, Msg_hashRate, Msg_nonceMatch, Msg_keyAdded, Msg_velocityOut, Msg_velocityIn, Msg_positionIn, Msg_positionOut, Msg_rotations, Msg_torque, Msg_error};
adehadd 26:fb6151e5907d 41
estott 10:a4b5723b6c9d 42
adehadd 26:fb6151e5907d 43 //~~~~~~~New data type to carry the messages~~~~~~~~
adehadd 26:fb6151e5907d 44 typedef struct {
adehadd 26:fb6151e5907d 45 MsgCode code;
adehadd 26:fb6151e5907d 46 uint32_t data;
adehadd 26:fb6151e5907d 47 } message_t;
estott 5:08f338b5e4d9 48
adehadd 26:fb6151e5907d 49
adehadd 26:fb6151e5907d 50
adehadd 26:fb6151e5907d 51 //~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~Global Variables~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
estott 0:de4320f74764 52
estott 0:de4320f74764 53 //Mapping from sequential drive states to motor phase outputs
estott 0:de4320f74764 54 /*
estott 0:de4320f74764 55 State L1 L2 L3
estott 0:de4320f74764 56 0 H - L
estott 0:de4320f74764 57 1 - H L
estott 0:de4320f74764 58 2 L H -
estott 0:de4320f74764 59 3 L - H
estott 0:de4320f74764 60 4 - L H
estott 0:de4320f74764 61 5 H L -
estott 0:de4320f74764 62 6 - - -
estott 0:de4320f74764 63 7 - - -
estott 0:de4320f74764 64 */
adehadd 26:fb6151e5907d 65
adehadd 26:fb6151e5907d 66
adehadd 26:fb6151e5907d 67 //~~~~~~~~~~~Drive state to output table~~~~~~~~~~~~
estott 0:de4320f74764 68 const int8_t driveTable[] = {0x12,0x18,0x09,0x21,0x24,0x06,0x00,0x00};
estott 2:4e88faab6988 69
adehadd 26:fb6151e5907d 70
adehadd 26:fb6151e5907d 71 //Mapping from interrupter inputs to sequential rotor states. 0x00 and 0x07 are not valid.
adehadd 26:fb6151e5907d 72 const int8_t stateMap[] = {0x07,0x05,0x03,0x04,0x01,0x00,0x02,0x07};
adehadd 26:fb6151e5907d 73 //Alternative if phase order of input or drive is reversed.
adehadd 26:fb6151e5907d 74 //const int8_t stateMap[] = {0x07,0x01,0x03,0x02,0x05,0x00,0x04,0x07};
adehadd 26:fb6151e5907d 75
estott 2:4e88faab6988 76
adehadd 26:fb6151e5907d 77 ////~~~~~~~~~Phase lead to make motor spin~~~~~~~~~
adehadd 26:fb6151e5907d 78 int8_t lead = 2; //2 for forwards, -2 for backwards
adehadd 26:fb6151e5907d 79
estott 0:de4320f74764 80
adehadd 26:fb6151e5907d 81 //~~~~~~~~~~~~~~~~~~Rotor states~~~~~~~~~~~~~~~~~~~
adehadd 26:fb6151e5907d 82 int8_t orState = 0; // Rotor offset at motor state 0
adehadd 26:fb6151e5907d 83 volatile int8_t intStateOld = 0; // Motor old state. Type is volatile since
adehadd 26:fb6151e5907d 84 // its value may change in ISR
adehadd 26:fb6151e5907d 85
adehadd 26:fb6151e5907d 86 //~~~~~~~~~~~~~~~~~~~Status LED~~~~~~~~~~~~~~~~~~~~
estott 0:de4320f74764 87 DigitalOut led1(LED1);
estott 0:de4320f74764 88
adehadd 26:fb6151e5907d 89
adehadd 26:fb6151e5907d 90 //~~~~~~~~~~~~~Photointerrupter inputs~~~~~~~~~~~~~
iachinweze1 12:41b3112021a3 91 InterruptIn I1(I1pin);
iachinweze1 12:41b3112021a3 92 InterruptIn I2(I2pin);
iachinweze1 12:41b3112021a3 93 InterruptIn I3(I3pin);
estott 0:de4320f74764 94
adehadd 26:fb6151e5907d 95
adehadd 26:fb6151e5907d 96 //~~~~~~~~~~~~~~Motor Drive outputs~~~~~~~~~~~~~~~~
adehadd 26:fb6151e5907d 97 PwmOut L1L(L1Lpin);
estott 0:de4320f74764 98 DigitalOut L1H(L1Hpin);
adehadd 26:fb6151e5907d 99 PwmOut L2L(L2Lpin);
estott 0:de4320f74764 100 DigitalOut L2H(L2Hpin);
adehadd 26:fb6151e5907d 101 PwmOut L3L(L3Lpin);
estott 0:de4320f74764 102 DigitalOut L3H(L3Hpin);
estott 0:de4320f74764 103
iachinweze1 23:ab1cb51527d1 104
adehadd 26:fb6151e5907d 105 //~Dats structure to pass information between threads~
adehadd 26:fb6151e5907d 106 Mail<message_t,16> outMessages;
iachinweze1 23:ab1cb51527d1 107
iachinweze1 23:ab1cb51527d1 108
adehadd 26:fb6151e5907d 109 //~~~~~~~~~~~~~~~~~~~~Queue~~~~~~~~~~~~~~~~~~~~~~~~
adehadd 26:fb6151e5907d 110 Queue<void, 8> inCharQ;
iachinweze1 23:ab1cb51527d1 111
iachinweze1 23:ab1cb51527d1 112
adehadd 26:fb6151e5907d 113 //~~~~~~~~~~~~Serial command buffer~~~~~~~~~~~~~~~
adehadd 26:fb6151e5907d 114 char newCmd[MAXCMDLENGTH];
adehadd 26:fb6151e5907d 115 volatile uint8_t cmdIndx = 0;
iachinweze1 23:ab1cb51527d1 116
iachinweze1 23:ab1cb51527d1 117
adehadd 26:fb6151e5907d 118 //~~~~~~~~~~Key to be passed for mining~~~~~~~~~~~
adehadd 26:fb6151e5907d 119 volatile uint64_t newKey; // Key
adehadd 26:fb6151e5907d 120 Mutex newKey_mutex; // Restrict access to prevent deadlock.
CallumAlder 19:805c87360b55 121
iachinweze1 23:ab1cb51527d1 122
adehadd 26:fb6151e5907d 123 //~~~~~~~~~~~~~~Initial conditions~~~~~~~~~~~~~~~~
adehadd 26:fb6151e5907d 124 volatile uint32_t motorPower = 300; // motor toque
adehadd 26:fb6151e5907d 125 volatile float targetVel = 45.0;
adehadd 26:fb6151e5907d 126 volatile float targetRot = 459.0;
CallumAlder 19:805c87360b55 127
iachinweze1 23:ab1cb51527d1 128
adehadd 26:fb6151e5907d 129 //~~~~~~~~~~~Motor position variable~~~~~~~~~~~~~~
adehadd 26:fb6151e5907d 130 volatile int32_t motorPos; // Motor position updated by interrupt.
iachinweze1 23:ab1cb51527d1 131
iachinweze1 23:ab1cb51527d1 132
adehadd 26:fb6151e5907d 133 //~~~~~~~~~~Serial port connection~~~~~~~~~~~~~~~~
adehadd 26:fb6151e5907d 134 RawSerial pc(SERIAL_TX, SERIAL_RX);
iachinweze1 23:ab1cb51527d1 135
iachinweze1 23:ab1cb51527d1 136
adehadd 26:fb6151e5907d 137
adehadd 26:fb6151e5907d 138
adehadd 26:fb6151e5907d 139 //~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~Threads~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
adehadd 20:c60f4785b556 140
adehadd 26:fb6151e5907d 141 Thread commOutT(osPriorityAboveNormal,1024); // Output to serial port.
adehadd 26:fb6151e5907d 142 Thread commInT(osPriorityAboveNormal,1024); // Input from serial port.
adehadd 26:fb6151e5907d 143 Thread motorCtrlT(osPriorityNormal,1024); // Motor control thread.
adehadd 20:c60f4785b556 144
adehadd 20:c60f4785b556 145
iachinweze1 23:ab1cb51527d1 146
adehadd 20:c60f4785b556 147
adehadd 26:fb6151e5907d 148 //~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~Function declarations~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
adehadd 20:c60f4785b556 149
adehadd 26:fb6151e5907d 150 void motorOut(int8_t driveState, uint32_t pw);
adehadd 26:fb6151e5907d 151 inline int8_t readRotorState();
adehadd 26:fb6151e5907d 152 int8_t motorHome();
adehadd 26:fb6151e5907d 153 void motorISR();
adehadd 26:fb6151e5907d 154 void cmdParser();
adehadd 26:fb6151e5907d 155 void commOutFn();
adehadd 26:fb6151e5907d 156 void putMessage(MsgCode code, uint32_t data);
adehadd 26:fb6151e5907d 157 void serialISR();
adehadd 26:fb6151e5907d 158 void commInFn();
adehadd 26:fb6151e5907d 159 void motorCtrlFn();
adehadd 26:fb6151e5907d 160 void motorCtrlTick();
iachinweze1 23:ab1cb51527d1 161
iachinweze1 23:ab1cb51527d1 162
iachinweze1 23:ab1cb51527d1 163
iachinweze1 23:ab1cb51527d1 164
adehadd 26:fb6151e5907d 165 //~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~Main~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
iachinweze1 23:ab1cb51527d1 166
adehadd 26:fb6151e5907d 167 int main() {
adehadd 26:fb6151e5907d 168 //~~~~~~~~~~~~~Initial serial prints~~~~~~~~~~~~~
adehadd 26:fb6151e5907d 169 pc.printf("\n\r\n\r Hello \n\r");
adehadd 26:fb6151e5907d 170 pc.printf("\n\r\n\rGroup: IndiCorp \n\r");
adehadd 26:fb6151e5907d 171 pc.printf("Initial hardcoded conditions:\n\r");
adehadd 26:fb6151e5907d 172 pc.printf("\tVelocity:\t%f\n\r", targetVel);
adehadd 26:fb6151e5907d 173 pc.printf("\tRotation:\t%f\n\r", targetRot);
adehadd 26:fb6151e5907d 174
iachinweze1 23:ab1cb51527d1 175
adehadd 26:fb6151e5907d 176 //~~~~~~~~~~~~~~~Start all threads~~~~~~~~~~~~~~~
adehadd 26:fb6151e5907d 177 commOutT.start(commOutFn);
adehadd 26:fb6151e5907d 178 commInT.start(commInFn);
adehadd 26:fb6151e5907d 179 motorCtrlT.start(motorCtrlFn);
adehadd 26:fb6151e5907d 180
adehadd 26:fb6151e5907d 181
adehadd 26:fb6151e5907d 182 //~~~~~~~~~~~~~~Attach ISR to serial~~~~~~~~~~~~
adehadd 26:fb6151e5907d 183 pc.attach(&serialISR);
adehadd 26:fb6151e5907d 184
adehadd 26:fb6151e5907d 185
adehadd 26:fb6151e5907d 186 //~~~~~~~~Attach ISR to photointerrupters~~~~~~~
adehadd 26:fb6151e5907d 187 I1.rise(&motorISR);
adehadd 26:fb6151e5907d 188 I1.fall(&motorISR);
adehadd 26:fb6151e5907d 189 I2.rise(&motorISR);
adehadd 26:fb6151e5907d 190 I2.fall(&motorISR);
adehadd 26:fb6151e5907d 191 I3.rise(&motorISR);
adehadd 26:fb6151e5907d 192 I3.fall(&motorISR);
adehadd 26:fb6151e5907d 193
iachinweze1 23:ab1cb51527d1 194
adehadd 26:fb6151e5907d 195 //~~~~~~~~~Declare Bitcoin Variables~~~~~~~~~~~
adehadd 26:fb6151e5907d 196 SHA256 sha256Inst;
adehadd 26:fb6151e5907d 197 uint8_t sequence[] = {\
adehadd 26:fb6151e5907d 198 0x45,0x6D,0x62,0x65,0x64,0x64,0x65,0x64,\
adehadd 26:fb6151e5907d 199 0x20,0x53,0x79,0x73,0x74,0x65,0x6D,0x73,\
adehadd 26:fb6151e5907d 200 0x20,0x61,0x72,0x65,0x20,0x66,0x75,0x6E,\
adehadd 26:fb6151e5907d 201 0x20,0x61,0x6E,0x64,0x20,0x64,0x6F,0x20,\
adehadd 26:fb6151e5907d 202 0x61,0x77,0x65,0x73,0x6F,0x6D,0x65,0x20,\
adehadd 26:fb6151e5907d 203 0x74,0x68,0x69,0x6E,0x67,0x73,0x21,0x20,\
adehadd 26:fb6151e5907d 204 0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,\
adehadd 26:fb6151e5907d 205 0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00
adehadd 26:fb6151e5907d 206 };
adehadd 26:fb6151e5907d 207 uint64_t* key = (uint64_t*)((int)sequence + 48);
adehadd 26:fb6151e5907d 208 uint64_t* nonce = (uint64_t*)((int)sequence + 56);
adehadd 26:fb6151e5907d 209 uint8_t hash[32];
adehadd 26:fb6151e5907d 210 uint32_t sequenceLength = 64;
adehadd 26:fb6151e5907d 211 uint32_t hashCounter = 0;
adehadd 26:fb6151e5907d 212 Timer bitcoinTimer;
iachinweze1 23:ab1cb51527d1 213
adehadd 26:fb6151e5907d 214
adehadd 26:fb6151e5907d 215 //Set PWM period to max 2000 due to hardware limitations
adehadd 26:fb6151e5907d 216 L1L.period_us(2000);
adehadd 26:fb6151e5907d 217 L2L.period_us(2000);
adehadd 26:fb6151e5907d 218 L3L.period_us(2000);
adehadd 26:fb6151e5907d 219
iachinweze1 23:ab1cb51527d1 220
adehadd 26:fb6151e5907d 221 /* Run the motor synchronisation: orState is subtracted from future rotor
adehadd 26:fb6151e5907d 222 state inputs to align rotor and motor states */
adehadd 26:fb6151e5907d 223 orState = motorHome();
adehadd 26:fb6151e5907d 224 pc.printf("Rotor origin: %x\n\r", orState); //Print state for debugging purposes.
adehadd 26:fb6151e5907d 225
adehadd 26:fb6151e5907d 226
adehadd 26:fb6151e5907d 227 //~~~~~~Give the motor a kick to begin~~~~~~~~
adehadd 26:fb6151e5907d 228 motorISR();
adehadd 26:fb6151e5907d 229
iachinweze1 23:ab1cb51527d1 230
adehadd 20:c60f4785b556 231
adehadd 26:fb6151e5907d 232 //~~~~~~~~~~~~~~~~Mining loop~~~~~~~~~~~~~~~~~
adehadd 26:fb6151e5907d 233 bitcoinTimer.start(); // start timer
adehadd 26:fb6151e5907d 234 while (1) {
adehadd 26:fb6151e5907d 235 newKey_mutex.lock();
adehadd 26:fb6151e5907d 236 (*key) = newKey;
adehadd 26:fb6151e5907d 237 newKey_mutex.unlock();
adehadd 26:fb6151e5907d 238 sha256Inst.computeHash(hash, sequence, sequenceLength);
adehadd 26:fb6151e5907d 239 hashCounter++;
adehadd 26:fb6151e5907d 240 if ((hash[0]==0) && (hash[1]==0)){
adehadd 26:fb6151e5907d 241 putMessage(Msg_nonceMatch, *nonce); // matching nonce 7
iachinweze1 23:ab1cb51527d1 242 }
adehadd 20:c60f4785b556 243
adehadd 26:fb6151e5907d 244 (*nonce)++;
iachinweze1 23:ab1cb51527d1 245
adehadd 26:fb6151e5907d 246 if (bitcoinTimer.read() >= 1){
adehadd 26:fb6151e5907d 247 putMessage(Msg_hashRate, hashCounter); // 5
adehadd 26:fb6151e5907d 248 hashCounter=0;
adehadd 26:fb6151e5907d 249 bitcoinTimer.reset();
adehadd 26:fb6151e5907d 250 }
adehadd 26:fb6151e5907d 251 }
adehadd 26:fb6151e5907d 252 }
CallumAlder 19:805c87360b55 253
adehadd 26:fb6151e5907d 254 //~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~Functions Definitions~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
adehadd 20:c60f4785b556 255
adehadd 26:fb6151e5907d 256 //~~~~~~~~~~~~Set a given drive state~~~~~~~~~~~~
adehadd 26:fb6151e5907d 257 void motorOut(int8_t driveState, uint32_t pw){
iachinweze1 23:ab1cb51527d1 258
adehadd 20:c60f4785b556 259 //Lookup the output byte from the drive state.
adehadd 20:c60f4785b556 260 int8_t driveOut = driveTable[driveState & 0x07];
adehadd 26:fb6151e5907d 261
adehadd 20:c60f4785b556 262 //Turn off first
adehadd 26:fb6151e5907d 263 if (~driveOut & 0x01) L1L.pulsewidth_us(0);
adehadd 20:c60f4785b556 264 if (~driveOut & 0x02) L1H = 1;
adehadd 26:fb6151e5907d 265 if (~driveOut & 0x04) L2L.pulsewidth_us(0);
adehadd 20:c60f4785b556 266 if (~driveOut & 0x08) L2H = 1;
adehadd 26:fb6151e5907d 267 if (~driveOut & 0x10) L3L.pulsewidth_us(0);
adehadd 20:c60f4785b556 268 if (~driveOut & 0x20) L3H = 1;
adehadd 26:fb6151e5907d 269
adehadd 20:c60f4785b556 270 //Then turn on
adehadd 26:fb6151e5907d 271 if (driveOut & 0x01) L1L.pulsewidth_us(pw);
adehadd 20:c60f4785b556 272 if (driveOut & 0x02) L1H = 0;
adehadd 26:fb6151e5907d 273 if (driveOut & 0x04) L2L.pulsewidth_us(pw);
adehadd 20:c60f4785b556 274 if (driveOut & 0x08) L2H = 0;
adehadd 26:fb6151e5907d 275 if (driveOut & 0x10) L3L.pulsewidth_us(pw);
adehadd 20:c60f4785b556 276 if (driveOut & 0x20) L3H = 0;
iachinweze1 23:ab1cb51527d1 277 }
adehadd 26:fb6151e5907d 278
iachinweze1 23:ab1cb51527d1 279
adehadd 26:fb6151e5907d 280 //~Convert photointerrupter inputs to a rotor state~
adehadd 20:c60f4785b556 281 inline int8_t readRotorState(){
adehadd 20:c60f4785b556 282 return stateMap[I1 + 2*I2 + 4*I3];
iachinweze1 23:ab1cb51527d1 283 }
adehadd 20:c60f4785b556 284
adehadd 26:fb6151e5907d 285 //~~~~~~Basic motor synchronisation routine~~~~~~
adehadd 20:c60f4785b556 286 int8_t motorHome() {
adehadd 20:c60f4785b556 287 //Put the motor in drive state 0 and wait for it to stabilise
adehadd 26:fb6151e5907d 288 motorOut(0, MAXPWM); // set to max PWM
adehadd 20:c60f4785b556 289 wait(2.0);
adehadd 26:fb6151e5907d 290
adehadd 20:c60f4785b556 291 //Get the rotor state
adehadd 20:c60f4785b556 292 return readRotorState();
adehadd 20:c60f4785b556 293 }
adehadd 20:c60f4785b556 294
adehadd 20:c60f4785b556 295
adehadd 26:fb6151e5907d 296 //~~~~~~~~~Motor ISR (photointerrupters)~~~~~~~~~
adehadd 26:fb6151e5907d 297 void motorISR() {
adehadd 26:fb6151e5907d 298 static int8_t oldRotorState;
adehadd 26:fb6151e5907d 299 int8_t rotorState = readRotorState();
adehadd 26:fb6151e5907d 300
adehadd 26:fb6151e5907d 301 motorOut((rotorState-orState+lead+6)%6,motorPower);
adehadd 26:fb6151e5907d 302
adehadd 26:fb6151e5907d 303 // update motorPosition and oldRotorState
adehadd 26:fb6151e5907d 304 if (rotorState - oldRotorState == 5) motorPos--;
adehadd 26:fb6151e5907d 305 else if (rotorState - oldRotorState == -5) motorPos++;
adehadd 26:fb6151e5907d 306 else motorPos += (rotorState - oldRotorState);
adehadd 26:fb6151e5907d 307 oldRotorState = rotorState;
adehadd 26:fb6151e5907d 308 }
adehadd 26:fb6151e5907d 309
adehadd 26:fb6151e5907d 310
adehadd 26:fb6151e5907d 311 //~~~~~Decode messages to print on serial port~~~~~
adehadd 26:fb6151e5907d 312 void commOutFn() {
adehadd 26:fb6151e5907d 313 while(1) {
adehadd 26:fb6151e5907d 314 osEvent newEvent = outMessages.get();
adehadd 26:fb6151e5907d 315 message_t *pMessage = (message_t*)newEvent.value.p;
iachinweze1 23:ab1cb51527d1 316
adehadd 26:fb6151e5907d 317 //Case switch to choose serial output based on incoming message
adehadd 26:fb6151e5907d 318 switch(pMessage->code) {
adehadd 26:fb6151e5907d 319 case Msg_motorState:
adehadd 26:fb6151e5907d 320 pc.printf("The motor is currently in state %x\n\r", pMessage->data);
adehadd 26:fb6151e5907d 321 break;
adehadd 26:fb6151e5907d 322 case Msg_hashRate:
adehadd 26:fb6151e5907d 323 pc.printf("Mining at a rate of %.2f Hash/s\n\r", (int32_t)pMessage->data);
adehadd 26:fb6151e5907d 324 break;
adehadd 26:fb6151e5907d 325 case Msg_nonceMatch:
adehadd 26:fb6151e5907d 326 pc.printf("Nonce found: %x\n\r", pMessage->data);
adehadd 26:fb6151e5907d 327 break;
adehadd 26:fb6151e5907d 328 case Msg_keyAdded:
adehadd 26:fb6151e5907d 329 pc.printf("New key added:\t0x%016x\n\r", pMessage->data);
adehadd 26:fb6151e5907d 330 break;
adehadd 26:fb6151e5907d 331 case Msg_torque:
adehadd 26:fb6151e5907d 332 pc.printf("Motor torque set to:\t%d\n\r", pMessage->data);
adehadd 26:fb6151e5907d 333 break;
adehadd 26:fb6151e5907d 334 case Msg_velocityIn:
adehadd 26:fb6151e5907d 335 pc.printf("Target velocity set to:\t%.2f\n\r", targetVel);
adehadd 26:fb6151e5907d 336 break;
adehadd 26:fb6151e5907d 337 case Msg_velocityOut:
adehadd 26:fb6151e5907d 338 pc.printf("Current Velocity:\t%.2f\n\r", \
adehadd 26:fb6151e5907d 339 (float)((int32_t)pMessage->data / 6));
adehadd 26:fb6151e5907d 340 break;
adehadd 26:fb6151e5907d 341 case Msg_positionIn:
adehadd 26:fb6151e5907d 342 pc.printf("Target rotation set to:\t%.2f\n\r", \
adehadd 26:fb6151e5907d 343 (float)((int32_t)pMessage->data / 6));
adehadd 26:fb6151e5907d 344 break;
adehadd 26:fb6151e5907d 345 case Msg_positionOut:
adehadd 26:fb6151e5907d 346 pc.printf("Current position:\t%.2f\n\r", \
adehadd 26:fb6151e5907d 347 (float)((int32_t)pMessage->data / 6));
adehadd 26:fb6151e5907d 348 break;
adehadd 26:fb6151e5907d 349 case Msg_error:
adehadd 26:fb6151e5907d 350 pc.printf("Debugging position:%x\n\r", pMessage->data);
adehadd 26:fb6151e5907d 351 break;
adehadd 26:fb6151e5907d 352 default:
adehadd 26:fb6151e5907d 353 pc.printf("Unknown Error. Data: %x\n\r", pMessage->data);
adehadd 26:fb6151e5907d 354 break;
adehadd 26:fb6151e5907d 355 }
adehadd 26:fb6151e5907d 356 outMessages.free(pMessage);
iachinweze1 24:be5fef3dace1 357 }
adehadd 26:fb6151e5907d 358 }
iachinweze1 24:be5fef3dace1 359
adehadd 26:fb6151e5907d 360
adehadd 26:fb6151e5907d 361 //~~~~~~~~~Put message in Mail queue~~~~~~~~~~~
adehadd 26:fb6151e5907d 362 void putMessage(MsgCode code, uint32_t data){
adehadd 26:fb6151e5907d 363 message_t *pMessage = outMessages.alloc();
adehadd 26:fb6151e5907d 364 pMessage->code = code;
adehadd 26:fb6151e5907d 365 pMessage->data = data;
adehadd 26:fb6151e5907d 366 outMessages.put(pMessage);
adehadd 26:fb6151e5907d 367 }
adehadd 26:fb6151e5907d 368
adehadd 26:fb6151e5907d 369
adehadd 26:fb6151e5907d 370 //~~~~Receive & decode serial input command~~~~~
adehadd 26:fb6151e5907d 371 void commInFn() {
adehadd 26:fb6151e5907d 372 while (1) {
adehadd 26:fb6151e5907d 373 osEvent newEvent = inCharQ.get();
adehadd 26:fb6151e5907d 374 uint8_t newChar = *((uint8_t*)(&newEvent.value.p));
adehadd 26:fb6151e5907d 375 pc.putc(newChar);
adehadd 26:fb6151e5907d 376 if(cmdIndx >= MAXCMDLENGTH){ //Make sure there is no overflow in comand.
adehadd 26:fb6151e5907d 377 cmdIndx = 0;
adehadd 26:fb6151e5907d 378 putMessage(Msg_error, 1);
adehadd 26:fb6151e5907d 379 }
adehadd 26:fb6151e5907d 380 else{
adehadd 26:fb6151e5907d 381 if(newChar != '\r'){ //While the command is not over,
adehadd 26:fb6151e5907d 382 newCmd[cmdIndx] = newChar; //save input character and
adehadd 26:fb6151e5907d 383 cmdIndx++; //advance index
adehadd 26:fb6151e5907d 384 }
adehadd 26:fb6151e5907d 385 else{
adehadd 26:fb6151e5907d 386 newCmd[cmdIndx] = '\0'; //When the command is finally over,
adehadd 26:fb6151e5907d 387 cmdIndx = 0; //reset index and
adehadd 26:fb6151e5907d 388 cmdParser(); //parse the command for decoding.
adehadd 26:fb6151e5907d 389 }
adehadd 26:fb6151e5907d 390 }
adehadd 26:fb6151e5907d 391 }
adehadd 20:c60f4785b556 392 }
adehadd 20:c60f4785b556 393
CallumAlder 19:805c87360b55 394
adehadd 20:c60f4785b556 395
adehadd 26:fb6151e5907d 396 //~~~~~~~~~~~~~Decode the command~~~~~~~~~~~
adehadd 26:fb6151e5907d 397 void cmdParser(){
adehadd 26:fb6151e5907d 398 switch(newCmd[0]) {
adehadd 26:fb6151e5907d 399 case 'K':
adehadd 26:fb6151e5907d 400 newKey_mutex.lock(); //Ensure there is no deadlock
adehadd 26:fb6151e5907d 401 sscanf(newCmd, "K%x", &newKey); //Find desired the Key code
adehadd 26:fb6151e5907d 402 putMessage(Msg_keyAdded, newKey); //Print it out
adehadd 26:fb6151e5907d 403 newKey_mutex.unlock();
adehadd 26:fb6151e5907d 404 break;
adehadd 26:fb6151e5907d 405 case 'V':
adehadd 26:fb6151e5907d 406 sscanf(newCmd, "V%f", &targetVel); //Find desired the target velocity
adehadd 26:fb6151e5907d 407 putMessage(Msg_velocityIn, targetVel); //Print it out
adehadd 26:fb6151e5907d 408 break;
adehadd 26:fb6151e5907d 409 case 'R':
adehadd 26:fb6151e5907d 410 sscanf(newCmd, "R%f", &targetRot); //Find desired target rotation
adehadd 26:fb6151e5907d 411 putMessage(Msg_positionIn, targetRot); //Print it out
adehadd 26:fb6151e5907d 412 break;
adehadd 26:fb6151e5907d 413 case 'T':
adehadd 26:fb6151e5907d 414 sscanf(newCmd, "T%d", &motorPower); //Find desired target torque
adehadd 26:fb6151e5907d 415 putMessage(Msg_torque, motorPower); //Print it out
adehadd 26:fb6151e5907d 416 break;
adehadd 26:fb6151e5907d 417 default: break;
adehadd 26:fb6151e5907d 418 }
adehadd 26:fb6151e5907d 419 }
iachinweze1 23:ab1cb51527d1 420
iachinweze1 23:ab1cb51527d1 421
adehadd 26:fb6151e5907d 422 //~~~~~~~~~~~~~Serial ISR~~~~~~~~~~~~
adehadd 26:fb6151e5907d 423 void serialISR() {
adehadd 26:fb6151e5907d 424 uint8_t newChar = pc.getc();
adehadd 26:fb6151e5907d 425 inCharQ.put((void*)newChar);
adehadd 26:fb6151e5907d 426 }
iachinweze1 23:ab1cb51527d1 427
adehadd 26:fb6151e5907d 428
adehadd 26:fb6151e5907d 429 //~~~~~~ISR triggered by Ticker~~~~~~
adehadd 26:fb6151e5907d 430 void motorCtrlTick(){
adehadd 26:fb6151e5907d 431 motorCtrlT.signal_set(0x1); //Set signal to motor control thread which carries out calculations to avoid CPU blocking
adehadd 26:fb6151e5907d 432 }
iachinweze1 12:41b3112021a3 433
CallumAlder 15:2f95f2fb68e3 434
adehadd 26:fb6151e5907d 435 //~~~~~~~~~~~~~Motor control function with proportional controller~~~~~~~~~~~
adehadd 26:fb6151e5907d 436 void motorCtrlFn() {
adehadd 26:fb6151e5907d 437
adehadd 26:fb6151e5907d 438 //~~~~~~~~~~~~~Variables~~~~~~~~~~~~~~~~
adehadd 26:fb6151e5907d 439 Ticker motorCtrlTicker; //Ticker to ba attached to callback function
adehadd 26:fb6151e5907d 440 int32_t velocity; //Variable for local velocity calculation
adehadd 26:fb6151e5907d 441 int32_t locMotorPos; //Local copy of motor position
adehadd 26:fb6151e5907d 442 static int32_t oldMotorPos = 0; //Old motor position used for calculations
adehadd 26:fb6151e5907d 443 static uint8_t motorCtrlCounter = 0; //Counter to be reset every 10 iterations to get velocity calculation in seconds
adehadd 26:fb6151e5907d 444 int32_t torque; //Local variable to set motor torque
adehadd 26:fb6151e5907d 445 float sError; //Velocity error between target and reality
adehadd 26:fb6151e5907d 446 float rError; //Rotation error between target and reality
adehadd 26:fb6151e5907d 447 static float rErrorOld; //Old rotation error used for calculation
adehadd 26:fb6151e5907d 448
adehadd 26:fb6151e5907d 449 //~~~Controller constants~~~~
adehadd 26:fb6151e5907d 450 int32_t Kp1=22; //Proportional controller constants
adehadd 26:fb6151e5907d 451 int32_t Kp2=22; //Calculated by trial and error to give optimal accuracy
adehadd 26:fb6151e5907d 452 float Kd=15.5;
adehadd 26:fb6151e5907d 453
adehadd 26:fb6151e5907d 454
adehadd 26:fb6151e5907d 455 //Attach ticker to callback function that will run every 100 ms
adehadd 26:fb6151e5907d 456 motorCtrlTicker.attach_us(&motorCtrlTick,100000);
adehadd 26:fb6151e5907d 457
adehadd 26:fb6151e5907d 458
adehadd 26:fb6151e5907d 459
adehadd 26:fb6151e5907d 460 while(1) {
adehadd 26:fb6151e5907d 461 motorCtrlT.signal_wait(0x1); // Wait for thread signal.
adehadd 26:fb6151e5907d 462
adehadd 26:fb6151e5907d 463 //Initial velocity calculation and report
adehadd 26:fb6151e5907d 464 locMotorPos = motorPos; //Read global variable motorPos which is updated by interrupt and store it in local variable
adehadd 26:fb6151e5907d 465 velocity = (locMotorPos - oldMotorPos) * 10; //Proceed with calculation
adehadd 26:fb6151e5907d 466 oldMotorPos = locMotorPos; //Update old motor position
adehadd 26:fb6151e5907d 467 motorCtrlCounter++; //Advance counter
adehadd 26:fb6151e5907d 468 if (motorCtrlCounter >= 10) { //Every 10th iteration
adehadd 26:fb6151e5907d 469 motorCtrlCounter = 0; //Reset counter
adehadd 26:fb6151e5907d 470 putMessage(Msg_velocityOut, velocity); //Report the current velocity
adehadd 26:fb6151e5907d 471 putMessage(Msg_positionOut, locMotorPos); //Report the current position
adehadd 26:fb6151e5907d 472 }
adehadd 26:fb6151e5907d 473 /*
adehadd 26:fb6151e5907d 474 //~~~~~Speed controller~~~~~~
adehadd 26:fb6151e5907d 475 sError = (targetVel * 6) - abs(velocity); //Read global variable targetVel updated by interrupt and calculate error between target and reality
adehadd 26:fb6151e5907d 476 int32_t Ys; //Initialise controller output Ys
adehadd 26:fb6151e5907d 477 if (sError == -abs(velocity)) { //Check if user entered V0,
adehadd 26:fb6151e5907d 478 Ys = MAXPWM; //and set the output to maximum as specified
adehadd 26:fb6151e5907d 479 }
adehadd 26:fb6151e5907d 480 else {
adehadd 26:fb6151e5907d 481 Ys = (int)(Kp1 * sError); //If the user didn't enter V0 implement controller transfer function: Ys = Kp * (s -|v|) where,
adehadd 26:fb6151e5907d 482 } //Ys = controller output, Kp = prop controller constant, s = target velocity and v is the measured velocity
adehadd 26:fb6151e5907d 483
adehadd 26:fb6151e5907d 484 //~~~~~Rotation control~~~~~~
adehadd 26:fb6151e5907d 485 rError = targetRot - (locMotorPos/6); //Read global variable targetRot updated by interrupt and calculate the rotation error.
adehadd 26:fb6151e5907d 486 int32_t Yr; //Initialise controller output Yr
adehadd 26:fb6151e5907d 487 Yr = Kp2*rError + Kd*(rError - rErrorOld); //Implement controller transfer function Ys= Kp*Er + Kd* (dEr/dt)
adehadd 26:fb6151e5907d 488 rErrorOld = rError; //Update rotation error
adehadd 26:fb6151e5907d 489 if(rError < 0){ //Use the sign of the error to set controller wrt direction of rotation
adehadd 26:fb6151e5907d 490 Ys = -Ys;
CallumAlder 15:2f95f2fb68e3 491 }
CallumAlder 15:2f95f2fb68e3 492
adehadd 26:fb6151e5907d 493 if((velocity>=0 && Ys<Yr) || (velocity<0 && Ys>Yr)){ //Choose Ys or Yr based on distance from target value so that it takes
adehadd 26:fb6151e5907d 494 torque = Ys; //appropriate steps in the right direction to reach target value
adehadd 26:fb6151e5907d 495 }
adehadd 26:fb6151e5907d 496 else{
adehadd 26:fb6151e5907d 497 torque = Yr;
adehadd 26:fb6151e5907d 498 }
adehadd 26:fb6151e5907d 499 if(torque < 0){ //Variable torque cannot be negative since it sets the PWM
adehadd 26:fb6151e5907d 500 torque = -torque; //Hence we make the value positive,
adehadd 26:fb6151e5907d 501 lead = -2; //and instead set the direction to the opposite one
adehadd 18:7ee632098fd4 502 }
adehadd 26:fb6151e5907d 503 else{
adehadd 26:fb6151e5907d 504 lead = 2;
adehadd 18:7ee632098fd4 505 }
adehadd 26:fb6151e5907d 506 if(torque > MAXPWM){ //In case the calculated PWM is higher than our maximum 50% allowance,
adehadd 26:fb6151e5907d 507 torque = MAXPWM; //Set it to our max.
adehadd 26:fb6151e5907d 508 }
adehadd 26:fb6151e5907d 509 motorPower = torque; //Lastly, update global variable motorPower which is updated by interrupt
adehadd 26:fb6151e5907d 510 */
CallumAlder 15:2f95f2fb68e3 511 }
adehadd 26:fb6151e5907d 512 }