Skad00sh / Mbed OS ES_CW2_Dev

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

Dependencies:   Crypto

main.cpp@47:21bf4096faa1, 2019-03-21 (annotated)

Committer:
CallumAlder
Date:
Thu Mar 21 16:45:30 2019 +0000
Revision:
47:21bf4096faa1
Parent:
46:b9081aa50bda
Child:
48:b2afe48ced0d
midday food

Who changed what in which revision?

UserRevisionLine numberNew contents of line
adehadd 27:ce05fed3c1ea 1 /*TODO:
CallumAlder 42:121148278dae 2 Change:
CallumAlder 42:121148278dae 3 Indx
CallumAlder 47:21bf4096faa1 4 _newCmd
CallumAlder 43:a6d20109b2f2 5 _MAXCMDLENGTH
CallumAlder 42:121148278dae 6 move the global variables to a class because we arent paeasents - Mission Failed
CallumAlder 42:121148278dae 7 use jack's motor motor position
CallumAlder 42:121148278dae 8 fix class variable naming
CallumAlder 42:121148278dae 9 dont make everything public becuase thats fucling dumb and defeats the whole point of a class
CallumAlder 47:21bf4096faa1 10
CallumAlder 47:21bf4096faa1 11 Move things out of public and into a protected part of the class
CallumAlder 47:21bf4096faa1 12
CallumAlder 47:21bf4096faa1 13 Move char Comm::_inCharQ[] = {'.','.','... into the class by making it a vector
CallumAlder 47:21bf4096faa1 14
CallumAlder 47:21bf4096faa1 15 Change abs lacro to
CallumAlder 47:21bf4096faa1 16 NOT V0 but R0 (to go on forever)
CallumAlder 47:21bf4096faa1 17
CallumAlder 47:21bf4096faa1 18 Actually make the code robust lol
adehadd 27:ce05fed3c1ea 19 */
estott 0:de4320f74764 20
estott 0:de4320f74764 21 //Mapping from sequential drive states to motor phase outputs
estott 0:de4320f74764 22 /*
estott 0:de4320f74764 23 State L1 L2 L3
estott 0:de4320f74764 24 0 H - L
estott 0:de4320f74764 25 1 - H L
estott 0:de4320f74764 26 2 L H -
estott 0:de4320f74764 27 3 L - H
estott 0:de4320f74764 28 4 - L H
estott 0:de4320f74764 29 5 H L -
estott 0:de4320f74764 30 6 - - -
estott 0:de4320f74764 31 7 - - -
estott 0:de4320f74764 32 */
CallumAlder 42:121148278dae 33
CallumAlder 42:121148278dae 34 //Header Files
CallumAlder 42:121148278dae 35 #include "SHA256.h"
CallumAlder 42:121148278dae 36 #include "mbed.h"
CallumAlder 42:121148278dae 37
CallumAlder 42:121148278dae 38 //Photointerrupter Input Pins
CallumAlder 42:121148278dae 39 #define I1pin D3
CallumAlder 42:121148278dae 40 #define I2pin D6
CallumAlder 42:121148278dae 41 #define I3pin D5
CallumAlder 42:121148278dae 42
CallumAlder 42:121148278dae 43 //Incremental Encoder Input Pins
CallumAlder 42:121148278dae 44 #define CHApin D12
CallumAlder 42:121148278dae 45 #define CHBpin D11
CallumAlder 42:121148278dae 46
CallumAlder 42:121148278dae 47 //Motor Drive High Pins //Mask in output byte
CallumAlder 42:121148278dae 48 #define L1Hpin A3 //0x02
CallumAlder 42:121148278dae 49 #define L2Hpin A6 //0x08
CallumAlder 42:121148278dae 50 #define L3Hpin D2 //0x20
CallumAlder 42:121148278dae 51
CallumAlder 42:121148278dae 52 //Motor Drive Low Pins
CallumAlder 42:121148278dae 53 #define L1Lpin D1 //0x01
CallumAlder 42:121148278dae 54 #define L2Lpin D0 //0x04
CallumAlder 42:121148278dae 55 #define L3Lpin D10 //0x10
CallumAlder 42:121148278dae 56
CallumAlder 42:121148278dae 57 //Motor Pulse Width Modulation (PWM) Pin
CallumAlder 42:121148278dae 58 #define PWMpin D9
CallumAlder 42:121148278dae 59
CallumAlder 42:121148278dae 60 //Motor current sense
CallumAlder 42:121148278dae 61 #define MCSPpin A1
CallumAlder 42:121148278dae 62 #define MCSNpin A0
CallumAlder 42:121148278dae 63
CallumAlder 42:121148278dae 64 // "Lacros" for utility
CallumAlder 47:21bf4096faa1 65 #define max(x,y) ( (x)>=(y) ? (x):(y) )
CallumAlder 47:21bf4096faa1 66 #define min(x,y) ( (x)>=(y) ? (y):(x) )
CallumAlder 47:21bf4096faa1 67 #define sgn(x) ( (x)>= 0 ? 1 :-1 )
CallumAlder 42:121148278dae 68
CallumAlder 42:121148278dae 69 //Status LED
CallumAlder 42:121148278dae 70 DigitalOut led1(LED1);
CallumAlder 42:121148278dae 71
CallumAlder 42:121148278dae 72 //Photointerrupter Inputs
CallumAlder 42:121148278dae 73 InterruptIn I1(I1pin);
CallumAlder 42:121148278dae 74 InterruptIn I2(I2pin);
CallumAlder 42:121148278dae 75 InterruptIn I3(I3pin);
CallumAlder 42:121148278dae 76
CallumAlder 42:121148278dae 77 //Motor Drive High Outputs
CallumAlder 42:121148278dae 78 DigitalOut L1H(L1Hpin);
CallumAlder 42:121148278dae 79 DigitalOut L2H(L2Hpin);
CallumAlder 42:121148278dae 80 DigitalOut L3H(L3Hpin);
CallumAlder 42:121148278dae 81
CallumAlder 42:121148278dae 82 //Motor Drive Low Outputs
CallumAlder 42:121148278dae 83 DigitalOut L1L(L1Lpin);
CallumAlder 42:121148278dae 84 DigitalOut L2L(L2Lpin);
CallumAlder 42:121148278dae 85 DigitalOut L3L(L3Lpin);
CallumAlder 42:121148278dae 86
CallumAlder 42:121148278dae 87 PwmOut pwmCtrl(PWMpin);
CallumAlder 42:121148278dae 88
adehadd 27:ce05fed3c1ea 89 //Drive state to output table
estott 0:de4320f74764 90 const int8_t driveTable[] = {0x12,0x18,0x09,0x21,0x24,0x06,0x00,0x00};
estott 2:4e88faab6988 91
adehadd 27:ce05fed3c1ea 92 //Mapping from interrupter inputs to sequential rotor states. 0x00 and 0x07 are not valid
CallumAlder 47:21bf4096faa1 93 const int8_t stateMap[] = {0x07,0x05,0x03,0x04,0x01,0x00,0x02,0x07};
CallumAlder 47:21bf4096faa1 94 //const int8_t stateMap[] = {0x07,0x01,0x03,0x02,0x05,0x00,0x04,0x07}; //Alternative if phase order of input or drive is reversed
CallumAlder 47:21bf4096faa1 95
estott 2:4e88faab6988 96
CallumAlder 42:121148278dae 97 class Comm{
CallumAlder 42:121148278dae 98
CallumAlder 42:121148278dae 99 public:
estott 0:de4320f74764 100
CallumAlder 43:a6d20109b2f2 101 volatile bool _outMining;
CallumAlder 43:a6d20109b2f2 102 volatile float _targetVel, _targetRot;
CallumAlder 47:21bf4096faa1 103 volatile char _notes[9]; // Array of actual _notes
estott 0:de4320f74764 104
CallumAlder 47:21bf4096faa1 105 volatile int8_t _modeBitField; // 0,0,0,... <=> Melody,Torque,Rotation,Velocity
CallumAlder 47:21bf4096faa1 106 const uint8_t _MAXCMDLENGTH; //
CallumAlder 47:21bf4096faa1 107 volatile uint8_t _inCharIndex, _cmdIndex,
CallumAlder 47:21bf4096faa1 108 _noteDur[9],_noteLen; // Array of note durations
CallumAlder 47:21bf4096faa1 109 volatile uint32_t _motorTorque; // Motor Toque
CallumAlder 47:21bf4096faa1 110 volatile uint64_t _newKey; // hash key
CallumAlder 47:21bf4096faa1 111 Mutex _newKeyMutex; // Restrict access to prevent deadlock.
CallumAlder 43:a6d20109b2f2 112
CallumAlder 47:21bf4096faa1 113 RawSerial _pc;
CallumAlder 47:21bf4096faa1 114 Thread _tCommOut;
CallumAlder 47:21bf4096faa1 115 bool _RUN;
CallumAlder 42:121148278dae 116
CallumAlder 47:21bf4096faa1 117 enum msgType { motorState, posIn, velIn, posOut, velOut,
CallumAlder 42:121148278dae 118 hashRate, keyAdded, nonceMatch,
CallumAlder 43:a6d20109b2f2 119 torque, rotations, melody,
CallumAlder 42:121148278dae 120 error};
adehadd 27:ce05fed3c1ea 121
CallumAlder 47:21bf4096faa1 122 typedef struct { msgType type;
CallumAlder 47:21bf4096faa1 123 uint32_t message;} msg;
adehadd 27:ce05fed3c1ea 124
CallumAlder 47:21bf4096faa1 125 Mail<msg, 32> _msgStack;
CallumAlder 47:21bf4096faa1 126
iachinweze1 23:ab1cb51527d1 127
CallumAlder 47:21bf4096faa1 128
CallumAlder 47:21bf4096faa1 129 //------------- Default Constructor With Inheritance From RawSerial Constructor -------------//
CallumAlder 47:21bf4096faa1 130 Comm(): _pc(SERIAL_TX, SERIAL_RX), _tCommOut(osPriorityAboveNormal, 1024), _MAXCMDLENGTH(18){
adehadd 27:ce05fed3c1ea 131
CallumAlder 47:21bf4096faa1 132 _cmdIndex = 0;
CallumAlder 47:21bf4096faa1 133 _inCharIndex = 0;
adehadd 45:402a8a9423b9 134
CallumAlder 47:21bf4096faa1 135 _outMining = false;
CallumAlder 47:21bf4096faa1 136 _motorTorque = 300;
CallumAlder 47:21bf4096faa1 137 _targetRot = 459.0;
CallumAlder 47:21bf4096faa1 138 _targetVel = 45.0;
iachinweze1 23:ab1cb51527d1 139
CallumAlder 47:21bf4096faa1 140 _modeBitField = 0x01; // Default velocity mode
adehadd 45:402a8a9423b9 141
CallumAlder 47:21bf4096faa1 142 _pc.printf("\n\r%s\n\r", "Welcome\n>" ); // Welcome
CallumAlder 47:21bf4096faa1 143 //_pc.putc('>');
CallumAlder 47:21bf4096faa1 144 for (int i = 0; i < _MAXCMDLENGTH; ++i) // Reset buffer
CallumAlder 47:21bf4096faa1 145 _inCharQ[i] = (char)'.'; // If a null terminator is printed Mbed prints 'Embedded Systems are fun and do awesome things!'
CallumAlder 42:121148278dae 146
CallumAlder 47:21bf4096faa1 147 _inCharQ[_MAXCMDLENGTH] = (char)'\0';
CallumAlder 47:21bf4096faa1 148 sprintf(_inCharQ, "%s", _inCharQ); // Handling of the correct string
CallumAlder 47:21bf4096faa1 149 strncpy(_newCmd, _inCharQ, _MAXCMDLENGTH);
CallumAlder 19:805c87360b55 150
CallumAlder 47:21bf4096faa1 151 _pc.printf("%s\n\r", _inCharQ);
CallumAlder 47:21bf4096faa1 152 _pc.putc('<');
CallumAlder 47:21bf4096faa1 153 _pc.attach(callback(this, &Comm::serialISR));
CallumAlder 42:121148278dae 154 }
iachinweze1 23:ab1cb51527d1 155
CallumAlder 42:121148278dae 156 //--------- Interrupt Service Routine for Serial Port and Character Queue Handling ---------//
CallumAlder 42:121148278dae 157 void serialISR(){
CallumAlder 43:a6d20109b2f2 158 if (_pc.readable()) {
CallumAlder 43:a6d20109b2f2 159 char newChar = _pc.getc();
CallumAlder 42:121148278dae 160
CallumAlder 43:a6d20109b2f2 161 if (_inCharIndex == (_MAXCMDLENGTH)) {
CallumAlder 47:21bf4096faa1 162 _inCharQ[_MAXCMDLENGTH] = '\0'; // Force the string to have an end character
CallumAlder 42:121148278dae 163 putMessage(error, 1);
CallumAlder 47:21bf4096faa1 164 _inCharIndex = 0; // Reset buffer index
adehadd 27:ce05fed3c1ea 165 }
adehadd 27:ce05fed3c1ea 166 else{
CallumAlder 47:21bf4096faa1 167 if(newChar != '\r'){ // While the command is not over,
CallumAlder 47:21bf4096faa1 168 _inCharQ[_inCharIndex] = newChar; // Save input character and
CallumAlder 47:21bf4096faa1 169 _inCharIndex++; // Advance index
CallumAlder 43:a6d20109b2f2 170 _pc.putc(newChar);
CallumAlder 42:121148278dae 171 }
CallumAlder 42:121148278dae 172 else{
CallumAlder 47:21bf4096faa1 173 _inCharQ[_inCharIndex] = '\0'; // When the command is finally over,
CallumAlder 47:21bf4096faa1 174 strncpy(_newCmd, _inCharQ, _MAXCMDLENGTH); // Will copy 18 characters from _inCharQ to _newCmd
CallumAlder 42:121148278dae 175
CallumAlder 47:21bf4096faa1 176 for (int i = 0; i < _MAXCMDLENGTH; ++i) // Reset buffer
CallumAlder 47:21bf4096faa1 177 _inCharQ[i] = ' ';
CallumAlder 47:21bf4096faa1 178
CallumAlder 47:21bf4096faa1 179 _inCharIndex = 0; // Reset index
CallumAlder 47:21bf4096faa1 180
CallumAlder 47:21bf4096faa1 181 cmdParser(); // Parse the command for decoding
CallumAlder 42:121148278dae 182 }
adehadd 27:ce05fed3c1ea 183 }
adehadd 27:ce05fed3c1ea 184 }
adehadd 27:ce05fed3c1ea 185 }
CallumAlder 19:805c87360b55 186
CallumAlder 42:121148278dae 187 //--------- Reset Cursor Position ---------//
CallumAlder 42:121148278dae 188 void returnCursor() {
CallumAlder 43:a6d20109b2f2 189 _pc.putc('>');
CallumAlder 43:a6d20109b2f2 190 for (int i = 0; i < _inCharIndex; ++i)
CallumAlder 47:21bf4096faa1 191 _pc.putc(_inCharQ[i]);
CallumAlder 42:121148278dae 192 }
CallumAlder 47:21bf4096faa1 193
CallumAlder 47:21bf4096faa1 194 //--------- Parse Incoming Data From Serial Port ---------//
CallumAlder 42:121148278dae 195 void cmdParser(){
CallumAlder 47:21bf4096faa1 196 switch(_newCmd[0]) {
CallumAlder 47:21bf4096faa1 197 case 'K': // keyAdded
CallumAlder 47:21bf4096faa1 198 _newKeyMutex.lock(); // Ensure there is no deadlock
CallumAlder 47:21bf4096faa1 199 sscanf(_newCmd, "K%x", &_newKey); // Find desired the Key code
CallumAlder 47:21bf4096faa1 200 putMessage(keyAdded, _newKey); // Print it out
CallumAlder 43:a6d20109b2f2 201 _newKeyMutex.unlock();
CallumAlder 43:a6d20109b2f2 202 break;
CallumAlder 43:a6d20109b2f2 203
CallumAlder 47:21bf4096faa1 204 case 'V': // velIn
CallumAlder 47:21bf4096faa1 205 sscanf(_newCmd, "V%f", &_targetVel); // Find desired the target velocity
CallumAlder 47:21bf4096faa1 206 _modeBitField = 0x01; // Adjust bitfield pos 1
CallumAlder 47:21bf4096faa1 207 putMessage(velIn, _targetVel); // Print it out
CallumAlder 43:a6d20109b2f2 208 break;
CallumAlder 43:a6d20109b2f2 209
CallumAlder 47:21bf4096faa1 210 case 'R': // posIn
CallumAlder 47:21bf4096faa1 211 sscanf(_newCmd, "R%f", &_targetRot); // Find desired target rotation
CallumAlder 47:21bf4096faa1 212 _modeBitField = 0x02; // Adjust bitfield pos 2
CallumAlder 47:21bf4096faa1 213 putMessage(posIn, _targetRot); // Print it out
CallumAlder 42:121148278dae 214 break;
iachinweze1 23:ab1cb51527d1 215
CallumAlder 47:21bf4096faa1 216 case 'x': // Torque
CallumAlder 47:21bf4096faa1 217 sscanf(_newCmd, "x%u", &_motorTorque); // Find desired target torque
CallumAlder 47:21bf4096faa1 218 _modeBitField = 0x04; // Adjust bitfield pos 3
CallumAlder 47:21bf4096faa1 219 putMessage(torque, _motorTorque); // Print it out
adehadd 27:ce05fed3c1ea 220 break;
CallumAlder 42:121148278dae 221
CallumAlder 47:21bf4096faa1 222 case 'M': // Mining display toggle
CallumAlder 47:21bf4096faa1 223 int8_t miningTest;
CallumAlder 47:21bf4096faa1 224 sscanf(_newCmd, "M%d", &miningTest); // Display if input is 1
CallumAlder 47:21bf4096faa1 225 miningTest == 1 ? _outMining = true : _outMining = false;
CallumAlder 47:21bf4096faa1 226 break;
CallumAlder 47:21bf4096faa1 227
CallumAlder 47:21bf4096faa1 228 case 'T': // Play tune
CallumAlder 47:21bf4096faa1 229 regexTune() ? putMessage(melody, 1) : putMessage(error, 2);
CallumAlder 47:21bf4096faa1 230 break; // Break from case 'T'
CallumAlder 47:21bf4096faa1 231
CallumAlder 47:21bf4096faa1 232 default: // Break from switch
adehadd 27:ce05fed3c1ea 233 break;
adehadd 27:ce05fed3c1ea 234 }
adehadd 27:ce05fed3c1ea 235 }
adehadd 27:ce05fed3c1ea 236
CallumAlder 47:21bf4096faa1 237 bool regexTune() {
CallumAlder 47:21bf4096faa1 238
CallumAlder 47:21bf4096faa1 239 uint8_t len = 0;
CallumAlder 47:21bf4096faa1 240
CallumAlder 47:21bf4096faa1 241 for (int i = 1; i < _MAXCMDLENGTH; ++i) // Find first #
CallumAlder 47:21bf4096faa1 242 if (_newCmd[i] == '#') {
CallumAlder 47:21bf4096faa1 243 len = i;
CallumAlder 47:21bf4096faa1 244 break; // Stop at first # found
CallumAlder 47:21bf4096faa1 245 }
CallumAlder 47:21bf4096faa1 246
CallumAlder 47:21bf4096faa1 247 if (len>0) { // Parse the input only if # found
CallumAlder 47:21bf4096faa1 248 uint8_t newLen = 2*(len+1)+1;
CallumAlder 47:21bf4096faa1 249 bool isChar = true;
CallumAlder 47:21bf4096faa1 250 char formatSpec[newLen];
CallumAlder 47:21bf4096faa1 251 formatSpec[0]='T';
CallumAlder 47:21bf4096faa1 252 for (int i = 1; i < newLen; i=i+2) { // Create a format spec based on length of input
CallumAlder 47:21bf4096faa1 253 formatSpec[i] = '%';
CallumAlder 47:21bf4096faa1 254 isChar ? formatSpec[i+1] = 'c' : \
CallumAlder 47:21bf4096faa1 255 formatSpec[i+1] = 'u' ;
CallumAlder 47:21bf4096faa1 256 isChar = !isChar;
CallumAlder 47:21bf4096faa1 257 }
CallumAlder 47:21bf4096faa1 258
CallumAlder 47:21bf4096faa1 259 formatSpec[newLen] = '\0';
CallumAlder 47:21bf4096faa1 260 sprintf(formatSpec, "%s", formatSpec); // Set string format correctly
CallumAlder 47:21bf4096faa1 261 // _pc.printf("%s\n", formatSpec );
CallumAlder 47:21bf4096faa1 262 sscanf(_newCmd, formatSpec, &_notes[0], &_noteDur[0],
CallumAlder 47:21bf4096faa1 263 &_notes[1], &_noteDur[1],
CallumAlder 47:21bf4096faa1 264 &_notes[2], &_noteDur[2],
CallumAlder 47:21bf4096faa1 265 &_notes[3], &_noteDur[3],
CallumAlder 47:21bf4096faa1 266 &_notes[4], &_noteDur[4],
CallumAlder 47:21bf4096faa1 267 &_notes[5], &_noteDur[5],
CallumAlder 47:21bf4096faa1 268 &_notes[6], &_noteDur[6],
CallumAlder 47:21bf4096faa1 269 &_notes[7], &_noteDur[7],
CallumAlder 47:21bf4096faa1 270 &_notes[8], &_noteDur[8]);
CallumAlder 47:21bf4096faa1 271
CallumAlder 47:21bf4096faa1 272
CallumAlder 47:21bf4096faa1 273 // Update _newCmd for putMessage print
CallumAlder 47:21bf4096faa1 274 sprintf(_newCmd,formatSpec, _notes[0], _noteDur[0],\
CallumAlder 47:21bf4096faa1 275 _notes[1], _noteDur[1],\
CallumAlder 47:21bf4096faa1 276 _notes[2], _noteDur[2],\
CallumAlder 47:21bf4096faa1 277 _notes[3], _noteDur[3],\
CallumAlder 47:21bf4096faa1 278 _notes[4], _noteDur[4],\
CallumAlder 47:21bf4096faa1 279 _notes[5], _noteDur[5],\
CallumAlder 47:21bf4096faa1 280 _notes[6], _noteDur[6],\
CallumAlder 47:21bf4096faa1 281 _notes[7], _noteDur[7],\
CallumAlder 47:21bf4096faa1 282 _notes[8], _noteDur[8]);
CallumAlder 47:21bf4096faa1 283 _noteLen = len;
CallumAlder 47:21bf4096faa1 284 return true;
CallumAlder 47:21bf4096faa1 285 }
CallumAlder 47:21bf4096faa1 286
CallumAlder 47:21bf4096faa1 287 else {
CallumAlder 47:21bf4096faa1 288 return false;
CallumAlder 47:21bf4096faa1 289 }
CallumAlder 47:21bf4096faa1 290 }
CallumAlder 47:21bf4096faa1 291
CallumAlder 42:121148278dae 292 //--------- Decode Messages to Print on Serial Port ---------//
CallumAlder 42:121148278dae 293 void commOutFn() {
CallumAlder 42:121148278dae 294 while (_RUN) {
CallumAlder 47:21bf4096faa1 295 osEvent newEvent = _msgStack.get();
CallumAlder 42:121148278dae 296 msg *pMessage = (msg *) newEvent.value.p;
adehadd 27:ce05fed3c1ea 297
CallumAlder 47:21bf4096faa1 298 //Case switch to choose serial output based on incoming message enum
CallumAlder 42:121148278dae 299 switch (pMessage->type) {
CallumAlder 42:121148278dae 300 case motorState:
CallumAlder 47:21bf4096faa1 301 _pc.printf("\r>%s< The motor is currently in state %x\n\r", _inCharQ, pMessage->message);
CallumAlder 42:121148278dae 302 break;
CallumAlder 42:121148278dae 303 case hashRate:
CallumAlder 43:a6d20109b2f2 304 if (_outMining) {
CallumAlder 47:21bf4096faa1 305 _pc.printf("\r>%s< Mining: %.4u Hash/s\r", _inCharQ, (uint32_t) pMessage->message);
CallumAlder 42:121148278dae 306 returnCursor();
CallumAlder 43:a6d20109b2f2 307 _outMining = false;
CallumAlder 42:121148278dae 308 }
CallumAlder 42:121148278dae 309 break;
CallumAlder 42:121148278dae 310 case nonceMatch:
CallumAlder 47:21bf4096faa1 311 _pc.printf("\r>%s< Nonce found: %x\n\r", _inCharQ, pMessage->message);
CallumAlder 42:121148278dae 312 returnCursor();
CallumAlder 42:121148278dae 313 break;
CallumAlder 42:121148278dae 314 case keyAdded:
CallumAlder 47:21bf4096faa1 315 _pc.printf("\r>%s< New Key Added:\t0x%016x\n\r", _inCharQ, pMessage->message);
CallumAlder 42:121148278dae 316 break;
CallumAlder 42:121148278dae 317 case torque:
CallumAlder 47:21bf4096faa1 318 _pc.printf("\r>%s< Motor Torque set to:\t%d\n\r", _inCharQ, (int32_t) pMessage->message);
CallumAlder 42:121148278dae 319 break;
CallumAlder 42:121148278dae 320 case velIn:
CallumAlder 47:21bf4096faa1 321 _pc.printf("\r>%s< Target Velocity set to:\t%.2f\n\r", _inCharQ, _targetVel);
CallumAlder 42:121148278dae 322 break;
CallumAlder 42:121148278dae 323 case velOut:
CallumAlder 47:21bf4096faa1 324 _pc.printf("\r>%s< Current Velocity:\t%.2f States/sec\n\r", _inCharQ, (float) ((int32_t) pMessage->message));
CallumAlder 42:121148278dae 325 break;
CallumAlder 42:121148278dae 326 case posIn:
CallumAlder 47:21bf4096faa1 327 _pc.printf("\r>%s< Target # Rotations:\t%.2f\n\r", _inCharQ, (float) ((int32_t) pMessage->message));
CallumAlder 42:121148278dae 328 break;
CallumAlder 42:121148278dae 329 case posOut:
CallumAlder 47:21bf4096faa1 330 _pc.printf("\r>%s< Current Position:\t%.2f\n\r", _inCharQ, (float) ((int32_t) pMessage->message));
CallumAlder 42:121148278dae 331 break;
CallumAlder 47:21bf4096faa1 332 case melody:
CallumAlder 47:21bf4096faa1 333 _pc.printf("\r>%s< New Tune:\t%s\n\r", _inCharQ, _newCmd);
CallumAlder 47:21bf4096faa1 334 break;
CallumAlder 42:121148278dae 335 case error:
CallumAlder 47:21bf4096faa1 336 switch (pMessage->message) {
CallumAlder 47:21bf4096faa1 337 case 1:
CallumAlder 47:21bf4096faa1 338 _pc.printf("\r>%s< Error:%s\n\r", _inCharQ, "Overfull Buffer Reset" );
CallumAlder 47:21bf4096faa1 339 break;
CallumAlder 47:21bf4096faa1 340 case 2:
CallumAlder 47:21bf4096faa1 341 _pc.printf("\r>%s< Error:%s\n\r", _inCharQ, "Invalid Melody" );
CallumAlder 47:21bf4096faa1 342 default:
CallumAlder 47:21bf4096faa1 343 break;
CallumAlder 47:21bf4096faa1 344 }
CallumAlder 47:21bf4096faa1 345 for (int i = 0; i < _MAXCMDLENGTH; ++i) // reset buffer
CallumAlder 47:21bf4096faa1 346 _inCharQ[i] = ' ';
CallumAlder 47:21bf4096faa1 347
CallumAlder 47:21bf4096faa1 348 _inCharIndex = 0;
CallumAlder 42:121148278dae 349 break;
CallumAlder 42:121148278dae 350 default:
CallumAlder 47:21bf4096faa1 351 _pc.printf("\r>%s< Unknown Error. Message: %x\n\r", _inCharQ, pMessage->message);
CallumAlder 42:121148278dae 352 break;
CallumAlder 42:121148278dae 353 }
CallumAlder 42:121148278dae 354
CallumAlder 47:21bf4096faa1 355
CallumAlder 47:21bf4096faa1 356
CallumAlder 47:21bf4096faa1 357 _msgStack.free(pMessage);
CallumAlder 42:121148278dae 358 }
CallumAlder 42:121148278dae 359 }
CallumAlder 42:121148278dae 360
CallumAlder 42:121148278dae 361 void putMessage(msgType type, uint32_t message){
CallumAlder 47:21bf4096faa1 362 msg *p_msg = _msgStack.alloc();
CallumAlder 42:121148278dae 363 p_msg->type = type;
CallumAlder 42:121148278dae 364 p_msg->message = message;
CallumAlder 47:21bf4096faa1 365 _msgStack.put(p_msg);
CallumAlder 42:121148278dae 366 }
CallumAlder 42:121148278dae 367
CallumAlder 42:121148278dae 368 void start_comm(){
CallumAlder 42:121148278dae 369 _RUN = true;
CallumAlder 47:21bf4096faa1 370 _tCommOut.start(callback(this, &Comm::commOutFn));
iachinweze1 23:ab1cb51527d1 371
CallumAlder 42:121148278dae 372 }
iachinweze1 23:ab1cb51527d1 373
CallumAlder 47:21bf4096faa1 374 char _newCmd[]; // Unallocated must be defined at the bottom of the class
CallumAlder 47:21bf4096faa1 375 static char _inCharQ[];
CallumAlder 42:121148278dae 376 };
CallumAlder 47:21bf4096faa1 377 char Comm::_inCharQ[] = {'.','.','.','.','.','.','.','.','.','.','.','.','.','.','.','.','.','\0'}; // Static member must be defined outside class
adehadd 46:b9081aa50bda 378
CallumAlder 42:121148278dae 379 class Motor {
adehadd 27:ce05fed3c1ea 380
CallumAlder 42:121148278dae 381 protected:
CallumAlder 47:21bf4096faa1 382 volatile int8_t orState, // Rotor offset at motor state 0, motor specific
CallumAlder 47:21bf4096faa1 383 currentState, // Current Rotor State
CallumAlder 47:21bf4096faa1 384 stateList[6], // All possible rotor states stored
CallumAlder 47:21bf4096faa1 385 lead; // Phase lead to make motor spin
CallumAlder 42:121148278dae 386
CallumAlder 47:21bf4096faa1 387 uint8_t theStates[3], // The Key states
CallumAlder 47:21bf4096faa1 388 stateCount[3]; // State Counter
CallumAlder 47:21bf4096faa1 389 uint32_t mtrPeriod, // Motor period
CallumAlder 47:21bf4096faa1 390 _MAXPWM_PRD;
CallumAlder 47:21bf4096faa1 391 float dutyC; // 1 = 100%
CallumAlder 47:21bf4096faa1 392 bool _RUN;
adehadd 27:ce05fed3c1ea 393
CallumAlder 47:21bf4096faa1 394 Comm* p_comm;
CallumAlder 47:21bf4096faa1 395 Thread t_motor_ctrl; // Thread for motor Control
CallumAlder 42:121148278dae 396
CallumAlder 42:121148278dae 397
CallumAlder 42:121148278dae 398 public:
iachinweze1 23:ab1cb51527d1 399
CallumAlder 47:21bf4096faa1 400 Motor() : t_motor_ctrl(osPriorityAboveNormal2, 1024){
CallumAlder 47:21bf4096faa1 401
CallumAlder 47:21bf4096faa1 402 dutyC = 1.0f; // Set Power to maximum to drive motorHome()
CallumAlder 47:21bf4096faa1 403 mtrPeriod = 2e3; // Motor period
CallumAlder 42:121148278dae 404 pwmCtrl.period_us(mtrPeriod);
CallumAlder 42:121148278dae 405 pwmCtrl.pulsewidth_us(mtrPeriod);
CallumAlder 42:121148278dae 406
CallumAlder 47:21bf4096faa1 407 orState = motorHome(); // Rotot offset at motor state 0
CallumAlder 47:21bf4096faa1 408 currentState = readRotorState(); // Current Rotor State
CallumAlder 47:21bf4096faa1 409 lead = 2; // 2 for forwards, -2 for backwards
adehadd 27:ce05fed3c1ea 410
CallumAlder 42:121148278dae 411 // It skips the origin state and it's 'lead' increments?
CallumAlder 42:121148278dae 412 theStates[0] = orState +1;
CallumAlder 42:121148278dae 413 theStates[1] = (orState + lead) % 6 +1;
CallumAlder 42:121148278dae 414 theStates[2] = (orState + (lead*2)) % 6 +1;
CallumAlder 42:121148278dae 415
CallumAlder 42:121148278dae 416 stateCount[0] = 0; stateCount[1] = 0; stateCount[2] = 0;
CallumAlder 42:121148278dae 417
CallumAlder 42:121148278dae 418 p_comm = NULL; // null pointer for now
CallumAlder 42:121148278dae 419 _RUN = false;
CallumAlder 42:121148278dae 420
CallumAlder 47:21bf4096faa1 421 _MAXPWM_PRD = 2e3;
CallumAlder 42:121148278dae 422
CallumAlder 42:121148278dae 423 }
adehadd 27:ce05fed3c1ea 424
iachinweze1 23:ab1cb51527d1 425
CallumAlder 42:121148278dae 426 void motorStart(Comm *comm) {
CallumAlder 42:121148278dae 427
CallumAlder 42:121148278dae 428 // Establish Photointerrupter Service Routines (auto choose next state)
CallumAlder 42:121148278dae 429 I1.fall(callback(this, &Motor::stateUpdate));
CallumAlder 42:121148278dae 430 I2.fall(callback(this, &Motor::stateUpdate));
CallumAlder 42:121148278dae 431 I3.fall(callback(this, &Motor::stateUpdate));
CallumAlder 42:121148278dae 432 I1.rise(callback(this, &Motor::stateUpdate));
CallumAlder 42:121148278dae 433 I2.rise(callback(this, &Motor::stateUpdate));
CallumAlder 42:121148278dae 434 I3.rise(callback(this, &Motor::stateUpdate));
CallumAlder 42:121148278dae 435
CallumAlder 42:121148278dae 436 // push digitally so if motor is static it will start moving
CallumAlder 42:121148278dae 437 motorOut((currentState-orState+lead+6)%6); // We push it digitally
CallumAlder 42:121148278dae 438
CallumAlder 42:121148278dae 439 // Default a lower duty cylce
CallumAlder 42:121148278dae 440 dutyC = 0.8;
CallumAlder 42:121148278dae 441 pwmCtrl.period_us((uint32_t)mtrPeriod);
CallumAlder 42:121148278dae 442 pwmCtrl.pulsewidth_us((uint32_t)mtrPeriod*dutyC);
CallumAlder 42:121148278dae 443
CallumAlder 42:121148278dae 444 p_comm = comm;
CallumAlder 42:121148278dae 445 _RUN = true;
CallumAlder 42:121148278dae 446
CallumAlder 42:121148278dae 447 // Start motor control thread
CallumAlder 42:121148278dae 448 t_motor_ctrl.start(callback(this, &Motor::motorCtrlFn));
CallumAlder 42:121148278dae 449
CallumAlder 43:a6d20109b2f2 450 p_comm->_pc.printf("origin=%i, theStates=[%i,%i,%i]\n\r", orState, theStates[0], theStates[1], theStates[2]);
CallumAlder 42:121148278dae 451
CallumAlder 42:121148278dae 452 }
CallumAlder 42:121148278dae 453
CallumAlder 42:121148278dae 454 //Set a given drive state
CallumAlder 42:121148278dae 455 void motorOut(int8_t driveState) {
iachinweze1 23:ab1cb51527d1 456
CallumAlder 42:121148278dae 457 //Lookup the output byte from the drive state.
CallumAlder 42:121148278dae 458 int8_t driveOut = driveTable[driveState & 0x07];
CallumAlder 42:121148278dae 459
CallumAlder 42:121148278dae 460 //Turn off first
CallumAlder 42:121148278dae 461 if (~driveOut & 0x01) L1L = 0;
CallumAlder 42:121148278dae 462 if (~driveOut & 0x02) L1H = 1;
CallumAlder 42:121148278dae 463 if (~driveOut & 0x04) L2L = 0;
CallumAlder 42:121148278dae 464 if (~driveOut & 0x08) L2H = 1;
CallumAlder 42:121148278dae 465 if (~driveOut & 0x10) L3L = 0;
CallumAlder 42:121148278dae 466 if (~driveOut & 0x20) L3H = 1;
CallumAlder 42:121148278dae 467
CallumAlder 42:121148278dae 468 //Then turn on
CallumAlder 42:121148278dae 469 if (driveOut & 0x01) L1L = 1;
CallumAlder 42:121148278dae 470 if (driveOut & 0x02) L1H = 0;
CallumAlder 42:121148278dae 471 if (driveOut & 0x04) L2L = 1;
CallumAlder 42:121148278dae 472 if (driveOut & 0x08) L2H = 0;
CallumAlder 42:121148278dae 473 if (driveOut & 0x10) L3L = 1;
CallumAlder 42:121148278dae 474 if (driveOut & 0x20) L3H = 0;
CallumAlder 42:121148278dae 475 }
CallumAlder 42:121148278dae 476
CallumAlder 42:121148278dae 477 //Convert photointerrupter inputs to a rotor state
CallumAlder 42:121148278dae 478 inline int8_t readRotorState() {
CallumAlder 42:121148278dae 479 return stateMap[I1 + 2*I2 + 4*I3];
CallumAlder 42:121148278dae 480 }
CallumAlder 42:121148278dae 481
CallumAlder 42:121148278dae 482 //Basic synchronisation routine
CallumAlder 42:121148278dae 483 int8_t motorHome() {
CallumAlder 42:121148278dae 484 //Put the motor in drive state 0 and wait for it to stabilise
CallumAlder 42:121148278dae 485 motorOut(0);
CallumAlder 42:121148278dae 486 wait(3.0);
CallumAlder 42:121148278dae 487
CallumAlder 42:121148278dae 488 //Get the rotor state
CallumAlder 42:121148278dae 489 return readRotorState();
CallumAlder 42:121148278dae 490 }
iachinweze1 23:ab1cb51527d1 491
adehadd 20:c60f4785b556 492
CallumAlder 42:121148278dae 493 void stateUpdate() { // () { // **params
CallumAlder 42:121148278dae 494 currentState = readRotorState();
adehadd 27:ce05fed3c1ea 495
CallumAlder 42:121148278dae 496 // Store into state counter
CallumAlder 42:121148278dae 497 if (currentState == theStates[0])
CallumAlder 42:121148278dae 498 stateCount[0]++;
CallumAlder 42:121148278dae 499 else if (currentState == theStates[1])
CallumAlder 42:121148278dae 500 stateCount[1]++;
CallumAlder 42:121148278dae 501 else if (currentState == theStates[2])
CallumAlder 42:121148278dae 502 stateCount[2]++;
adehadd 27:ce05fed3c1ea 503
adehadd 27:ce05fed3c1ea 504
CallumAlder 42:121148278dae 505 // (Current - Offset + lead + 6) %6
CallumAlder 42:121148278dae 506 motorOut((currentState - orState + lead + 6) % 6);
iachinweze1 23:ab1cb51527d1 507
CallumAlder 42:121148278dae 508 }
CallumAlder 19:805c87360b55 509
adehadd 27:ce05fed3c1ea 510
adehadd 20:c60f4785b556 511
CallumAlder 42:121148278dae 512 // attach_us -> runs funtion every 100ms
CallumAlder 42:121148278dae 513 void motorCtrlFn() {
CallumAlder 42:121148278dae 514 Ticker motorCtrlTicker;
CallumAlder 42:121148278dae 515 Timer m_timer;
CallumAlder 42:121148278dae 516 motorCtrlTicker.attach_us(callback(this,&Motor::motorCtrlTick), 1e5);
iachinweze1 23:ab1cb51527d1 517
CallumAlder 42:121148278dae 518 // Init some things
CallumAlder 42:121148278dae 519 uint8_t cpyStateCount[3];
CallumAlder 42:121148278dae 520 uint8_t cpyCurrentState;
CallumAlder 42:121148278dae 521 int8_t cpyModeBitfield;
CallumAlder 42:121148278dae 522
CallumAlder 47:21bf4096faa1 523 int32_t ting[2] = {6,1}; // 360,60 (for degrees), 5,1 (for states)
CallumAlder 42:121148278dae 524 uint8_t iterElementMax;
CallumAlder 42:121148278dae 525 int32_t totalDegrees;
CallumAlder 42:121148278dae 526 int32_t stateDiff;
adehadd 27:ce05fed3c1ea 527
CallumAlder 47:21bf4096faa1 528 int32_t cur_speed; // Variable for local velocity calculation
CallumAlder 47:21bf4096faa1 529 int32_t locMotorPos; // Local copy of motor position
CallumAlder 47:21bf4096faa1 530 volatile int32_t torque; // Local variable to set motor torque
CallumAlder 42:121148278dae 531 static int32_t oldTorque =0;
CallumAlder 47:21bf4096faa1 532 float sError, // Velocity error between target and reality
CallumAlder 47:21bf4096faa1 533 rError; // Rotation error between target and reality
CallumAlder 47:21bf4096faa1 534 static float rErrorOld; // Old rotation error used for calculation
CallumAlder 42:121148278dae 535
CallumAlder 42:121148278dae 536 //~~~Controller constants~~~~
CallumAlder 47:21bf4096faa1 537 int32_t Kp1=22; // Proportional controller constants
CallumAlder 47:21bf4096faa1 538 int32_t Kp2=22; // Calculated by trial and error to give optimal accuracy
CallumAlder 42:121148278dae 539 int32_t Ki = 12;
CallumAlder 42:121148278dae 540 float Kd=15.5;
CallumAlder 42:121148278dae 541
CallumAlder 42:121148278dae 542
CallumAlder 47:21bf4096faa1 543 int32_t Ys; // Initialise controller output Ys (s=speed)
CallumAlder 47:21bf4096faa1 544 int32_t Yr; // Initialise controller output Yr (r=rotations)
CallumAlder 42:121148278dae 545
CallumAlder 42:121148278dae 546 int32_t old_pos = 0;
adehadd 27:ce05fed3c1ea 547
CallumAlder 42:121148278dae 548 uint32_t cur_time = 0,
CallumAlder 42:121148278dae 549 old_time = 0,
CallumAlder 42:121148278dae 550 time_diff;
CallumAlder 42:121148278dae 551
CallumAlder 42:121148278dae 552 float cur_err = 0.0f,
CallumAlder 42:121148278dae 553 old_err = 0.0f,
CallumAlder 42:121148278dae 554 err_diff;
CallumAlder 42:121148278dae 555
CallumAlder 42:121148278dae 556 m_timer.start();
CallumAlder 42:121148278dae 557
CallumAlder 42:121148278dae 558 while (_RUN) {
CallumAlder 42:121148278dae 559 t_motor_ctrl.signal_wait((int32_t)0x1);
iachinweze1 23:ab1cb51527d1 560
CallumAlder 47:21bf4096faa1 561 core_util_critical_section_enter(); //Access shared variables here
CallumAlder 47:21bf4096faa1 562 cpyModeBitfield = p_comm->_modeBitField;
CallumAlder 47:21bf4096faa1 563 // p_comm->_modeBitField = 0; // nah
CallumAlder 47:21bf4096faa1 564 std::copy(stateCount, stateCount+3, cpyStateCount);
CallumAlder 47:21bf4096faa1 565 cpyCurrentState = currentState;
CallumAlder 47:21bf4096faa1 566 for (int i = 0; i < 3; ++i) {
CallumAlder 47:21bf4096faa1 567 stateCount[i] = 0;
CallumAlder 47:21bf4096faa1 568 }
CallumAlder 42:121148278dae 569 core_util_critical_section_exit();
adehadd 20:c60f4785b556 570
CallumAlder 42:121148278dae 571 // read state & timestamp
CallumAlder 42:121148278dae 572 cur_time = m_timer.read();
CallumAlder 42:121148278dae 573
CallumAlder 42:121148278dae 574 // compute speed
CallumAlder 42:121148278dae 575 time_diff = cur_time - old_time;
CallumAlder 42:121148278dae 576 // cur_speed = (cur_pos - old_pos) / time_diff;
adehadd 27:ce05fed3c1ea 577
CallumAlder 42:121148278dae 578 // prep values for next time through loop
CallumAlder 42:121148278dae 579 old_time = cur_time;
CallumAlder 42:121148278dae 580 old_pos = cpyCurrentState;
CallumAlder 42:121148278dae 581
CallumAlder 47:21bf4096faa1 582 // Hence we make the value positive,// and instead set the direction to the opposite one
CallumAlder 42:121148278dae 583
CallumAlder 42:121148278dae 584 iterElementMax = std::max_element(cpyStateCount, cpyStateCount+3) - cpyStateCount;
adehadd 20:c60f4785b556 585
CallumAlder 42:121148278dae 586 totalDegrees = ting[0] * cpyStateCount[iterElementMax];
CallumAlder 42:121148278dae 587 stateDiff = theStates[iterElementMax]-cpyCurrentState;
CallumAlder 47:21bf4096faa1 588 stateDiff >= 0 ? totalDegrees = totalDegrees + (ting[1]* stateDiff) : \
CallumAlder 47:21bf4096faa1 589 totalDegrees = totalDegrees + (ting[1]* stateDiff *-1);
CallumAlder 42:121148278dae 590
CallumAlder 47:21bf4096faa1 591 if ((cpyModeBitfield & 0x01)|(cpyModeBitfield & 0x02)) {// Speed, torque control and PID
CallumAlder 42:121148278dae 592 cur_speed = totalDegrees / time_diff;
CallumAlder 47:21bf4096faa1 593 sError = (p_comm->_targetVel * 6) - abs(cur_speed); // Read global variable _targetVel updated by interrupt and calculate error between target and reality
CallumAlder 42:121148278dae 594
CallumAlder 47:21bf4096faa1 595 // Ys = Kp * (s -|v|) where, // SPEED CONTROLLER
CallumAlder 47:21bf4096faa1 596 // Ys = controller output, Kp = prop controller constant, s = target velocity and v is the measured velocity
CallumAlder 42:121148278dae 597
CallumAlder 47:21bf4096faa1 598 // Check if user entered V0 and set the output to maximum as specified
CallumAlder 47:21bf4096faa1 599 sError == -abs(cur_speed) ? Ys = _MAXPWM_PRD : \
CallumAlder 47:21bf4096faa1 600 Ys = (Kp1 * sError); // If the user didn't enter V0 implement controller transfer function:
CallumAlder 47:21bf4096faa1 601
CallumAlder 47:21bf4096faa1 602
CallumAlder 47:21bf4096faa1 603 // Yr= Kp*Er + Kd* (dEr/dt) where, // ROTATION CONTROLLER
CallumAlder 47:21bf4096faa1 604 // Yr = controller output, Kp = prop controller constant, Er = error in number of rotations
CallumAlder 47:21bf4096faa1 605 rError = (p_comm->_targetRot)*6 - totalDegrees; // Read global variable _targetRot updated by interrupt and calculate the rotation error.
CallumAlder 47:21bf4096faa1 606 Yr = Kp2*rError + Kd*(rError - rErrorOld); // Implement controller transfer function
CallumAlder 47:21bf4096faa1 607 rErrorOld = rError; // Update rotation error
CallumAlder 47:21bf4096faa1 608
CallumAlder 47:21bf4096faa1 609 Ys = (int32_t)( Ys * sgn(rError) ); // Use the sign of the error to set controller wrt direction of rotation
CallumAlder 42:121148278dae 610
CallumAlder 47:21bf4096faa1 611 cur_speed < 0 ? torque = max(Ys, Yr): torque = min(Ys, Yr);
CallumAlder 47:21bf4096faa1 612
CallumAlder 47:21bf4096faa1 613 }
CallumAlder 47:21bf4096faa1 614 else if (cpyModeBitfield & 0x04) { // If it is in torque mode, do no PID math, just set pulsewidth
CallumAlder 47:21bf4096faa1 615 torque = (int32_t)p_comm->_motorTorque;
CallumAlder 47:21bf4096faa1 616 if (oldTorque != torque) {
CallumAlder 47:21bf4096faa1 617 p_comm->putMessage((Comm::msgType)8, torque);
CallumAlder 47:21bf4096faa1 618 oldTorque = torque;
CallumAlder 42:121148278dae 619 }
CallumAlder 47:21bf4096faa1 620 }
CallumAlder 47:21bf4096faa1 621 else{
CallumAlder 47:21bf4096faa1 622 torque = _MAXPWM_PRD * 0.5; // Run at 50% duty cycle if argument not properly defined
adehadd 27:ce05fed3c1ea 623
CallumAlder 42:121148278dae 624 }
adehadd 27:ce05fed3c1ea 625
CallumAlder 47:21bf4096faa1 626 torque < 0 ? lead = -2 : lead = +2;
CallumAlder 47:21bf4096faa1 627 torque = abs(torque);
adehadd 26:fb6151e5907d 628
CallumAlder 47:21bf4096faa1 629 if(torque > _MAXPWM_PRD) torque = _MAXPWM_PRD; // In case the calculated PWM is higher than our maximum 50% allowance,
CallumAlder 47:21bf4096faa1 630 // Set it to our max.
CallumAlder 47:21bf4096faa1 631 p_comm->_motorTorque = torque;
CallumAlder 47:21bf4096faa1 632 pwmCtrl.pulsewidth_us(torque);
CallumAlder 47:21bf4096faa1 633
CallumAlder 42:121148278dae 634 }
CallumAlder 42:121148278dae 635 }
CallumAlder 42:121148278dae 636
CallumAlder 42:121148278dae 637 void motorCtrlTick(){
CallumAlder 42:121148278dae 638 t_motor_ctrl.signal_set(0x1);
CallumAlder 42:121148278dae 639 }
CallumAlder 42:121148278dae 640 };
CallumAlder 42:121148278dae 641
CallumAlder 42:121148278dae 642
adehadd 27:ce05fed3c1ea 643 int main() {
adehadd 26:fb6151e5907d 644
CallumAlder 42:121148278dae 645 // Declare Objects
CallumAlder 42:121148278dae 646 Comm comm_port;
CallumAlder 42:121148278dae 647 SHA256 miner;
CallumAlder 42:121148278dae 648 Motor motor;
adehadd 27:ce05fed3c1ea 649
CallumAlder 42:121148278dae 650 // Start Motor and Comm Port
CallumAlder 42:121148278dae 651 motor.motorStart(&comm_port);
CallumAlder 42:121148278dae 652 comm_port.start_comm();
adehadd 27:ce05fed3c1ea 653
CallumAlder 42:121148278dae 654 // Declare Hash Variables
CallumAlder 47:21bf4096faa1 655 uint8_t sequence[] = {0x45,0x6D,0x62,0x65,0x64,0x64,0x65,0x64,
CallumAlder 47:21bf4096faa1 656 0x20,0x53,0x79,0x73,0x74,0x65,0x6D,0x73,
CallumAlder 47:21bf4096faa1 657 0x20,0x61,0x72,0x65,0x20,0x66,0x75,0x6E,
CallumAlder 47:21bf4096faa1 658 0x20,0x61,0x6E,0x64,0x20,0x64,0x6F,0x20,
CallumAlder 47:21bf4096faa1 659 0x61,0x77,0x65,0x73,0x6F,0x6D,0x65,0x20,
CallumAlder 47:21bf4096faa1 660 0x74,0x68,0x69,0x6E,0x67,0x73,0x21,0x20,
CallumAlder 47:21bf4096faa1 661 0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,
CallumAlder 47:21bf4096faa1 662 0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00};
CallumAlder 47:21bf4096faa1 663 uint8_t hash[32];
CallumAlder 47:21bf4096faa1 664 uint32_t length64 = 64;
CallumAlder 47:21bf4096faa1 665 uint32_t hashCounter = 0;
CallumAlder 47:21bf4096faa1 666 uint64_t* nonce = (uint64_t*)((int)sequence + 56);
adehadd 27:ce05fed3c1ea 667 uint64_t* key = (uint64_t*)((int)sequence + 48);
iachinweze1 23:ab1cb51527d1 668
CallumAlder 42:121148278dae 669 // Begin Main Timer
CallumAlder 42:121148278dae 670 Timer timer;
CallumAlder 42:121148278dae 671 timer.start();
adehadd 27:ce05fed3c1ea 672
CallumAlder 47:21bf4096faa1 673 // Loop Program
CallumAlder 42:121148278dae 674 while (1) {
adehadd 26:fb6151e5907d 675
CallumAlder 47:21bf4096faa1 676 //try{
CallumAlder 47:21bf4096faa1 677
CallumAlder 47:21bf4096faa1 678 // Mutex For Access Control
CallumAlder 47:21bf4096faa1 679 comm_port._newKeyMutex.lock();
CallumAlder 47:21bf4096faa1 680 *key = comm_port._newKey;
CallumAlder 47:21bf4096faa1 681 comm_port._newKeyMutex.unlock();
adehadd 20:c60f4785b556 682
CallumAlder 47:21bf4096faa1 683 // Compute Hash and Counter
CallumAlder 47:21bf4096faa1 684 miner.computeHash(hash, sequence, length64);
CallumAlder 47:21bf4096faa1 685 hashCounter++;
CallumAlder 47:21bf4096faa1 686
CallumAlder 47:21bf4096faa1 687 // Enum Casting and Condition
CallumAlder 47:21bf4096faa1 688 if (hash[0]==0 && hash[1]==0)
CallumAlder 47:21bf4096faa1 689 comm_port.putMessage((Comm::msgType)7, *nonce);
adehadd 20:c60f4785b556 690
CallumAlder 47:21bf4096faa1 691 // Try Nonce
CallumAlder 47:21bf4096faa1 692 (*nonce)++;
adehadd 26:fb6151e5907d 693
CallumAlder 47:21bf4096faa1 694 // Display via Comm Port
CallumAlder 47:21bf4096faa1 695 if (timer.read() >= 1){
CallumAlder 47:21bf4096faa1 696 comm_port.putMessage((Comm::msgType)5, hashCounter);
CallumAlder 47:21bf4096faa1 697 hashCounter=0;
CallumAlder 47:21bf4096faa1 698 timer.reset();
CallumAlder 47:21bf4096faa1 699 }
CallumAlder 47:21bf4096faa1 700 //}
adehadd 26:fb6151e5907d 701
CallumAlder 47:21bf4096faa1 702 //catch(...){
CallumAlder 47:21bf4096faa1 703 // break;
CallumAlder 47:21bf4096faa1 704 //}
CallumAlder 47:21bf4096faa1 705
CallumAlder 15:2f95f2fb68e3 706 }
CallumAlder 42:121148278dae 707
CallumAlder 42:121148278dae 708 return 0;
adehadd 27:ce05fed3c1ea 709 }