Skad00sh / Mbed OS ES_CW2_Dev

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

Dependencies:   Crypto

main.cpp@50:d1b983a0dd6f, 2019-03-22 (annotated)

Committer:
adehadd
Date:
Fri Mar 22 19:05:40 2019 +0000
Revision:
50:d1b983a0dd6f
Parent:
49:ae8dedfe2d0f
Child:
51:c03f63c6f930
The tune is working, not nice sounding, but it okay

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
adehadd 49:ae8dedfe2d0f 96 #ifndef MAXCMDLENGTH
adehadd 49:ae8dedfe2d0f 97 #define MAXCMDLENGTH 18
adehadd 49:ae8dedfe2d0f 98 #endif
adehadd 49:ae8dedfe2d0f 99
adehadd 49:ae8dedfe2d0f 100 #ifndef MAXCMDLENGTH_HALF
adehadd 49:ae8dedfe2d0f 101 #define MAXCMDLENGTH_HALF 9
adehadd 49:ae8dedfe2d0f 102 #endif
adehadd 49:ae8dedfe2d0f 103
CallumAlder 42:121148278dae 104 class Comm{
CallumAlder 42:121148278dae 105
CallumAlder 42:121148278dae 106 public:
estott 0:de4320f74764 107
CallumAlder 43:a6d20109b2f2 108 volatile bool _outMining;
CallumAlder 43:a6d20109b2f2 109 volatile float _targetVel, _targetRot;
adehadd 49:ae8dedfe2d0f 110 volatile char _notes[MAXCMDLENGTH_HALF]; // Array of actual _notes
estott 0:de4320f74764 111
CallumAlder 47:21bf4096faa1 112 volatile int8_t _modeBitField; // 0,0,0,... <=> Melody,Torque,Rotation,Velocity
CallumAlder 47:21bf4096faa1 113 const uint8_t _MAXCMDLENGTH; //
adehadd 50:d1b983a0dd6f 114 volatile uint8_t _inCharIndex, _cmdIndex, _noteRep,
adehadd 49:ae8dedfe2d0f 115 _noteDur[MAXCMDLENGTH_HALF],_noteLen; // Array of note durations
CallumAlder 47:21bf4096faa1 116 volatile uint32_t _motorTorque; // Motor Toque
CallumAlder 47:21bf4096faa1 117 volatile uint64_t _newKey; // hash key
adehadd 49:ae8dedfe2d0f 118
CallumAlder 48:b2afe48ced0d 119
adehadd 49:ae8dedfe2d0f 120 char _inCharQ[MAXCMDLENGTH],
adehadd 49:ae8dedfe2d0f 121 _newCmd[MAXCMDLENGTH];
adehadd 49:ae8dedfe2d0f 122
CallumAlder 47:21bf4096faa1 123 RawSerial _pc;
adehadd 49:ae8dedfe2d0f 124 Thread _tCommOut, _tCommIn;
adehadd 49:ae8dedfe2d0f 125 Mutex _newKeyMutex; // Restrict access to prevent deadlock.
CallumAlder 47:21bf4096faa1 126 bool _RUN;
CallumAlder 42:121148278dae 127
CallumAlder 48:b2afe48ced0d 128 enum msgType { mot_orState, posIn, velIn, posOut, velOut,
CallumAlder 42:121148278dae 129 hashRate, keyAdded, nonceMatch,
CallumAlder 43:a6d20109b2f2 130 torque, rotations, melody,
CallumAlder 42:121148278dae 131 error};
adehadd 27:ce05fed3c1ea 132
CallumAlder 47:21bf4096faa1 133 typedef struct { msgType type;
adehadd 49:ae8dedfe2d0f 134 uint32_t message;} msg; // TODO: Maybe add a thing that stores the newCmd message as well
adehadd 27:ce05fed3c1ea 135
CallumAlder 47:21bf4096faa1 136 Mail<msg, 32> _msgStack;
adehadd 49:ae8dedfe2d0f 137 Mail<bool,32> _msgReceived;
CallumAlder 47:21bf4096faa1 138
iachinweze1 23:ab1cb51527d1 139
CallumAlder 48:b2afe48ced0d 140 //-- Default Constructor With Inheritance From RawSerial Constructor ------------------------------------------------//
adehadd 49:ae8dedfe2d0f 141 Comm(): _pc(SERIAL_TX, SERIAL_RX), _tCommOut(osPriorityNormal, 1024), _tCommIn(osPriorityAboveNormal, 1024), _MAXCMDLENGTH(MAXCMDLENGTH){
adehadd 27:ce05fed3c1ea 142
CallumAlder 47:21bf4096faa1 143 _cmdIndex = 0;
CallumAlder 47:21bf4096faa1 144 _inCharIndex = 0;
adehadd 45:402a8a9423b9 145
CallumAlder 47:21bf4096faa1 146 _outMining = false;
CallumAlder 47:21bf4096faa1 147 _motorTorque = 300;
CallumAlder 47:21bf4096faa1 148 _targetRot = 459.0;
CallumAlder 47:21bf4096faa1 149 _targetVel = 45.0;
iachinweze1 23:ab1cb51527d1 150
CallumAlder 47:21bf4096faa1 151 _modeBitField = 0x01; // Default velocity mode
adehadd 45:402a8a9423b9 152
adehadd 49:ae8dedfe2d0f 153 _pc.printf("\n\r%s %d\n\r>", "Welcome", _MAXCMDLENGTH ); // Welcome
CallumAlder 47:21bf4096faa1 154 for (int i = 0; i < _MAXCMDLENGTH; ++i) // Reset buffer
CallumAlder 48:b2afe48ced0d 155 _inCharQ[i] = (char)'.'; // If a null terminator is printed Mbed prints 'Embedded Systems are fun and do awesome things!'
CallumAlder 42:121148278dae 156
CallumAlder 47:21bf4096faa1 157 _inCharQ[_MAXCMDLENGTH] = (char)'\0';
CallumAlder 48:b2afe48ced0d 158 sprintf(_inCharQ, "%s", _inCharQ); // Handling of the correct string
CallumAlder 47:21bf4096faa1 159 strncpy(_newCmd, _inCharQ, _MAXCMDLENGTH);
CallumAlder 19:805c87360b55 160
adehadd 49:ae8dedfe2d0f 161 _pc.printf("%s\n\r<", _inCharQ);
adehadd 49:ae8dedfe2d0f 162
CallumAlder 42:121148278dae 163 }
iachinweze1 23:ab1cb51527d1 164
adehadd 49:ae8dedfe2d0f 165 void commInFn() {
adehadd 49:ae8dedfe2d0f 166 _pc.attach(callback(this, &Comm::serialISR));
adehadd 49:ae8dedfe2d0f 167 char newChar;
CallumAlder 42:121148278dae 168
adehadd 49:ae8dedfe2d0f 169 while (_RUN) {
adehadd 49:ae8dedfe2d0f 170 osEvent newEvent = _msgReceived.get(); // Waits forever until it receives a thing
adehadd 49:ae8dedfe2d0f 171 _msgReceived.free((bool *)newEvent.value.p);
adehadd 49:ae8dedfe2d0f 172
CallumAlder 43:a6d20109b2f2 173 if (_inCharIndex == (_MAXCMDLENGTH)) {
CallumAlder 47:21bf4096faa1 174 _inCharQ[_MAXCMDLENGTH] = '\0'; // Force the string to have an end character
CallumAlder 42:121148278dae 175 putMessage(error, 1);
CallumAlder 47:21bf4096faa1 176 _inCharIndex = 0; // Reset buffer index
adehadd 27:ce05fed3c1ea 177 }
adehadd 27:ce05fed3c1ea 178 else{
adehadd 49:ae8dedfe2d0f 179 newChar = _inCharQ[_inCharIndex];
adehadd 49:ae8dedfe2d0f 180
adehadd 49:ae8dedfe2d0f 181 if(newChar != '\r'){ // While the command is not over,
CallumAlder 47:21bf4096faa1 182 _inCharIndex++; // Advance index
CallumAlder 43:a6d20109b2f2 183 _pc.putc(newChar);
CallumAlder 42:121148278dae 184 }
CallumAlder 42:121148278dae 185 else{
CallumAlder 47:21bf4096faa1 186 _inCharQ[_inCharIndex] = '\0'; // When the command is finally over,
CallumAlder 47:21bf4096faa1 187 strncpy(_newCmd, _inCharQ, _MAXCMDLENGTH); // Will copy 18 characters from _inCharQ to _newCmd
CallumAlder 42:121148278dae 188
CallumAlder 47:21bf4096faa1 189 for (int i = 0; i < _MAXCMDLENGTH; ++i) // Reset buffer
CallumAlder 47:21bf4096faa1 190 _inCharQ[i] = ' ';
adehadd 49:ae8dedfe2d0f 191
CallumAlder 47:21bf4096faa1 192 _inCharIndex = 0; // Reset index
adehadd 49:ae8dedfe2d0f 193 cmdParser();
adehadd 49:ae8dedfe2d0f 194
adehadd 49:ae8dedfe2d0f 195 // _tCommIn.signal_wait(0x01);
CallumAlder 42:121148278dae 196 }
adehadd 27:ce05fed3c1ea 197 }
adehadd 27:ce05fed3c1ea 198 }
adehadd 27:ce05fed3c1ea 199 }
CallumAlder 19:805c87360b55 200
adehadd 49:ae8dedfe2d0f 201 //-- Interrupt Service Routine for Serial Port and Character Queue Handling -----------------------------------------//
adehadd 49:ae8dedfe2d0f 202 void serialISR() {
adehadd 49:ae8dedfe2d0f 203 if (_pc.readable()) {
adehadd 49:ae8dedfe2d0f 204 char newChar = _pc.getc();
adehadd 49:ae8dedfe2d0f 205 _inCharQ[_inCharIndex] = newChar; // Save input character
adehadd 49:ae8dedfe2d0f 206
adehadd 49:ae8dedfe2d0f 207 bool *new_msg = _msgReceived.alloc();
adehadd 49:ae8dedfe2d0f 208 *new_msg = true;
adehadd 49:ae8dedfe2d0f 209 _msgReceived.put(new_msg);
adehadd 49:ae8dedfe2d0f 210 }
adehadd 49:ae8dedfe2d0f 211 }
adehadd 49:ae8dedfe2d0f 212
CallumAlder 48:b2afe48ced0d 213 //-- Reset Cursor Position ------------------------------------------------------------------------------------------//
CallumAlder 42:121148278dae 214 void returnCursor() {
CallumAlder 43:a6d20109b2f2 215 _pc.putc('>');
CallumAlder 43:a6d20109b2f2 216 for (int i = 0; i < _inCharIndex; ++i)
CallumAlder 47:21bf4096faa1 217 _pc.putc(_inCharQ[i]);
CallumAlder 42:121148278dae 218 }
CallumAlder 47:21bf4096faa1 219
CallumAlder 48:b2afe48ced0d 220 //-- Parse Incoming Data From Serial Port ---------------------------------------------------------------------------//
CallumAlder 48:b2afe48ced0d 221 void cmdParser() {
CallumAlder 47:21bf4096faa1 222 switch(_newCmd[0]) {
CallumAlder 47:21bf4096faa1 223 case 'K': // keyAdded
CallumAlder 47:21bf4096faa1 224 _newKeyMutex.lock(); // Ensure there is no deadlock
CallumAlder 47:21bf4096faa1 225 sscanf(_newCmd, "K%x", &_newKey); // Find desired the Key code
CallumAlder 47:21bf4096faa1 226 putMessage(keyAdded, _newKey); // Print it out
CallumAlder 43:a6d20109b2f2 227 _newKeyMutex.unlock();
CallumAlder 43:a6d20109b2f2 228 break;
CallumAlder 43:a6d20109b2f2 229
CallumAlder 47:21bf4096faa1 230 case 'V': // velIn
CallumAlder 47:21bf4096faa1 231 sscanf(_newCmd, "V%f", &_targetVel); // Find desired the target velocity
CallumAlder 47:21bf4096faa1 232 _modeBitField = 0x01; // Adjust bitfield pos 1
CallumAlder 47:21bf4096faa1 233 putMessage(velIn, _targetVel); // Print it out
CallumAlder 43:a6d20109b2f2 234 break;
CallumAlder 43:a6d20109b2f2 235
CallumAlder 47:21bf4096faa1 236 case 'R': // posIn
CallumAlder 47:21bf4096faa1 237 sscanf(_newCmd, "R%f", &_targetRot); // Find desired target rotation
CallumAlder 47:21bf4096faa1 238 _modeBitField = 0x02; // Adjust bitfield pos 2
CallumAlder 47:21bf4096faa1 239 putMessage(posIn, _targetRot); // Print it out
CallumAlder 42:121148278dae 240 break;
iachinweze1 23:ab1cb51527d1 241
CallumAlder 47:21bf4096faa1 242 case 'x': // Torque
CallumAlder 47:21bf4096faa1 243 sscanf(_newCmd, "x%u", &_motorTorque); // Find desired target torque
CallumAlder 47:21bf4096faa1 244 _modeBitField = 0x04; // Adjust bitfield pos 3
CallumAlder 47:21bf4096faa1 245 putMessage(torque, _motorTorque); // Print it out
adehadd 27:ce05fed3c1ea 246 break;
CallumAlder 42:121148278dae 247
CallumAlder 47:21bf4096faa1 248 case 'M': // Mining display toggle
CallumAlder 47:21bf4096faa1 249 int8_t miningTest;
CallumAlder 47:21bf4096faa1 250 sscanf(_newCmd, "M%d", &miningTest); // Display if input is 1
CallumAlder 47:21bf4096faa1 251 miningTest == 1 ? _outMining = true : _outMining = false;
CallumAlder 47:21bf4096faa1 252 break;
CallumAlder 47:21bf4096faa1 253
CallumAlder 47:21bf4096faa1 254 case 'T': // Play tune
CallumAlder 47:21bf4096faa1 255 regexTune() ? putMessage(melody, 1) : putMessage(error, 2);
CallumAlder 47:21bf4096faa1 256 break; // Break from case 'T'
CallumAlder 47:21bf4096faa1 257
CallumAlder 47:21bf4096faa1 258 default: // Break from switch
adehadd 27:ce05fed3c1ea 259 break;
adehadd 27:ce05fed3c1ea 260 }
adehadd 27:ce05fed3c1ea 261 }
adehadd 27:ce05fed3c1ea 262
CallumAlder 48:b2afe48ced0d 263 //-- Read In Note Data From Serial Port and Parse Into Class Variables ----------------------------------------------//
CallumAlder 47:21bf4096faa1 264 bool regexTune() {
CallumAlder 47:21bf4096faa1 265
CallumAlder 47:21bf4096faa1 266 uint8_t len = 0;
CallumAlder 47:21bf4096faa1 267
CallumAlder 47:21bf4096faa1 268 for (int i = 1; i < _MAXCMDLENGTH; ++i) // Find first #
CallumAlder 47:21bf4096faa1 269 if (_newCmd[i] == '#') {
CallumAlder 47:21bf4096faa1 270 len = i;
CallumAlder 47:21bf4096faa1 271 break; // Stop at first # found
CallumAlder 47:21bf4096faa1 272 }
CallumAlder 47:21bf4096faa1 273
CallumAlder 47:21bf4096faa1 274 if (len>0) { // Parse the input only if # found
adehadd 50:d1b983a0dd6f 275 uint8_t specLen = 2*(len+1) +1; // Add extra character for number of repeats, and +1 for the letter T
adehadd 50:d1b983a0dd6f 276 bool isChar = true; // After 'T' first is character note
adehadd 50:d1b983a0dd6f 277 char formatSpec[specLen];
CallumAlder 47:21bf4096faa1 278 formatSpec[0]='T';
adehadd 50:d1b983a0dd6f 279 for (int i = 1; i < specLen; i=i+2) { // Create a format spec based on length of input
CallumAlder 47:21bf4096faa1 280 formatSpec[i] = '%';
CallumAlder 47:21bf4096faa1 281 isChar ? formatSpec[i+1] = 'c' : \
CallumAlder 47:21bf4096faa1 282 formatSpec[i+1] = 'u' ;
CallumAlder 47:21bf4096faa1 283 isChar = !isChar;
CallumAlder 47:21bf4096faa1 284 }
CallumAlder 47:21bf4096faa1 285
adehadd 50:d1b983a0dd6f 286 formatSpec[specLen] = '\0';
CallumAlder 47:21bf4096faa1 287 sprintf(formatSpec, "%s", formatSpec); // Set string format correctly
CallumAlder 47:21bf4096faa1 288 // _pc.printf("%s\n", formatSpec );
CallumAlder 47:21bf4096faa1 289 sscanf(_newCmd, formatSpec, &_notes[0], &_noteDur[0],
adehadd 50:d1b983a0dd6f 290 &_notes[1], &_noteDur[1],
adehadd 50:d1b983a0dd6f 291 &_notes[2], &_noteDur[2],
adehadd 50:d1b983a0dd6f 292 &_notes[3], &_noteDur[3],
adehadd 50:d1b983a0dd6f 293 &_notes[4], &_noteDur[4],
adehadd 50:d1b983a0dd6f 294 &_notes[5], &_noteDur[5],
adehadd 50:d1b983a0dd6f 295 &_notes[6], &_noteDur[6],
adehadd 50:d1b983a0dd6f 296 &_notes[7], &_noteDur[7],
adehadd 50:d1b983a0dd6f 297 &_notes[8], &_noteDur[8]);
CallumAlder 47:21bf4096faa1 298
CallumAlder 47:21bf4096faa1 299
CallumAlder 47:21bf4096faa1 300 // Update _newCmd for putMessage print
CallumAlder 48:b2afe48ced0d 301 sprintf(_newCmd,formatSpec, _notes[0], _noteDur[0],\
CallumAlder 47:21bf4096faa1 302 _notes[1], _noteDur[1],\
CallumAlder 47:21bf4096faa1 303 _notes[2], _noteDur[2],\
CallumAlder 47:21bf4096faa1 304 _notes[3], _noteDur[3],\
CallumAlder 47:21bf4096faa1 305 _notes[4], _noteDur[4],\
CallumAlder 47:21bf4096faa1 306 _notes[5], _noteDur[5],\
CallumAlder 47:21bf4096faa1 307 _notes[6], _noteDur[6],\
CallumAlder 47:21bf4096faa1 308 _notes[7], _noteDur[7],\
CallumAlder 47:21bf4096faa1 309 _notes[8], _noteDur[8]);
adehadd 50:d1b983a0dd6f 310 _noteLen = (len-1)/2;
adehadd 50:d1b983a0dd6f 311 _modeBitField = 0x08;
adehadd 50:d1b983a0dd6f 312 _noteRep = _noteDur[(len-1)/2];
CallumAlder 47:21bf4096faa1 313 return true;
CallumAlder 47:21bf4096faa1 314 }
CallumAlder 47:21bf4096faa1 315 else {
CallumAlder 47:21bf4096faa1 316 return false;
CallumAlder 47:21bf4096faa1 317 }
CallumAlder 47:21bf4096faa1 318 }
CallumAlder 47:21bf4096faa1 319
CallumAlder 48:b2afe48ced0d 320 //-- Decode Messages to Print on Serial Port ------------------------------------------------------------------------//
CallumAlder 42:121148278dae 321 void commOutFn() {
CallumAlder 42:121148278dae 322 while (_RUN) {
CallumAlder 48:b2afe48ced0d 323
CallumAlder 47:21bf4096faa1 324 osEvent newEvent = _msgStack.get();
CallumAlder 42:121148278dae 325 msg *pMessage = (msg *) newEvent.value.p;
adehadd 27:ce05fed3c1ea 326
CallumAlder 47:21bf4096faa1 327 //Case switch to choose serial output based on incoming message enum
CallumAlder 42:121148278dae 328 switch (pMessage->type) {
CallumAlder 48:b2afe48ced0d 329 case mot_orState:
CallumAlder 47:21bf4096faa1 330 _pc.printf("\r>%s< The motor is currently in state %x\n\r", _inCharQ, pMessage->message);
CallumAlder 42:121148278dae 331 break;
CallumAlder 42:121148278dae 332 case hashRate:
CallumAlder 43:a6d20109b2f2 333 if (_outMining) {
CallumAlder 47:21bf4096faa1 334 _pc.printf("\r>%s< Mining: %.4u Hash/s\r", _inCharQ, (uint32_t) pMessage->message);
CallumAlder 42:121148278dae 335 returnCursor();
CallumAlder 43:a6d20109b2f2 336 _outMining = false;
CallumAlder 42:121148278dae 337 }
CallumAlder 42:121148278dae 338 break;
CallumAlder 42:121148278dae 339 case nonceMatch:
CallumAlder 47:21bf4096faa1 340 _pc.printf("\r>%s< Nonce found: %x\n\r", _inCharQ, pMessage->message);
CallumAlder 42:121148278dae 341 returnCursor();
CallumAlder 42:121148278dae 342 break;
CallumAlder 42:121148278dae 343 case keyAdded:
CallumAlder 47:21bf4096faa1 344 _pc.printf("\r>%s< New Key Added:\t0x%016x\n\r", _inCharQ, pMessage->message);
CallumAlder 42:121148278dae 345 break;
CallumAlder 42:121148278dae 346 case torque:
CallumAlder 47:21bf4096faa1 347 _pc.printf("\r>%s< Motor Torque set to:\t%d\n\r", _inCharQ, (int32_t) pMessage->message);
CallumAlder 42:121148278dae 348 break;
CallumAlder 42:121148278dae 349 case velIn:
CallumAlder 47:21bf4096faa1 350 _pc.printf("\r>%s< Target Velocity set to:\t%.2f\n\r", _inCharQ, _targetVel);
CallumAlder 42:121148278dae 351 break;
CallumAlder 42:121148278dae 352 case velOut:
CallumAlder 47:21bf4096faa1 353 _pc.printf("\r>%s< Current Velocity:\t%.2f States/sec\n\r", _inCharQ, (float) ((int32_t) pMessage->message));
CallumAlder 42:121148278dae 354 break;
CallumAlder 42:121148278dae 355 case posIn:
CallumAlder 47:21bf4096faa1 356 _pc.printf("\r>%s< Target # Rotations:\t%.2f\n\r", _inCharQ, (float) ((int32_t) pMessage->message));
CallumAlder 42:121148278dae 357 break;
CallumAlder 42:121148278dae 358 case posOut:
CallumAlder 47:21bf4096faa1 359 _pc.printf("\r>%s< Current Position:\t%.2f\n\r", _inCharQ, (float) ((int32_t) pMessage->message));
CallumAlder 42:121148278dae 360 break;
CallumAlder 47:21bf4096faa1 361 case melody:
CallumAlder 47:21bf4096faa1 362 _pc.printf("\r>%s< New Tune:\t%s\n\r", _inCharQ, _newCmd);
CallumAlder 47:21bf4096faa1 363 break;
CallumAlder 42:121148278dae 364 case error:
CallumAlder 47:21bf4096faa1 365 switch (pMessage->message) {
CallumAlder 47:21bf4096faa1 366 case 1:
CallumAlder 47:21bf4096faa1 367 _pc.printf("\r>%s< Error:%s\n\r", _inCharQ, "Overfull Buffer Reset" );
CallumAlder 47:21bf4096faa1 368 break;
CallumAlder 47:21bf4096faa1 369 case 2:
CallumAlder 47:21bf4096faa1 370 _pc.printf("\r>%s< Error:%s\n\r", _inCharQ, "Invalid Melody" );
CallumAlder 47:21bf4096faa1 371 default:
CallumAlder 47:21bf4096faa1 372 break;
CallumAlder 47:21bf4096faa1 373 }
CallumAlder 47:21bf4096faa1 374 for (int i = 0; i < _MAXCMDLENGTH; ++i) // reset buffer
CallumAlder 47:21bf4096faa1 375 _inCharQ[i] = ' ';
CallumAlder 47:21bf4096faa1 376
CallumAlder 47:21bf4096faa1 377 _inCharIndex = 0;
CallumAlder 42:121148278dae 378 break;
CallumAlder 42:121148278dae 379 default:
CallumAlder 47:21bf4096faa1 380 _pc.printf("\r>%s< Unknown Error. Message: %x\n\r", _inCharQ, pMessage->message);
CallumAlder 42:121148278dae 381 break;
CallumAlder 42:121148278dae 382 }
CallumAlder 42:121148278dae 383
CallumAlder 47:21bf4096faa1 384 _msgStack.free(pMessage);
CallumAlder 42:121148278dae 385 }
CallumAlder 42:121148278dae 386 }
CallumAlder 42:121148278dae 387
CallumAlder 48:b2afe48ced0d 388 //-- Put a Message On the Outgoing Message Stack --------------------------------------------------------------------//
CallumAlder 42:121148278dae 389 void putMessage(msgType type, uint32_t message){
CallumAlder 47:21bf4096faa1 390 msg *p_msg = _msgStack.alloc();
CallumAlder 42:121148278dae 391 p_msg->type = type;
CallumAlder 42:121148278dae 392 p_msg->message = message;
CallumAlder 47:21bf4096faa1 393 _msgStack.put(p_msg);
CallumAlder 42:121148278dae 394 }
CallumAlder 42:121148278dae 395
CallumAlder 48:b2afe48ced0d 396 //-- Attach CommOut Thread to the Outgoing Communication Function ---------------------------------------------------//
CallumAlder 42:121148278dae 397 void start_comm(){
CallumAlder 42:121148278dae 398 _RUN = true;
CallumAlder 47:21bf4096faa1 399 _tCommOut.start(callback(this, &Comm::commOutFn));
adehadd 49:ae8dedfe2d0f 400 _tCommIn.start(callback(this, &Comm::commInFn));
CallumAlder 42:121148278dae 401 }
iachinweze1 23:ab1cb51527d1 402
adehadd 49:ae8dedfe2d0f 403
CallumAlder 42:121148278dae 404 };
adehadd 49:ae8dedfe2d0f 405 // char Comm::_inCharQ[] = {'.','.','.','.','.','.','.','.','.','.','.','.','.','.','.','.','.','\0'}; // Static member must be defined outside class
CallumAlder 48:b2afe48ced0d 406 //Mutex Comm::_newKeyMutex;
adehadd 46:b9081aa50bda 407
CallumAlder 42:121148278dae 408 class Motor {
adehadd 27:ce05fed3c1ea 409
CallumAlder 42:121148278dae 410 protected:
CallumAlder 48:b2afe48ced0d 411 volatile int8_t _orState, // Rotor offset at motor state 0, motor specific
CallumAlder 48:b2afe48ced0d 412 _currentState, // Current Rotor State
CallumAlder 48:b2afe48ced0d 413 _stateList[6], // All possible rotor states stored
CallumAlder 48:b2afe48ced0d 414 _lead; // Phase _lead to make motor spin
CallumAlder 42:121148278dae 415
CallumAlder 48:b2afe48ced0d 416 uint8_t _theStates[3], // The Key states
CallumAlder 48:b2afe48ced0d 417 _stateCount[3]; // State Counter
CallumAlder 47:21bf4096faa1 418 uint32_t mtrPeriod, // Motor period
CallumAlder 47:21bf4096faa1 419 _MAXPWM_PRD;
CallumAlder 48:b2afe48ced0d 420 float _dutyC; // 1 = 100%
CallumAlder 47:21bf4096faa1 421 bool _RUN;
adehadd 27:ce05fed3c1ea 422
CallumAlder 48:b2afe48ced0d 423 Comm* _pComm;
CallumAlder 48:b2afe48ced0d 424 Thread _tMotorCtrl; // Thread for motor Control
CallumAlder 42:121148278dae 425
CallumAlder 42:121148278dae 426 public:
CallumAlder 48:b2afe48ced0d 427 //-- Default Constructor With Thread Object Constructor -------------------------------------------------------------//
CallumAlder 48:b2afe48ced0d 428 Motor() : _tMotorCtrl(osPriorityAboveNormal2, 1024){
CallumAlder 47:21bf4096faa1 429
CallumAlder 48:b2afe48ced0d 430 _dutyC = 1.0f; // Set Power to maximum to drive motorHome()
CallumAlder 48:b2afe48ced0d 431 mtrPeriod = 2e3; // Motor period
CallumAlder 48:b2afe48ced0d 432 pwmCtrl.period_us(mtrPeriod); // Initialise PWM
CallumAlder 42:121148278dae 433 pwmCtrl.pulsewidth_us(mtrPeriod);
CallumAlder 42:121148278dae 434
CallumAlder 48:b2afe48ced0d 435 _orState = motorHome(); // Rotor offset at motor state 0
adehadd 49:ae8dedfe2d0f 436 _currentState = readRotorState(); // Current Rotor State
CallumAlder 48:b2afe48ced0d 437 _lead = 2; // 2 for forwards, -2 for backwards
adehadd 27:ce05fed3c1ea 438
CallumAlder 48:b2afe48ced0d 439 _theStates[0] = _orState +1; // Initialise repeatable states, for us this is state 1
CallumAlder 48:b2afe48ced0d 440 _theStates[1] = (_orState + _lead) % 6 +1; // state 3
CallumAlder 48:b2afe48ced0d 441 _theStates[2] = (_orState + (_lead*2)) % 6 +1; // state 5
CallumAlder 42:121148278dae 442
CallumAlder 48:b2afe48ced0d 443 _stateCount[0] = 0; // Initialise
CallumAlder 48:b2afe48ced0d 444 _stateCount[1] = 0;
CallumAlder 48:b2afe48ced0d 445 _stateCount[2] = 0;
CallumAlder 42:121148278dae 446
CallumAlder 48:b2afe48ced0d 447 _pComm = NULL; // Initialise as NULL
CallumAlder 48:b2afe48ced0d 448 _RUN = false;
CallumAlder 42:121148278dae 449
CallumAlder 47:21bf4096faa1 450 _MAXPWM_PRD = 2e3;
CallumAlder 42:121148278dae 451
CallumAlder 42:121148278dae 452 }
adehadd 27:ce05fed3c1ea 453
CallumAlder 48:b2afe48ced0d 454 //-- Start Motor and Attach State Update Function to Rise/Fall Interrupts -------------------------------------------//
CallumAlder 42:121148278dae 455 void motorStart(Comm *comm) {
CallumAlder 42:121148278dae 456
CallumAlder 48:b2afe48ced0d 457 I1.fall(callback(this, &Motor::stateUpdate)); // Establish Photo Interrupter Service Routines (auto choose next state)
CallumAlder 42:121148278dae 458 I2.fall(callback(this, &Motor::stateUpdate));
CallumAlder 42:121148278dae 459 I3.fall(callback(this, &Motor::stateUpdate));
CallumAlder 42:121148278dae 460 I1.rise(callback(this, &Motor::stateUpdate));
CallumAlder 42:121148278dae 461 I2.rise(callback(this, &Motor::stateUpdate));
CallumAlder 42:121148278dae 462 I3.rise(callback(this, &Motor::stateUpdate));
CallumAlder 42:121148278dae 463
CallumAlder 48:b2afe48ced0d 464 motorOut((_currentState-_orState+_lead+6)%6); // Push digitally so static motor will start moving
CallumAlder 42:121148278dae 465
CallumAlder 48:b2afe48ced0d 466
CallumAlder 48:b2afe48ced0d 467 _dutyC = 0.8; // Default a lower duty cycle
CallumAlder 42:121148278dae 468 pwmCtrl.period_us((uint32_t)mtrPeriod);
adehadd 50:d1b983a0dd6f 469 pwmCtrl.pulsewidth_us((uint32_t)(mtrPeriod*_dutyC));
CallumAlder 42:121148278dae 470
CallumAlder 48:b2afe48ced0d 471 _pComm = comm;
CallumAlder 42:121148278dae 472 _RUN = true;
CallumAlder 42:121148278dae 473
CallumAlder 48:b2afe48ced0d 474 _tMotorCtrl.start(callback(this, &Motor::motorCtrlFn)); // Start motor control thread
CallumAlder 42:121148278dae 475
CallumAlder 48:b2afe48ced0d 476 _pComm->_pc.printf("origin=%i, _theStates=[%i,%i,%i]\n\r", _orState, _theStates[0], _theStates[1], _theStates[2]); // Print information to terminal
CallumAlder 42:121148278dae 477
CallumAlder 42:121148278dae 478 }
CallumAlder 42:121148278dae 479
CallumAlder 48:b2afe48ced0d 480 //-- Set a Predetermined Drive State -------------------------------------------------------------------------------//
CallumAlder 42:121148278dae 481 void motorOut(int8_t driveState) {
iachinweze1 23:ab1cb51527d1 482
CallumAlder 48:b2afe48ced0d 483 int8_t driveOut = driveTable[driveState & 0x07]; //Lookup the output byte from the drive state
CallumAlder 42:121148278dae 484
CallumAlder 48:b2afe48ced0d 485 if (~driveOut & 0x01) L1L = 0; //Turn off first
CallumAlder 42:121148278dae 486 if (~driveOut & 0x02) L1H = 1;
CallumAlder 42:121148278dae 487 if (~driveOut & 0x04) L2L = 0;
CallumAlder 42:121148278dae 488 if (~driveOut & 0x08) L2H = 1;
CallumAlder 42:121148278dae 489 if (~driveOut & 0x10) L3L = 0;
CallumAlder 42:121148278dae 490 if (~driveOut & 0x20) L3H = 1;
CallumAlder 42:121148278dae 491
CallumAlder 48:b2afe48ced0d 492 if (driveOut & 0x01) L1L = 1; //Then turn on
CallumAlder 48:b2afe48ced0d 493 if (driveOut & 0x02) L1H = 0;
CallumAlder 48:b2afe48ced0d 494 if (driveOut & 0x04) L2L = 1;
CallumAlder 48:b2afe48ced0d 495 if (driveOut & 0x08) L2H = 0;
CallumAlder 48:b2afe48ced0d 496 if (driveOut & 0x10) L3L = 1;
CallumAlder 48:b2afe48ced0d 497 if (driveOut & 0x20) L3H = 0;
CallumAlder 42:121148278dae 498 }
CallumAlder 42:121148278dae 499
CallumAlder 48:b2afe48ced0d 500 //-- Inline Conversion of Photointerrupts to a Rotor State ----------------------------------------------------------//
adehadd 49:ae8dedfe2d0f 501 inline int8_t readRotorState() {
CallumAlder 42:121148278dae 502 return stateMap[I1 + 2*I2 + 4*I3];
CallumAlder 42:121148278dae 503 }
CallumAlder 42:121148278dae 504
CallumAlder 48:b2afe48ced0d 505 //-- Basic Motor Stabilisation and Synchronisation ------------------------------------------------------------------//
CallumAlder 42:121148278dae 506 int8_t motorHome() {
CallumAlder 48:b2afe48ced0d 507
CallumAlder 48:b2afe48ced0d 508 motorOut(0); //Put the motor in drive state 0 and wait for it to stabilise
CallumAlder 42:121148278dae 509 wait(3.0);
CallumAlder 42:121148278dae 510
adehadd 49:ae8dedfe2d0f 511 return readRotorState(); //Get the rotor state
CallumAlder 42:121148278dae 512 }
iachinweze1 23:ab1cb51527d1 513
CallumAlder 48:b2afe48ced0d 514 //-- Motor State Log, Circumvents Issues From Occasionally Skipping Certain States ----------------------------------//
CallumAlder 42:121148278dae 515 void stateUpdate() { // () { // **params
adehadd 49:ae8dedfe2d0f 516 _currentState = readRotorState(); // Get current state
adehadd 27:ce05fed3c1ea 517
CallumAlder 48:b2afe48ced0d 518 if (_currentState == _theStates[0]) // Cumulative state counter (1 for our group's motor)
CallumAlder 48:b2afe48ced0d 519 _stateCount[0]++;
CallumAlder 48:b2afe48ced0d 520 else if (_currentState == _theStates[1]) // Cumulative state counter (3 for our group's motor)
CallumAlder 48:b2afe48ced0d 521 _stateCount[1]++;
CallumAlder 48:b2afe48ced0d 522 else if (_currentState == _theStates[2]) // Cumulative state counter (5 for our group's motor)
CallumAlder 48:b2afe48ced0d 523 _stateCount[2]++;
adehadd 27:ce05fed3c1ea 524
CallumAlder 48:b2afe48ced0d 525 motorOut((_currentState - _orState + _lead + 6) % 6); // Send the next state to the motor
iachinweze1 23:ab1cb51527d1 526
CallumAlder 42:121148278dae 527 }
CallumAlder 19:805c87360b55 528
CallumAlder 48:b2afe48ced0d 529 //-- Motor PID Control ---------------------------------------------------------------------------------------------//
CallumAlder 42:121148278dae 530 void motorCtrlFn() {
CallumAlder 42:121148278dae 531 Ticker motorCtrlTicker;
CallumAlder 42:121148278dae 532 Timer m_timer;
CallumAlder 42:121148278dae 533 motorCtrlTicker.attach_us(callback(this,&Motor::motorCtrlTick), 1e5);
iachinweze1 23:ab1cb51527d1 534
CallumAlder 42:121148278dae 535 // Init some things
CallumAlder 48:b2afe48ced0d 536 uint8_t cpyStateCount[3];
CallumAlder 48:b2afe48ced0d 537 uint8_t cpyCurrentState;
CallumAlder 48:b2afe48ced0d 538 int8_t cpyModeBitfield;
CallumAlder 42:121148278dae 539
CallumAlder 48:b2afe48ced0d 540 int32_t ting[2] = {6,1}; // 360,60 (for degrees), 5,1 (for states)
CallumAlder 48:b2afe48ced0d 541 uint8_t iterElementMax;
CallumAlder 48:b2afe48ced0d 542 int32_t totalDegrees;
CallumAlder 48:b2afe48ced0d 543 int32_t stateDiff;
adehadd 27:ce05fed3c1ea 544
CallumAlder 48:b2afe48ced0d 545 int32_t cur_speed; // Variable for local velocity calculation
CallumAlder 48:b2afe48ced0d 546 int32_t locMotorPos; // Local copy of motor position
CallumAlder 47:21bf4096faa1 547 volatile int32_t torque; // Local variable to set motor torque
CallumAlder 48:b2afe48ced0d 548 static int32_t oldTorque =0;
CallumAlder 48:b2afe48ced0d 549 float sError, // Velocity error between target and reality
CallumAlder 48:b2afe48ced0d 550 rError; // Rotation error between target and reality
CallumAlder 48:b2afe48ced0d 551 static float rErrorOld; // Old rotation error used for calculation
CallumAlder 42:121148278dae 552
CallumAlder 42:121148278dae 553 //~~~Controller constants~~~~
CallumAlder 48:b2afe48ced0d 554 int32_t Kp1=22; // Proportional controller constants
CallumAlder 48:b2afe48ced0d 555 int32_t Kp2=22; // Calculated by trial and error to give optimal accuracy
CallumAlder 48:b2afe48ced0d 556 int32_t Ki = 12;
CallumAlder 48:b2afe48ced0d 557 float Kd=15.5;
CallumAlder 42:121148278dae 558
CallumAlder 42:121148278dae 559
CallumAlder 48:b2afe48ced0d 560 int32_t Ts; // Initialise controller output Ts (s=speed)
CallumAlder 48:b2afe48ced0d 561 int32_t Tr; // Initialise controller output Tr (r=rotations)
CallumAlder 42:121148278dae 562
CallumAlder 48:b2afe48ced0d 563 int32_t old_pos = 0;
adehadd 27:ce05fed3c1ea 564
CallumAlder 48:b2afe48ced0d 565 uint32_t cur_time = 0,
CallumAlder 48:b2afe48ced0d 566 old_time = 0,
CallumAlder 48:b2afe48ced0d 567 time_diff;
CallumAlder 42:121148278dae 568
CallumAlder 48:b2afe48ced0d 569 float cur_err = 0.0f,
CallumAlder 48:b2afe48ced0d 570 old_err = 0.0f,
CallumAlder 48:b2afe48ced0d 571 err_diff;
CallumAlder 42:121148278dae 572
adehadd 50:d1b983a0dd6f 573
adehadd 50:d1b983a0dd6f 574 enum { // To nearest Hz
adehadd 50:d1b983a0dd6f 575 NOTE_C = 261, // C4
adehadd 50:d1b983a0dd6f 576 NOTE_D = 293, // D4
adehadd 50:d1b983a0dd6f 577 NOTE_E = 330, // E4
adehadd 50:d1b983a0dd6f 578 NOTE_F = 349, // F4
adehadd 50:d1b983a0dd6f 579 NOTE_G = 392, // G4
adehadd 50:d1b983a0dd6f 580 NOTE_A = 440, // A4
adehadd 50:d1b983a0dd6f 581 NOTE_B = 494 // B4
adehadd 50:d1b983a0dd6f 582 };
adehadd 50:d1b983a0dd6f 583
CallumAlder 42:121148278dae 584 m_timer.start();
CallumAlder 42:121148278dae 585
CallumAlder 42:121148278dae 586 while (_RUN) {
CallumAlder 48:b2afe48ced0d 587 _tMotorCtrl.signal_wait((int32_t)0x1);
adehadd 49:ae8dedfe2d0f 588 // _pComm->_tCommIn.signal_set(0x01);
iachinweze1 23:ab1cb51527d1 589
CallumAlder 48:b2afe48ced0d 590 core_util_critical_section_enter(); // Access shared variables here
CallumAlder 48:b2afe48ced0d 591 cpyModeBitfield = _pComm->_modeBitField;
CallumAlder 48:b2afe48ced0d 592 std::copy(_stateCount, _stateCount+3, cpyStateCount);
CallumAlder 48:b2afe48ced0d 593 cpyCurrentState = _currentState;
CallumAlder 47:21bf4096faa1 594 for (int i = 0; i < 3; ++i) {
CallumAlder 48:b2afe48ced0d 595 _stateCount[i] = 0;
CallumAlder 47:21bf4096faa1 596 }
CallumAlder 42:121148278dae 597 core_util_critical_section_exit();
adehadd 20:c60f4785b556 598
CallumAlder 48:b2afe48ced0d 599
CallumAlder 48:b2afe48ced0d 600 cur_time = m_timer.read(); // Read state & timestamp
CallumAlder 42:121148278dae 601
CallumAlder 42:121148278dae 602 time_diff = cur_time - old_time;
CallumAlder 42:121148278dae 603 // cur_speed = (cur_pos - old_pos) / time_diff;
CallumAlder 48:b2afe48ced0d 604
CallumAlder 48:b2afe48ced0d 605 old_time = cur_time; // prep values for next time through loop
CallumAlder 42:121148278dae 606 old_pos = cpyCurrentState;
CallumAlder 42:121148278dae 607
CallumAlder 47:21bf4096faa1 608 // Hence we make the value positive,// and instead set the direction to the opposite one
CallumAlder 42:121148278dae 609
CallumAlder 42:121148278dae 610 iterElementMax = std::max_element(cpyStateCount, cpyStateCount+3) - cpyStateCount;
adehadd 20:c60f4785b556 611
CallumAlder 42:121148278dae 612 totalDegrees = ting[0] * cpyStateCount[iterElementMax];
CallumAlder 48:b2afe48ced0d 613 stateDiff = _theStates[iterElementMax]-cpyCurrentState;
CallumAlder 47:21bf4096faa1 614 stateDiff >= 0 ? totalDegrees = totalDegrees + (ting[1]* stateDiff) : \
CallumAlder 47:21bf4096faa1 615 totalDegrees = totalDegrees + (ting[1]* stateDiff *-1);
CallumAlder 42:121148278dae 616
CallumAlder 47:21bf4096faa1 617 if ((cpyModeBitfield & 0x01)|(cpyModeBitfield & 0x02)) {// Speed, torque control and PID
CallumAlder 42:121148278dae 618 cur_speed = totalDegrees / time_diff;
CallumAlder 48:b2afe48ced0d 619 sError = (_pComm->_targetVel * 6) - abs(cur_speed); // Read global variable _targetVel updated by interrupt and calculate error between target and reality
CallumAlder 42:121148278dae 620
CallumAlder 48:b2afe48ced0d 621 // Ts = Kp * (s -|v|) where, // SPEED CONTROLLER
CallumAlder 48:b2afe48ced0d 622 // Ts = controller output, Kp = prop controller constant, s = target velocity and v is the measured velocity
CallumAlder 42:121148278dae 623
CallumAlder 47:21bf4096faa1 624 // Check if user entered V0 and set the output to maximum as specified
adehadd 49:ae8dedfe2d0f 625 sError == -abs(cur_speed) ? Ts = _MAXPWM_PRD : \
CallumAlder 48:b2afe48ced0d 626 Ts = (Kp1 * sError); // If the user didn't enter V0 implement controller transfer function:
CallumAlder 47:21bf4096faa1 627
CallumAlder 47:21bf4096faa1 628
CallumAlder 48:b2afe48ced0d 629 // Tr= Kp*Er + Kd* (dEr/dt) where, // ROTATION CONTROLLER
CallumAlder 48:b2afe48ced0d 630 // Tr = controller output, Kp = prop controller constant, Er = error in number of rotations
CallumAlder 48:b2afe48ced0d 631 rError = (_pComm->_targetRot)*6 - totalDegrees; // Read global variable _targetRot updated by interrupt and calculate the rotation error.
CallumAlder 48:b2afe48ced0d 632 Tr = Kp2*rError + Kd*(rError - rErrorOld); // Implement controller transfer function
CallumAlder 47:21bf4096faa1 633 rErrorOld = rError; // Update rotation error
CallumAlder 47:21bf4096faa1 634
CallumAlder 48:b2afe48ced0d 635 Ts = (int32_t)( Ts * sgn(rError) ); // Use the sign of the error to set controller wrt direction of rotation
CallumAlder 42:121148278dae 636
CallumAlder 48:b2afe48ced0d 637 cur_speed < 0 ? torque = max(Ts, Tr): torque = min(Ts, Tr);
CallumAlder 47:21bf4096faa1 638
CallumAlder 47:21bf4096faa1 639 }
CallumAlder 47:21bf4096faa1 640 else if (cpyModeBitfield & 0x04) { // If it is in torque mode, do no PID math, just set pulsewidth
CallumAlder 48:b2afe48ced0d 641 torque = (int32_t)_pComm->_motorTorque;
CallumAlder 47:21bf4096faa1 642 if (oldTorque != torque) {
CallumAlder 48:b2afe48ced0d 643 _pComm->putMessage((Comm::msgType)8, torque);
CallumAlder 47:21bf4096faa1 644 oldTorque = torque;
CallumAlder 42:121148278dae 645 }
CallumAlder 47:21bf4096faa1 646 }
adehadd 50:d1b983a0dd6f 647 else if (cpyModeBitfield & 0x08) { // If it is in Melody mode
adehadd 50:d1b983a0dd6f 648 uint32_t oldMtrPeriod = mtrPeriod; // Backup of current motor period
adehadd 50:d1b983a0dd6f 649 float oldDutyC = _dutyC; // Backup of current duty cycle
adehadd 50:d1b983a0dd6f 650 uint32_t noteFreq, newDur, newPeriod =0,
adehadd 50:d1b983a0dd6f 651 oldPeriod = mtrPeriod;
adehadd 50:d1b983a0dd6f 652
adehadd 50:d1b983a0dd6f 653 Timer testTimer;
adehadd 50:d1b983a0dd6f 654 testTimer.start();
adehadd 50:d1b983a0dd6f 655 _pComm->_pc.printf("rep:%i, len:%i \n\r", _pComm->_noteRep, _pComm->_noteLen);
adehadd 50:d1b983a0dd6f 656 for (int k = 0; k < _pComm->_noteRep; ++k) {
adehadd 50:d1b983a0dd6f 657
adehadd 50:d1b983a0dd6f 658 for (int i = 0; i < _pComm->_noteLen; ++i) {
adehadd 50:d1b983a0dd6f 659
adehadd 50:d1b983a0dd6f 660 noteFreq = 0;
adehadd 50:d1b983a0dd6f 661 newDur = (uint32_t)_pComm->_noteDur[i]*1e3; // ms
adehadd 50:d1b983a0dd6f 662
adehadd 50:d1b983a0dd6f 663 switch (_pComm->_notes[i]) {
adehadd 50:d1b983a0dd6f 664 case 'C':
adehadd 50:d1b983a0dd6f 665 noteFreq = NOTE_C;
adehadd 50:d1b983a0dd6f 666 break;
adehadd 50:d1b983a0dd6f 667 case 'D':
adehadd 50:d1b983a0dd6f 668 noteFreq = NOTE_D;
adehadd 50:d1b983a0dd6f 669 break;
adehadd 50:d1b983a0dd6f 670 case 'E':
adehadd 50:d1b983a0dd6f 671 noteFreq = NOTE_E;
adehadd 50:d1b983a0dd6f 672 break;
adehadd 50:d1b983a0dd6f 673 case 'F':
adehadd 50:d1b983a0dd6f 674 noteFreq = NOTE_F;
adehadd 50:d1b983a0dd6f 675 break;
adehadd 50:d1b983a0dd6f 676 case 'G':
adehadd 50:d1b983a0dd6f 677 noteFreq = NOTE_G;
adehadd 50:d1b983a0dd6f 678 break;
adehadd 50:d1b983a0dd6f 679 case 'A':
adehadd 50:d1b983a0dd6f 680 noteFreq = NOTE_A;
adehadd 50:d1b983a0dd6f 681 break;
adehadd 50:d1b983a0dd6f 682 case 'B':
adehadd 50:d1b983a0dd6f 683 noteFreq = NOTE_B;
adehadd 50:d1b983a0dd6f 684 break;
adehadd 50:d1b983a0dd6f 685 default:
adehadd 50:d1b983a0dd6f 686 break;
adehadd 50:d1b983a0dd6f 687 }
adehadd 50:d1b983a0dd6f 688
adehadd 50:d1b983a0dd6f 689 newPeriod = (uint32_t)(1e6/(uint32_t)noteFreq); // us
adehadd 50:d1b983a0dd6f 690
adehadd 50:d1b983a0dd6f 691 if (newPeriod>oldPeriod) { // Change Period First
adehadd 50:d1b983a0dd6f 692 _pComm->_pc.printf("Period:changed \n" );
adehadd 50:d1b983a0dd6f 693 pwmCtrl.period_us(newPeriod); //set frequency of PWM
adehadd 50:d1b983a0dd6f 694 pwmCtrl.pulsewidth_us((uint32_t)(newPeriod*0.8));
adehadd 50:d1b983a0dd6f 695 } else { // Change Pulse WidthFirst
adehadd 50:d1b983a0dd6f 696 pwmCtrl.pulsewidth_us((uint32_t)(newPeriod*0.8));
adehadd 50:d1b983a0dd6f 697 pwmCtrl.period_us(newPeriod); //set frequency of PWM
adehadd 50:d1b983a0dd6f 698 }
adehadd 50:d1b983a0dd6f 699 // stateUpdate();
adehadd 50:d1b983a0dd6f 700 oldPeriod = newPeriod;
adehadd 50:d1b983a0dd6f 701 testTimer.reset();
adehadd 50:d1b983a0dd6f 702 while (testTimer.read_ms() < newDur ) {} // Do nothing
adehadd 50:d1b983a0dd6f 703 _pComm->_pc.printf("Period:%d Dur:%d \n",newPeriod, newDur );
adehadd 50:d1b983a0dd6f 704 }
adehadd 50:d1b983a0dd6f 705
adehadd 50:d1b983a0dd6f 706 _pComm->_modeBitField = 0x04; // stop melody mode
adehadd 50:d1b983a0dd6f 707
adehadd 50:d1b983a0dd6f 708 }
adehadd 50:d1b983a0dd6f 709 // After playing notes, go back to the old speed
adehadd 50:d1b983a0dd6f 710 torque = (int32_t)_pComm->_motorTorque;
adehadd 50:d1b983a0dd6f 711 pwmCtrl.pulsewidth_us((uint32_t)(torque*0.8));
adehadd 50:d1b983a0dd6f 712 // And reset the motor period and duty cycle
adehadd 50:d1b983a0dd6f 713
adehadd 50:d1b983a0dd6f 714 }
CallumAlder 47:21bf4096faa1 715 else{
CallumAlder 47:21bf4096faa1 716 torque = _MAXPWM_PRD * 0.5; // Run at 50% duty cycle if argument not properly defined
adehadd 27:ce05fed3c1ea 717
CallumAlder 42:121148278dae 718 }
adehadd 27:ce05fed3c1ea 719
CallumAlder 48:b2afe48ced0d 720 torque < 0 ? _lead = -2 : _lead = +2;
CallumAlder 47:21bf4096faa1 721 torque = abs(torque);
adehadd 26:fb6151e5907d 722
CallumAlder 47:21bf4096faa1 723 if(torque > _MAXPWM_PRD) torque = _MAXPWM_PRD; // In case the calculated PWM is higher than our maximum 50% allowance,
CallumAlder 47:21bf4096faa1 724 // Set it to our max.
CallumAlder 48:b2afe48ced0d 725 _pComm->_motorTorque = torque;
CallumAlder 47:21bf4096faa1 726 pwmCtrl.pulsewidth_us(torque);
CallumAlder 47:21bf4096faa1 727
CallumAlder 42:121148278dae 728 }
CallumAlder 42:121148278dae 729 }
CallumAlder 42:121148278dae 730
CallumAlder 42:121148278dae 731 void motorCtrlTick(){
CallumAlder 48:b2afe48ced0d 732 _tMotorCtrl.signal_set(0x1);
CallumAlder 42:121148278dae 733 }
CallumAlder 42:121148278dae 734 };
CallumAlder 42:121148278dae 735
CallumAlder 42:121148278dae 736
adehadd 27:ce05fed3c1ea 737 int main() {
adehadd 26:fb6151e5907d 738
CallumAlder 42:121148278dae 739 // Declare Objects
CallumAlder 42:121148278dae 740 Comm comm_port;
CallumAlder 42:121148278dae 741 SHA256 miner;
CallumAlder 42:121148278dae 742 Motor motor;
adehadd 27:ce05fed3c1ea 743
CallumAlder 42:121148278dae 744 // Start Motor and Comm Port
CallumAlder 42:121148278dae 745 motor.motorStart(&comm_port);
CallumAlder 42:121148278dae 746 comm_port.start_comm();
adehadd 27:ce05fed3c1ea 747
CallumAlder 42:121148278dae 748 // Declare Hash Variables
CallumAlder 47:21bf4096faa1 749 uint8_t sequence[] = {0x45,0x6D,0x62,0x65,0x64,0x64,0x65,0x64,
CallumAlder 47:21bf4096faa1 750 0x20,0x53,0x79,0x73,0x74,0x65,0x6D,0x73,
CallumAlder 47:21bf4096faa1 751 0x20,0x61,0x72,0x65,0x20,0x66,0x75,0x6E,
CallumAlder 47:21bf4096faa1 752 0x20,0x61,0x6E,0x64,0x20,0x64,0x6F,0x20,
CallumAlder 47:21bf4096faa1 753 0x61,0x77,0x65,0x73,0x6F,0x6D,0x65,0x20,
CallumAlder 47:21bf4096faa1 754 0x74,0x68,0x69,0x6E,0x67,0x73,0x21,0x20,
CallumAlder 47:21bf4096faa1 755 0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,
CallumAlder 47:21bf4096faa1 756 0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00};
CallumAlder 47:21bf4096faa1 757 uint8_t hash[32];
CallumAlder 47:21bf4096faa1 758 uint32_t length64 = 64;
CallumAlder 47:21bf4096faa1 759 uint32_t hashCounter = 0;
CallumAlder 47:21bf4096faa1 760 uint64_t* nonce = (uint64_t*)((int)sequence + 56);
adehadd 27:ce05fed3c1ea 761 uint64_t* key = (uint64_t*)((int)sequence + 48);
iachinweze1 23:ab1cb51527d1 762
CallumAlder 42:121148278dae 763 // Begin Main Timer
CallumAlder 42:121148278dae 764 Timer timer;
CallumAlder 42:121148278dae 765 timer.start();
adehadd 27:ce05fed3c1ea 766
CallumAlder 47:21bf4096faa1 767 // Loop Program
CallumAlder 42:121148278dae 768 while (1) {
adehadd 26:fb6151e5907d 769
CallumAlder 47:21bf4096faa1 770 //try{
CallumAlder 47:21bf4096faa1 771
CallumAlder 47:21bf4096faa1 772 // Mutex For Access Control
adehadd 49:ae8dedfe2d0f 773 comm_port._newKeyMutex.lock();
CallumAlder 47:21bf4096faa1 774 *key = comm_port._newKey;
adehadd 49:ae8dedfe2d0f 775 comm_port._newKeyMutex.unlock();
adehadd 20:c60f4785b556 776
CallumAlder 47:21bf4096faa1 777 // Compute Hash and Counter
CallumAlder 47:21bf4096faa1 778 miner.computeHash(hash, sequence, length64);
CallumAlder 47:21bf4096faa1 779 hashCounter++;
CallumAlder 47:21bf4096faa1 780
CallumAlder 47:21bf4096faa1 781 // Enum Casting and Condition
CallumAlder 47:21bf4096faa1 782 if (hash[0]==0 && hash[1]==0)
CallumAlder 47:21bf4096faa1 783 comm_port.putMessage((Comm::msgType)7, *nonce);
adehadd 20:c60f4785b556 784
CallumAlder 47:21bf4096faa1 785 // Try Nonce
CallumAlder 47:21bf4096faa1 786 (*nonce)++;
adehadd 26:fb6151e5907d 787
CallumAlder 47:21bf4096faa1 788 // Display via Comm Port
CallumAlder 47:21bf4096faa1 789 if (timer.read() >= 1){
CallumAlder 47:21bf4096faa1 790 comm_port.putMessage((Comm::msgType)5, hashCounter);
CallumAlder 47:21bf4096faa1 791 hashCounter=0;
CallumAlder 47:21bf4096faa1 792 timer.reset();
CallumAlder 47:21bf4096faa1 793 }
CallumAlder 47:21bf4096faa1 794 //}
adehadd 26:fb6151e5907d 795
CallumAlder 47:21bf4096faa1 796 //catch(...){
CallumAlder 47:21bf4096faa1 797 // break;
CallumAlder 47:21bf4096faa1 798 //}
CallumAlder 47:21bf4096faa1 799
CallumAlder 15:2f95f2fb68e3 800 }
CallumAlder 42:121148278dae 801
CallumAlder 42:121148278dae 802 return 0;
adehadd 27:ce05fed3c1ea 803 }