Skad00sh / Mbed OS ES_CW2_Dev

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

Dependencies:   Crypto

main.cpp@51:c03f63c6f930, 2019-03-22 (annotated)

Committer:
adehadd
Date:
Fri Mar 22 21:58:48 2019 +0000
Revision:
51:c03f63c6f930
Parent:
50:d1b983a0dd6f
Child:
53:89d16b398615
ife commit

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 51:c03f63c6f930 89
adehadd 51:c03f63c6f930 90 //Encoder inputs
adehadd 51:c03f63c6f930 91 InterruptIn CHA(CHApin);
adehadd 51:c03f63c6f930 92 InterruptIn CHB(CHBpin);
adehadd 51:c03f63c6f930 93
adehadd 51:c03f63c6f930 94
adehadd 27:ce05fed3c1ea 95 //Drive state to output table
estott 0:de4320f74764 96 const int8_t driveTable[] = {0x12,0x18,0x09,0x21,0x24,0x06,0x00,0x00};
estott 2:4e88faab6988 97
adehadd 27:ce05fed3c1ea 98 //Mapping from interrupter inputs to sequential rotor states. 0x00 and 0x07 are not valid
CallumAlder 47:21bf4096faa1 99 const int8_t stateMap[] = {0x07,0x05,0x03,0x04,0x01,0x00,0x02,0x07};
CallumAlder 47:21bf4096faa1 100 //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 101
adehadd 49:ae8dedfe2d0f 102 #ifndef MAXCMDLENGTH
adehadd 49:ae8dedfe2d0f 103 #define MAXCMDLENGTH 18
adehadd 49:ae8dedfe2d0f 104 #endif
adehadd 49:ae8dedfe2d0f 105
adehadd 49:ae8dedfe2d0f 106 #ifndef MAXCMDLENGTH_HALF
adehadd 49:ae8dedfe2d0f 107 #define MAXCMDLENGTH_HALF 9
adehadd 49:ae8dedfe2d0f 108 #endif
adehadd 49:ae8dedfe2d0f 109
CallumAlder 42:121148278dae 110 class Comm{
CallumAlder 42:121148278dae 111
CallumAlder 42:121148278dae 112 public:
estott 0:de4320f74764 113
CallumAlder 43:a6d20109b2f2 114 volatile bool _outMining;
CallumAlder 43:a6d20109b2f2 115 volatile float _targetVel, _targetRot;
adehadd 49:ae8dedfe2d0f 116 volatile char _notes[MAXCMDLENGTH_HALF]; // Array of actual _notes
estott 0:de4320f74764 117
CallumAlder 47:21bf4096faa1 118 volatile int8_t _modeBitField; // 0,0,0,... <=> Melody,Torque,Rotation,Velocity
CallumAlder 47:21bf4096faa1 119 const uint8_t _MAXCMDLENGTH; //
adehadd 50:d1b983a0dd6f 120 volatile uint8_t _inCharIndex, _cmdIndex, _noteRep,
adehadd 49:ae8dedfe2d0f 121 _noteDur[MAXCMDLENGTH_HALF],_noteLen; // Array of note durations
CallumAlder 47:21bf4096faa1 122 volatile uint32_t _motorTorque; // Motor Toque
CallumAlder 47:21bf4096faa1 123 volatile uint64_t _newKey; // hash key
adehadd 49:ae8dedfe2d0f 124
adehadd 51:c03f63c6f930 125 volatile int32_t _motor_pos;
adehadd 51:c03f63c6f930 126
adehadd 49:ae8dedfe2d0f 127 char _inCharQ[MAXCMDLENGTH],
adehadd 49:ae8dedfe2d0f 128 _newCmd[MAXCMDLENGTH];
adehadd 49:ae8dedfe2d0f 129
CallumAlder 47:21bf4096faa1 130 RawSerial _pc;
adehadd 49:ae8dedfe2d0f 131 Thread _tCommOut, _tCommIn;
adehadd 49:ae8dedfe2d0f 132 Mutex _newKeyMutex; // Restrict access to prevent deadlock.
CallumAlder 47:21bf4096faa1 133 bool _RUN;
CallumAlder 42:121148278dae 134
CallumAlder 48:b2afe48ced0d 135 enum msgType { mot_orState, posIn, velIn, posOut, velOut,
CallumAlder 42:121148278dae 136 hashRate, keyAdded, nonceMatch,
CallumAlder 43:a6d20109b2f2 137 torque, rotations, melody,
CallumAlder 42:121148278dae 138 error};
adehadd 27:ce05fed3c1ea 139
CallumAlder 47:21bf4096faa1 140 typedef struct { msgType type;
adehadd 49:ae8dedfe2d0f 141 uint32_t message;} msg; // TODO: Maybe add a thing that stores the newCmd message as well
adehadd 27:ce05fed3c1ea 142
CallumAlder 47:21bf4096faa1 143 Mail<msg, 32> _msgStack;
adehadd 49:ae8dedfe2d0f 144 Mail<bool,32> _msgReceived;
CallumAlder 47:21bf4096faa1 145
iachinweze1 23:ab1cb51527d1 146
CallumAlder 48:b2afe48ced0d 147 //-- Default Constructor With Inheritance From RawSerial Constructor ------------------------------------------------//
adehadd 49:ae8dedfe2d0f 148 Comm(): _pc(SERIAL_TX, SERIAL_RX), _tCommOut(osPriorityNormal, 1024), _tCommIn(osPriorityAboveNormal, 1024), _MAXCMDLENGTH(MAXCMDLENGTH){
adehadd 27:ce05fed3c1ea 149
CallumAlder 47:21bf4096faa1 150 _cmdIndex = 0;
CallumAlder 47:21bf4096faa1 151 _inCharIndex = 0;
adehadd 45:402a8a9423b9 152
CallumAlder 47:21bf4096faa1 153 _outMining = false;
CallumAlder 47:21bf4096faa1 154 _motorTorque = 300;
CallumAlder 47:21bf4096faa1 155 _targetRot = 459.0;
CallumAlder 47:21bf4096faa1 156 _targetVel = 45.0;
iachinweze1 23:ab1cb51527d1 157
adehadd 51:c03f63c6f930 158 _motor_pos = 0;
adehadd 51:c03f63c6f930 159
adehadd 51:c03f63c6f930 160
CallumAlder 47:21bf4096faa1 161 _modeBitField = 0x01; // Default velocity mode
adehadd 45:402a8a9423b9 162
adehadd 49:ae8dedfe2d0f 163 _pc.printf("\n\r%s %d\n\r>", "Welcome", _MAXCMDLENGTH ); // Welcome
CallumAlder 47:21bf4096faa1 164 for (int i = 0; i < _MAXCMDLENGTH; ++i) // Reset buffer
CallumAlder 48:b2afe48ced0d 165 _inCharQ[i] = (char)'.'; // If a null terminator is printed Mbed prints 'Embedded Systems are fun and do awesome things!'
CallumAlder 42:121148278dae 166
CallumAlder 47:21bf4096faa1 167 _inCharQ[_MAXCMDLENGTH] = (char)'\0';
CallumAlder 48:b2afe48ced0d 168 sprintf(_inCharQ, "%s", _inCharQ); // Handling of the correct string
CallumAlder 47:21bf4096faa1 169 strncpy(_newCmd, _inCharQ, _MAXCMDLENGTH);
CallumAlder 19:805c87360b55 170
adehadd 49:ae8dedfe2d0f 171 _pc.printf("%s\n\r<", _inCharQ);
adehadd 49:ae8dedfe2d0f 172
CallumAlder 42:121148278dae 173 }
iachinweze1 23:ab1cb51527d1 174
adehadd 49:ae8dedfe2d0f 175 void commInFn() {
adehadd 49:ae8dedfe2d0f 176 _pc.attach(callback(this, &Comm::serialISR));
adehadd 49:ae8dedfe2d0f 177 char newChar;
CallumAlder 42:121148278dae 178
adehadd 49:ae8dedfe2d0f 179 while (_RUN) {
adehadd 49:ae8dedfe2d0f 180 osEvent newEvent = _msgReceived.get(); // Waits forever until it receives a thing
adehadd 49:ae8dedfe2d0f 181 _msgReceived.free((bool *)newEvent.value.p);
adehadd 49:ae8dedfe2d0f 182
CallumAlder 43:a6d20109b2f2 183 if (_inCharIndex == (_MAXCMDLENGTH)) {
CallumAlder 47:21bf4096faa1 184 _inCharQ[_MAXCMDLENGTH] = '\0'; // Force the string to have an end character
CallumAlder 42:121148278dae 185 putMessage(error, 1);
CallumAlder 47:21bf4096faa1 186 _inCharIndex = 0; // Reset buffer index
adehadd 27:ce05fed3c1ea 187 }
adehadd 27:ce05fed3c1ea 188 else{
adehadd 49:ae8dedfe2d0f 189 newChar = _inCharQ[_inCharIndex];
adehadd 49:ae8dedfe2d0f 190
adehadd 49:ae8dedfe2d0f 191 if(newChar != '\r'){ // While the command is not over,
CallumAlder 47:21bf4096faa1 192 _inCharIndex++; // Advance index
CallumAlder 43:a6d20109b2f2 193 _pc.putc(newChar);
CallumAlder 42:121148278dae 194 }
CallumAlder 42:121148278dae 195 else{
CallumAlder 47:21bf4096faa1 196 _inCharQ[_inCharIndex] = '\0'; // When the command is finally over,
CallumAlder 47:21bf4096faa1 197 strncpy(_newCmd, _inCharQ, _MAXCMDLENGTH); // Will copy 18 characters from _inCharQ to _newCmd
CallumAlder 42:121148278dae 198
CallumAlder 47:21bf4096faa1 199 for (int i = 0; i < _MAXCMDLENGTH; ++i) // Reset buffer
CallumAlder 47:21bf4096faa1 200 _inCharQ[i] = ' ';
adehadd 49:ae8dedfe2d0f 201
CallumAlder 47:21bf4096faa1 202 _inCharIndex = 0; // Reset index
adehadd 49:ae8dedfe2d0f 203 cmdParser();
adehadd 49:ae8dedfe2d0f 204
adehadd 49:ae8dedfe2d0f 205 // _tCommIn.signal_wait(0x01);
CallumAlder 42:121148278dae 206 }
adehadd 27:ce05fed3c1ea 207 }
adehadd 27:ce05fed3c1ea 208 }
adehadd 27:ce05fed3c1ea 209 }
CallumAlder 19:805c87360b55 210
adehadd 49:ae8dedfe2d0f 211 //-- Interrupt Service Routine for Serial Port and Character Queue Handling -----------------------------------------//
adehadd 49:ae8dedfe2d0f 212 void serialISR() {
adehadd 49:ae8dedfe2d0f 213 if (_pc.readable()) {
adehadd 49:ae8dedfe2d0f 214 char newChar = _pc.getc();
adehadd 49:ae8dedfe2d0f 215 _inCharQ[_inCharIndex] = newChar; // Save input character
adehadd 49:ae8dedfe2d0f 216
adehadd 49:ae8dedfe2d0f 217 bool *new_msg = _msgReceived.alloc();
adehadd 49:ae8dedfe2d0f 218 *new_msg = true;
adehadd 49:ae8dedfe2d0f 219 _msgReceived.put(new_msg);
adehadd 49:ae8dedfe2d0f 220 }
adehadd 49:ae8dedfe2d0f 221 }
adehadd 49:ae8dedfe2d0f 222
CallumAlder 48:b2afe48ced0d 223 //-- Reset Cursor Position ------------------------------------------------------------------------------------------//
CallumAlder 42:121148278dae 224 void returnCursor() {
CallumAlder 43:a6d20109b2f2 225 _pc.putc('>');
CallumAlder 43:a6d20109b2f2 226 for (int i = 0; i < _inCharIndex; ++i)
CallumAlder 47:21bf4096faa1 227 _pc.putc(_inCharQ[i]);
CallumAlder 42:121148278dae 228 }
CallumAlder 47:21bf4096faa1 229
CallumAlder 48:b2afe48ced0d 230 //-- Parse Incoming Data From Serial Port ---------------------------------------------------------------------------//
CallumAlder 48:b2afe48ced0d 231 void cmdParser() {
CallumAlder 47:21bf4096faa1 232 switch(_newCmd[0]) {
CallumAlder 47:21bf4096faa1 233 case 'K': // keyAdded
CallumAlder 47:21bf4096faa1 234 _newKeyMutex.lock(); // Ensure there is no deadlock
CallumAlder 47:21bf4096faa1 235 sscanf(_newCmd, "K%x", &_newKey); // Find desired the Key code
CallumAlder 47:21bf4096faa1 236 putMessage(keyAdded, _newKey); // Print it out
CallumAlder 43:a6d20109b2f2 237 _newKeyMutex.unlock();
CallumAlder 43:a6d20109b2f2 238 break;
CallumAlder 43:a6d20109b2f2 239
CallumAlder 47:21bf4096faa1 240 case 'V': // velIn
CallumAlder 47:21bf4096faa1 241 sscanf(_newCmd, "V%f", &_targetVel); // Find desired the target velocity
CallumAlder 47:21bf4096faa1 242 _modeBitField = 0x01; // Adjust bitfield pos 1
adehadd 51:c03f63c6f930 243 _motor_pos = 0;
CallumAlder 47:21bf4096faa1 244 putMessage(velIn, _targetVel); // Print it out
CallumAlder 43:a6d20109b2f2 245 break;
CallumAlder 43:a6d20109b2f2 246
CallumAlder 47:21bf4096faa1 247 case 'R': // posIn
CallumAlder 47:21bf4096faa1 248 sscanf(_newCmd, "R%f", &_targetRot); // Find desired target rotation
CallumAlder 47:21bf4096faa1 249 _modeBitField = 0x02; // Adjust bitfield pos 2
adehadd 51:c03f63c6f930 250 _targetVel = 2e3;
adehadd 51:c03f63c6f930 251 _motor_pos = 0;
CallumAlder 47:21bf4096faa1 252 putMessage(posIn, _targetRot); // Print it out
CallumAlder 42:121148278dae 253 break;
iachinweze1 23:ab1cb51527d1 254
CallumAlder 47:21bf4096faa1 255 case 'x': // Torque
CallumAlder 47:21bf4096faa1 256 sscanf(_newCmd, "x%u", &_motorTorque); // Find desired target torque
CallumAlder 47:21bf4096faa1 257 _modeBitField = 0x04; // Adjust bitfield pos 3
CallumAlder 47:21bf4096faa1 258 putMessage(torque, _motorTorque); // Print it out
adehadd 27:ce05fed3c1ea 259 break;
CallumAlder 42:121148278dae 260
CallumAlder 47:21bf4096faa1 261 case 'M': // Mining display toggle
CallumAlder 47:21bf4096faa1 262 int8_t miningTest;
CallumAlder 47:21bf4096faa1 263 sscanf(_newCmd, "M%d", &miningTest); // Display if input is 1
CallumAlder 47:21bf4096faa1 264 miningTest == 1 ? _outMining = true : _outMining = false;
CallumAlder 47:21bf4096faa1 265 break;
CallumAlder 47:21bf4096faa1 266
CallumAlder 47:21bf4096faa1 267 case 'T': // Play tune
CallumAlder 47:21bf4096faa1 268 regexTune() ? putMessage(melody, 1) : putMessage(error, 2);
CallumAlder 47:21bf4096faa1 269 break; // Break from case 'T'
CallumAlder 47:21bf4096faa1 270
CallumAlder 47:21bf4096faa1 271 default: // Break from switch
adehadd 27:ce05fed3c1ea 272 break;
adehadd 27:ce05fed3c1ea 273 }
adehadd 27:ce05fed3c1ea 274 }
adehadd 27:ce05fed3c1ea 275
CallumAlder 48:b2afe48ced0d 276 //-- Read In Note Data From Serial Port and Parse Into Class Variables ----------------------------------------------//
CallumAlder 47:21bf4096faa1 277 bool regexTune() {
CallumAlder 47:21bf4096faa1 278
CallumAlder 47:21bf4096faa1 279 uint8_t len = 0;
CallumAlder 47:21bf4096faa1 280
CallumAlder 47:21bf4096faa1 281 for (int i = 1; i < _MAXCMDLENGTH; ++i) // Find first #
CallumAlder 47:21bf4096faa1 282 if (_newCmd[i] == '#') {
CallumAlder 47:21bf4096faa1 283 len = i;
CallumAlder 47:21bf4096faa1 284 break; // Stop at first # found
CallumAlder 47:21bf4096faa1 285 }
CallumAlder 47:21bf4096faa1 286
CallumAlder 47:21bf4096faa1 287 if (len>0) { // Parse the input only if # found
adehadd 50:d1b983a0dd6f 288 uint8_t specLen = 2*(len+1) +1; // Add extra character for number of repeats, and +1 for the letter T
adehadd 50:d1b983a0dd6f 289 bool isChar = true; // After 'T' first is character note
adehadd 50:d1b983a0dd6f 290 char formatSpec[specLen];
CallumAlder 47:21bf4096faa1 291 formatSpec[0]='T';
adehadd 50:d1b983a0dd6f 292 for (int i = 1; i < specLen; i=i+2) { // Create a format spec based on length of input
CallumAlder 47:21bf4096faa1 293 formatSpec[i] = '%';
CallumAlder 47:21bf4096faa1 294 isChar ? formatSpec[i+1] = 'c' : \
CallumAlder 47:21bf4096faa1 295 formatSpec[i+1] = 'u' ;
CallumAlder 47:21bf4096faa1 296 isChar = !isChar;
CallumAlder 47:21bf4096faa1 297 }
CallumAlder 47:21bf4096faa1 298
adehadd 50:d1b983a0dd6f 299 formatSpec[specLen] = '\0';
CallumAlder 47:21bf4096faa1 300 sprintf(formatSpec, "%s", formatSpec); // Set string format correctly
CallumAlder 47:21bf4096faa1 301 // _pc.printf("%s\n", formatSpec );
CallumAlder 47:21bf4096faa1 302 sscanf(_newCmd, formatSpec, &_notes[0], &_noteDur[0],
adehadd 50:d1b983a0dd6f 303 &_notes[1], &_noteDur[1],
adehadd 50:d1b983a0dd6f 304 &_notes[2], &_noteDur[2],
adehadd 50:d1b983a0dd6f 305 &_notes[3], &_noteDur[3],
adehadd 50:d1b983a0dd6f 306 &_notes[4], &_noteDur[4],
adehadd 50:d1b983a0dd6f 307 &_notes[5], &_noteDur[5],
adehadd 50:d1b983a0dd6f 308 &_notes[6], &_noteDur[6],
adehadd 50:d1b983a0dd6f 309 &_notes[7], &_noteDur[7],
adehadd 50:d1b983a0dd6f 310 &_notes[8], &_noteDur[8]);
CallumAlder 47:21bf4096faa1 311
CallumAlder 47:21bf4096faa1 312
CallumAlder 47:21bf4096faa1 313 // Update _newCmd for putMessage print
CallumAlder 48:b2afe48ced0d 314 sprintf(_newCmd,formatSpec, _notes[0], _noteDur[0],\
CallumAlder 47:21bf4096faa1 315 _notes[1], _noteDur[1],\
CallumAlder 47:21bf4096faa1 316 _notes[2], _noteDur[2],\
CallumAlder 47:21bf4096faa1 317 _notes[3], _noteDur[3],\
CallumAlder 47:21bf4096faa1 318 _notes[4], _noteDur[4],\
CallumAlder 47:21bf4096faa1 319 _notes[5], _noteDur[5],\
CallumAlder 47:21bf4096faa1 320 _notes[6], _noteDur[6],\
CallumAlder 47:21bf4096faa1 321 _notes[7], _noteDur[7],\
CallumAlder 47:21bf4096faa1 322 _notes[8], _noteDur[8]);
adehadd 50:d1b983a0dd6f 323 _noteLen = (len-1)/2;
adehadd 50:d1b983a0dd6f 324 _modeBitField = 0x08;
adehadd 50:d1b983a0dd6f 325 _noteRep = _noteDur[(len-1)/2];
CallumAlder 47:21bf4096faa1 326 return true;
CallumAlder 47:21bf4096faa1 327 }
CallumAlder 47:21bf4096faa1 328 else {
CallumAlder 47:21bf4096faa1 329 return false;
CallumAlder 47:21bf4096faa1 330 }
CallumAlder 47:21bf4096faa1 331 }
CallumAlder 47:21bf4096faa1 332
CallumAlder 48:b2afe48ced0d 333 //-- Decode Messages to Print on Serial Port ------------------------------------------------------------------------//
CallumAlder 42:121148278dae 334 void commOutFn() {
CallumAlder 42:121148278dae 335 while (_RUN) {
CallumAlder 48:b2afe48ced0d 336
CallumAlder 47:21bf4096faa1 337 osEvent newEvent = _msgStack.get();
CallumAlder 42:121148278dae 338 msg *pMessage = (msg *) newEvent.value.p;
adehadd 27:ce05fed3c1ea 339
CallumAlder 47:21bf4096faa1 340 //Case switch to choose serial output based on incoming message enum
CallumAlder 42:121148278dae 341 switch (pMessage->type) {
CallumAlder 48:b2afe48ced0d 342 case mot_orState:
CallumAlder 47:21bf4096faa1 343 _pc.printf("\r>%s< The motor is currently in state %x\n\r", _inCharQ, pMessage->message);
CallumAlder 42:121148278dae 344 break;
CallumAlder 42:121148278dae 345 case hashRate:
CallumAlder 43:a6d20109b2f2 346 if (_outMining) {
CallumAlder 47:21bf4096faa1 347 _pc.printf("\r>%s< Mining: %.4u Hash/s\r", _inCharQ, (uint32_t) pMessage->message);
CallumAlder 42:121148278dae 348 returnCursor();
CallumAlder 43:a6d20109b2f2 349 _outMining = false;
CallumAlder 42:121148278dae 350 }
CallumAlder 42:121148278dae 351 break;
CallumAlder 42:121148278dae 352 case nonceMatch:
CallumAlder 47:21bf4096faa1 353 _pc.printf("\r>%s< Nonce found: %x\n\r", _inCharQ, pMessage->message);
CallumAlder 42:121148278dae 354 returnCursor();
CallumAlder 42:121148278dae 355 break;
CallumAlder 42:121148278dae 356 case keyAdded:
CallumAlder 47:21bf4096faa1 357 _pc.printf("\r>%s< New Key Added:\t0x%016x\n\r", _inCharQ, pMessage->message);
CallumAlder 42:121148278dae 358 break;
CallumAlder 42:121148278dae 359 case torque:
CallumAlder 47:21bf4096faa1 360 _pc.printf("\r>%s< Motor Torque set to:\t%d\n\r", _inCharQ, (int32_t) pMessage->message);
CallumAlder 42:121148278dae 361 break;
CallumAlder 42:121148278dae 362 case velIn:
CallumAlder 47:21bf4096faa1 363 _pc.printf("\r>%s< Target Velocity set to:\t%.2f\n\r", _inCharQ, _targetVel);
CallumAlder 42:121148278dae 364 break;
CallumAlder 42:121148278dae 365 case velOut:
CallumAlder 47:21bf4096faa1 366 _pc.printf("\r>%s< Current Velocity:\t%.2f States/sec\n\r", _inCharQ, (float) ((int32_t) pMessage->message));
CallumAlder 42:121148278dae 367 break;
CallumAlder 42:121148278dae 368 case posIn:
CallumAlder 47:21bf4096faa1 369 _pc.printf("\r>%s< Target # Rotations:\t%.2f\n\r", _inCharQ, (float) ((int32_t) pMessage->message));
CallumAlder 42:121148278dae 370 break;
CallumAlder 42:121148278dae 371 case posOut:
CallumAlder 47:21bf4096faa1 372 _pc.printf("\r>%s< Current Position:\t%.2f\n\r", _inCharQ, (float) ((int32_t) pMessage->message));
CallumAlder 42:121148278dae 373 break;
CallumAlder 47:21bf4096faa1 374 case melody:
CallumAlder 47:21bf4096faa1 375 _pc.printf("\r>%s< New Tune:\t%s\n\r", _inCharQ, _newCmd);
CallumAlder 47:21bf4096faa1 376 break;
CallumAlder 42:121148278dae 377 case error:
CallumAlder 47:21bf4096faa1 378 switch (pMessage->message) {
CallumAlder 47:21bf4096faa1 379 case 1:
CallumAlder 47:21bf4096faa1 380 _pc.printf("\r>%s< Error:%s\n\r", _inCharQ, "Overfull Buffer Reset" );
CallumAlder 47:21bf4096faa1 381 break;
CallumAlder 47:21bf4096faa1 382 case 2:
CallumAlder 47:21bf4096faa1 383 _pc.printf("\r>%s< Error:%s\n\r", _inCharQ, "Invalid Melody" );
CallumAlder 47:21bf4096faa1 384 default:
CallumAlder 47:21bf4096faa1 385 break;
CallumAlder 47:21bf4096faa1 386 }
CallumAlder 47:21bf4096faa1 387 for (int i = 0; i < _MAXCMDLENGTH; ++i) // reset buffer
CallumAlder 47:21bf4096faa1 388 _inCharQ[i] = ' ';
CallumAlder 47:21bf4096faa1 389
CallumAlder 47:21bf4096faa1 390 _inCharIndex = 0;
CallumAlder 42:121148278dae 391 break;
CallumAlder 42:121148278dae 392 default:
CallumAlder 47:21bf4096faa1 393 _pc.printf("\r>%s< Unknown Error. Message: %x\n\r", _inCharQ, pMessage->message);
CallumAlder 42:121148278dae 394 break;
CallumAlder 42:121148278dae 395 }
CallumAlder 42:121148278dae 396
CallumAlder 47:21bf4096faa1 397 _msgStack.free(pMessage);
CallumAlder 42:121148278dae 398 }
CallumAlder 42:121148278dae 399 }
CallumAlder 42:121148278dae 400
CallumAlder 48:b2afe48ced0d 401 //-- Put a Message On the Outgoing Message Stack --------------------------------------------------------------------//
CallumAlder 42:121148278dae 402 void putMessage(msgType type, uint32_t message){
CallumAlder 47:21bf4096faa1 403 msg *p_msg = _msgStack.alloc();
CallumAlder 42:121148278dae 404 p_msg->type = type;
CallumAlder 42:121148278dae 405 p_msg->message = message;
CallumAlder 47:21bf4096faa1 406 _msgStack.put(p_msg);
CallumAlder 42:121148278dae 407 }
CallumAlder 42:121148278dae 408
CallumAlder 48:b2afe48ced0d 409 //-- Attach CommOut Thread to the Outgoing Communication Function ---------------------------------------------------//
CallumAlder 42:121148278dae 410 void start_comm(){
CallumAlder 42:121148278dae 411 _RUN = true;
CallumAlder 47:21bf4096faa1 412 _tCommOut.start(callback(this, &Comm::commOutFn));
adehadd 49:ae8dedfe2d0f 413 _tCommIn.start(callback(this, &Comm::commInFn));
CallumAlder 42:121148278dae 414 }
iachinweze1 23:ab1cb51527d1 415
adehadd 49:ae8dedfe2d0f 416
CallumAlder 42:121148278dae 417 };
adehadd 49:ae8dedfe2d0f 418 // char Comm::_inCharQ[] = {'.','.','.','.','.','.','.','.','.','.','.','.','.','.','.','.','.','\0'}; // Static member must be defined outside class
CallumAlder 48:b2afe48ced0d 419 //Mutex Comm::_newKeyMutex;
adehadd 46:b9081aa50bda 420
CallumAlder 42:121148278dae 421 class Motor {
adehadd 27:ce05fed3c1ea 422
CallumAlder 42:121148278dae 423 protected:
CallumAlder 48:b2afe48ced0d 424 volatile int8_t _orState, // Rotor offset at motor state 0, motor specific
CallumAlder 48:b2afe48ced0d 425 _currentState, // Current Rotor State
CallumAlder 48:b2afe48ced0d 426 _stateList[6], // All possible rotor states stored
CallumAlder 48:b2afe48ced0d 427 _lead; // Phase _lead to make motor spin
CallumAlder 42:121148278dae 428
CallumAlder 48:b2afe48ced0d 429 uint8_t _theStates[3], // The Key states
CallumAlder 48:b2afe48ced0d 430 _stateCount[3]; // State Counter
adehadd 51:c03f63c6f930 431 uint32_t _mtrPeriod, // Motor period
CallumAlder 47:21bf4096faa1 432 _MAXPWM_PRD;
CallumAlder 48:b2afe48ced0d 433 float _dutyC; // 1 = 100%
CallumAlder 47:21bf4096faa1 434 bool _RUN;
adehadd 27:ce05fed3c1ea 435
CallumAlder 48:b2afe48ced0d 436 Comm* _pComm;
CallumAlder 48:b2afe48ced0d 437 Thread _tMotorCtrl; // Thread for motor Control
CallumAlder 42:121148278dae 438
adehadd 51:c03f63c6f930 439 int8_t old_rotor_state;
adehadd 51:c03f63c6f930 440 uint8_t encState;
adehadd 51:c03f63c6f930 441 uint32_t quadratureStates;
adehadd 51:c03f63c6f930 442 uint32_t MINPWM_PRD;
adehadd 51:c03f63c6f930 443 uint32_t maxEncCount;
adehadd 51:c03f63c6f930 444 uint32_t encCount;
adehadd 51:c03f63c6f930 445 uint32_t encTotal;
adehadd 51:c03f63c6f930 446 uint32_t badEdges;
adehadd 51:c03f63c6f930 447
CallumAlder 42:121148278dae 448 public:
CallumAlder 48:b2afe48ced0d 449 //-- Default Constructor With Thread Object Constructor -------------------------------------------------------------//
adehadd 51:c03f63c6f930 450 Motor() : _tMotorCtrl(osPriorityAboveNormal2, 2048){
CallumAlder 47:21bf4096faa1 451
CallumAlder 48:b2afe48ced0d 452 _dutyC = 1.0f; // Set Power to maximum to drive motorHome()
adehadd 51:c03f63c6f930 453 _mtrPeriod = 2e3; // Motor period
adehadd 51:c03f63c6f930 454 pwmCtrl.period_us(_mtrPeriod); // Initialise PWM
adehadd 51:c03f63c6f930 455 pwmCtrl.pulsewidth_us(_mtrPeriod);
CallumAlder 42:121148278dae 456
CallumAlder 48:b2afe48ced0d 457 _orState = motorHome(); // Rotor offset at motor state 0
adehadd 49:ae8dedfe2d0f 458 _currentState = readRotorState(); // Current Rotor State
CallumAlder 48:b2afe48ced0d 459 _lead = 2; // 2 for forwards, -2 for backwards
adehadd 51:c03f63c6f930 460 old_rotor_state = _orState; // Set old_rotor_state to the origin to begin with
adehadd 27:ce05fed3c1ea 461
CallumAlder 48:b2afe48ced0d 462 _theStates[0] = _orState +1; // Initialise repeatable states, for us this is state 1
CallumAlder 48:b2afe48ced0d 463 _theStates[1] = (_orState + _lead) % 6 +1; // state 3
CallumAlder 48:b2afe48ced0d 464 _theStates[2] = (_orState + (_lead*2)) % 6 +1; // state 5
CallumAlder 42:121148278dae 465
CallumAlder 48:b2afe48ced0d 466 _stateCount[0] = 0; // Initialise
CallumAlder 48:b2afe48ced0d 467 _stateCount[1] = 0;
CallumAlder 48:b2afe48ced0d 468 _stateCount[2] = 0;
CallumAlder 42:121148278dae 469
CallumAlder 48:b2afe48ced0d 470 _pComm = NULL; // Initialise as NULL
CallumAlder 48:b2afe48ced0d 471 _RUN = false;
CallumAlder 42:121148278dae 472
CallumAlder 47:21bf4096faa1 473 _MAXPWM_PRD = 2e3;
CallumAlder 42:121148278dae 474
adehadd 51:c03f63c6f930 475 maxEncCount = 0;
adehadd 51:c03f63c6f930 476 encCount = 0;
adehadd 51:c03f63c6f930 477 encTotal = 0;
adehadd 51:c03f63c6f930 478 badEdges = 0;
adehadd 51:c03f63c6f930 479
adehadd 51:c03f63c6f930 480 quadratureStates = 1112;
adehadd 51:c03f63c6f930 481
CallumAlder 42:121148278dae 482 }
adehadd 27:ce05fed3c1ea 483
adehadd 51:c03f63c6f930 484 int findMinTorque() {
adehadd 51:c03f63c6f930 485 int8_t prevState = readRotorState();
adehadd 51:c03f63c6f930 486 uint32_t _mtPeriod = 700;
adehadd 51:c03f63c6f930 487 Timer testTimer;
adehadd 51:c03f63c6f930 488 testTimer.start();
adehadd 51:c03f63c6f930 489
adehadd 51:c03f63c6f930 490 _pComm->_pc.printf("PState:%i, CState:%i\n", prevState, readRotorState());
adehadd 51:c03f63c6f930 491
adehadd 51:c03f63c6f930 492 // stateUpdate();
adehadd 51:c03f63c6f930 493
adehadd 51:c03f63c6f930 494 while (readRotorState() == prevState) {
adehadd 51:c03f63c6f930 495 testTimer.reset();
adehadd 51:c03f63c6f930 496 // pwmCtrl.period_us(2e3);
adehadd 51:c03f63c6f930 497 pwmCtrl.pulsewidth_us(_mtPeriod);
adehadd 51:c03f63c6f930 498 stateUpdate();
adehadd 51:c03f63c6f930 499
adehadd 51:c03f63c6f930 500 while (testTimer.read_ms() < 500) {}
adehadd 51:c03f63c6f930 501 // prevState = readRotorState();
adehadd 51:c03f63c6f930 502 _mtPeriod += 10;
adehadd 51:c03f63c6f930 503 _mtPeriod = _mtPeriod >= 1000 ? 700 : _mtPeriod;
adehadd 51:c03f63c6f930 504 }
adehadd 51:c03f63c6f930 505
adehadd 51:c03f63c6f930 506 _pComm->_pc.printf("Min Torque:%i\n", _mtPeriod);
adehadd 51:c03f63c6f930 507 return _mtPeriod;
adehadd 51:c03f63c6f930 508 }
adehadd 51:c03f63c6f930 509
adehadd 51:c03f63c6f930 510
CallumAlder 48:b2afe48ced0d 511 //-- Start Motor and Attach State Update Function to Rise/Fall Interrupts -------------------------------------------//
CallumAlder 42:121148278dae 512 void motorStart(Comm *comm) {
CallumAlder 42:121148278dae 513
CallumAlder 48:b2afe48ced0d 514 I1.fall(callback(this, &Motor::stateUpdate)); // Establish Photo Interrupter Service Routines (auto choose next state)
CallumAlder 42:121148278dae 515 I2.fall(callback(this, &Motor::stateUpdate));
CallumAlder 42:121148278dae 516 I3.fall(callback(this, &Motor::stateUpdate));
CallumAlder 42:121148278dae 517 I1.rise(callback(this, &Motor::stateUpdate));
CallumAlder 42:121148278dae 518 I2.rise(callback(this, &Motor::stateUpdate));
CallumAlder 42:121148278dae 519 I3.rise(callback(this, &Motor::stateUpdate));
CallumAlder 42:121148278dae 520
CallumAlder 48:b2afe48ced0d 521 motorOut((_currentState-_orState+_lead+6)%6); // Push digitally so static motor will start moving
CallumAlder 42:121148278dae 522
CallumAlder 48:b2afe48ced0d 523
CallumAlder 48:b2afe48ced0d 524 _dutyC = 0.8; // Default a lower duty cycle
adehadd 51:c03f63c6f930 525 pwmCtrl.period_us((uint32_t)_mtrPeriod);
adehadd 51:c03f63c6f930 526 pwmCtrl.pulsewidth_us((uint32_t)(_mtrPeriod*_dutyC));
CallumAlder 42:121148278dae 527
CallumAlder 48:b2afe48ced0d 528 _pComm = comm;
CallumAlder 42:121148278dae 529 _RUN = true;
adehadd 51:c03f63c6f930 530 MINPWM_PRD = 920;
adehadd 51:c03f63c6f930 531
CallumAlder 42:121148278dae 532
CallumAlder 48:b2afe48ced0d 533 _tMotorCtrl.start(callback(this, &Motor::motorCtrlFn)); // Start motor control thread
CallumAlder 42:121148278dae 534
CallumAlder 48:b2afe48ced0d 535 _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 536
CallumAlder 42:121148278dae 537 }
CallumAlder 42:121148278dae 538
CallumAlder 48:b2afe48ced0d 539 //-- Set a Predetermined Drive State -------------------------------------------------------------------------------//
CallumAlder 42:121148278dae 540 void motorOut(int8_t driveState) {
iachinweze1 23:ab1cb51527d1 541
CallumAlder 48:b2afe48ced0d 542 int8_t driveOut = driveTable[driveState & 0x07]; //Lookup the output byte from the drive state
CallumAlder 42:121148278dae 543
CallumAlder 48:b2afe48ced0d 544 if (~driveOut & 0x01) L1L = 0; //Turn off first
CallumAlder 42:121148278dae 545 if (~driveOut & 0x02) L1H = 1;
CallumAlder 42:121148278dae 546 if (~driveOut & 0x04) L2L = 0;
CallumAlder 42:121148278dae 547 if (~driveOut & 0x08) L2H = 1;
CallumAlder 42:121148278dae 548 if (~driveOut & 0x10) L3L = 0;
CallumAlder 42:121148278dae 549 if (~driveOut & 0x20) L3H = 1;
CallumAlder 42:121148278dae 550
CallumAlder 48:b2afe48ced0d 551 if (driveOut & 0x01) L1L = 1; //Then turn on
CallumAlder 48:b2afe48ced0d 552 if (driveOut & 0x02) L1H = 0;
CallumAlder 48:b2afe48ced0d 553 if (driveOut & 0x04) L2L = 1;
CallumAlder 48:b2afe48ced0d 554 if (driveOut & 0x08) L2H = 0;
CallumAlder 48:b2afe48ced0d 555 if (driveOut & 0x10) L3L = 1;
CallumAlder 48:b2afe48ced0d 556 if (driveOut & 0x20) L3H = 0;
CallumAlder 42:121148278dae 557 }
CallumAlder 42:121148278dae 558
CallumAlder 48:b2afe48ced0d 559 //-- Inline Conversion of Photointerrupts to a Rotor State ----------------------------------------------------------//
adehadd 49:ae8dedfe2d0f 560 inline int8_t readRotorState() {
CallumAlder 42:121148278dae 561 return stateMap[I1 + 2*I2 + 4*I3];
CallumAlder 42:121148278dae 562 }
CallumAlder 42:121148278dae 563
CallumAlder 48:b2afe48ced0d 564 //-- Basic Motor Stabilisation and Synchronisation ------------------------------------------------------------------//
CallumAlder 42:121148278dae 565 int8_t motorHome() {
CallumAlder 48:b2afe48ced0d 566
CallumAlder 48:b2afe48ced0d 567 motorOut(0); //Put the motor in drive state 0 and wait for it to stabilise
CallumAlder 42:121148278dae 568 wait(3.0);
CallumAlder 42:121148278dae 569
adehadd 49:ae8dedfe2d0f 570 return readRotorState(); //Get the rotor state
CallumAlder 42:121148278dae 571 }
iachinweze1 23:ab1cb51527d1 572
CallumAlder 48:b2afe48ced0d 573 //-- Motor State Log, Circumvents Issues From Occasionally Skipping Certain States ----------------------------------//
CallumAlder 42:121148278dae 574 void stateUpdate() { // () { // **params
adehadd 49:ae8dedfe2d0f 575 _currentState = readRotorState(); // Get current state
adehadd 27:ce05fed3c1ea 576
CallumAlder 48:b2afe48ced0d 577 motorOut((_currentState - _orState + _lead + 6) % 6); // Send the next state to the motor
iachinweze1 23:ab1cb51527d1 578
adehadd 51:c03f63c6f930 579 if (_currentState == 1) {
adehadd 51:c03f63c6f930 580 //if (encCount > maxEncCount) maxEncCount = encCount;
adehadd 51:c03f63c6f930 581 //if (encCount < minEncCount) minEncCount = encCount;
adehadd 51:c03f63c6f930 582 //if (badEdges > maxBadEdges) maxBadEdges = badEdges;
adehadd 51:c03f63c6f930 583
adehadd 51:c03f63c6f930 584 //revCount++;
adehadd 51:c03f63c6f930 585 encTotal += encCount;
adehadd 51:c03f63c6f930 586 encCount = 0;
adehadd 51:c03f63c6f930 587 badEdges = 0;
adehadd 51:c03f63c6f930 588 }
adehadd 51:c03f63c6f930 589
adehadd 51:c03f63c6f930 590 if (_currentState - old_rotor_state == 5) {
adehadd 51:c03f63c6f930 591 _pComm->_motor_pos--;
adehadd 51:c03f63c6f930 592 } else if (_currentState - old_rotor_state == -5) {
adehadd 51:c03f63c6f930 593 _pComm->_motor_pos++;
adehadd 51:c03f63c6f930 594 } else {
adehadd 51:c03f63c6f930 595 _pComm->_motor_pos += (_currentState - old_rotor_state);
adehadd 51:c03f63c6f930 596 }
adehadd 51:c03f63c6f930 597
adehadd 51:c03f63c6f930 598 old_rotor_state = _currentState;
adehadd 51:c03f63c6f930 599 }
adehadd 51:c03f63c6f930 600
adehadd 51:c03f63c6f930 601 // A Rise
adehadd 51:c03f63c6f930 602 void encISR0() {
adehadd 51:c03f63c6f930 603 if (encState == 3) {encCount++;}
adehadd 51:c03f63c6f930 604 else badEdges++;
adehadd 51:c03f63c6f930 605 encState = 0;
adehadd 51:c03f63c6f930 606 }
adehadd 51:c03f63c6f930 607
adehadd 51:c03f63c6f930 608 // B Rise
adehadd 51:c03f63c6f930 609 void encISR1() {
adehadd 51:c03f63c6f930 610 if (encState == 0) {encCount++;}
adehadd 51:c03f63c6f930 611 else badEdges++;
adehadd 51:c03f63c6f930 612 encState = 1;
adehadd 51:c03f63c6f930 613 }
adehadd 51:c03f63c6f930 614
adehadd 51:c03f63c6f930 615 // A Fall
adehadd 51:c03f63c6f930 616 void encISR2() {
adehadd 51:c03f63c6f930 617 if (encState == 1) {encCount++;}
adehadd 51:c03f63c6f930 618 else badEdges++;
adehadd 51:c03f63c6f930 619 encState = 2;
adehadd 51:c03f63c6f930 620 }
adehadd 51:c03f63c6f930 621
adehadd 51:c03f63c6f930 622 // B Fall
adehadd 51:c03f63c6f930 623 void encISR3() {
adehadd 51:c03f63c6f930 624 if (encState == 2) {encCount++;}
adehadd 51:c03f63c6f930 625 else badEdges++;
adehadd 51:c03f63c6f930 626 encState = 3;
CallumAlder 42:121148278dae 627 }
CallumAlder 19:805c87360b55 628
CallumAlder 48:b2afe48ced0d 629 //-- Motor PID Control ---------------------------------------------------------------------------------------------//
CallumAlder 42:121148278dae 630 void motorCtrlFn() {
CallumAlder 42:121148278dae 631 Ticker motorCtrlTicker;
CallumAlder 42:121148278dae 632 Timer m_timer;
CallumAlder 42:121148278dae 633 motorCtrlTicker.attach_us(callback(this,&Motor::motorCtrlTick), 1e5);
iachinweze1 23:ab1cb51527d1 634
CallumAlder 42:121148278dae 635 // Init some things
CallumAlder 48:b2afe48ced0d 636 uint8_t cpyStateCount[3];
CallumAlder 48:b2afe48ced0d 637 uint8_t cpyCurrentState;
CallumAlder 48:b2afe48ced0d 638 int8_t cpyModeBitfield;
CallumAlder 42:121148278dae 639
CallumAlder 48:b2afe48ced0d 640 int32_t ting[2] = {6,1}; // 360,60 (for degrees), 5,1 (for states)
CallumAlder 48:b2afe48ced0d 641 uint8_t iterElementMax;
CallumAlder 48:b2afe48ced0d 642 int32_t totalDegrees;
CallumAlder 48:b2afe48ced0d 643 int32_t stateDiff;
adehadd 27:ce05fed3c1ea 644
adehadd 51:c03f63c6f930 645 // int32_t cur_speed; // Variable for local velocity calculation
adehadd 51:c03f63c6f930 646 // int32_t locMotorPos; // Local copy of motor position
CallumAlder 47:21bf4096faa1 647 volatile int32_t torque; // Local variable to set motor torque
CallumAlder 48:b2afe48ced0d 648 static int32_t oldTorque =0;
CallumAlder 48:b2afe48ced0d 649 float sError, // Velocity error between target and reality
adehadd 51:c03f63c6f930 650 rError,
adehadd 51:c03f63c6f930 651 eError = 0; // Rotation error between target and reality
CallumAlder 48:b2afe48ced0d 652 static float rErrorOld; // Old rotation error used for calculation
adehadd 51:c03f63c6f930 653 float encRemain = 0;
adehadd 51:c03f63c6f930 654 bool attached = true;
adehadd 51:c03f63c6f930 655 bool declared = false;
CallumAlder 42:121148278dae 656
CallumAlder 42:121148278dae 657 //~~~Controller constants~~~~
adehadd 51:c03f63c6f930 658 int32_t Kp1=90; // Proportional controller constants
adehadd 51:c03f63c6f930 659 int32_t Kp2=33; // Calculated by trial and error to give optimal accuracy
adehadd 51:c03f63c6f930 660 float Kd=36.5;
adehadd 51:c03f63c6f930 661 float Kis = 10.0f; // 50
adehadd 51:c03f63c6f930 662 int32_t Kir = 0.0f;
adehadd 51:c03f63c6f930 663 float sIntegral = 0.0f;
adehadd 51:c03f63c6f930 664 float rIntegral = 0.0f;
CallumAlder 42:121148278dae 665
CallumAlder 48:b2afe48ced0d 666 int32_t Ts; // Initialise controller output Ts (s=speed)
CallumAlder 48:b2afe48ced0d 667 int32_t Tr; // Initialise controller output Tr (r=rotations)
CallumAlder 42:121148278dae 668
adehadd 51:c03f63c6f930 669 int32_t old_pos = 0,
adehadd 51:c03f63c6f930 670 cur_pos = 0;
adehadd 27:ce05fed3c1ea 671
adehadd 51:c03f63c6f930 672 float cur_err = 0.0f,
adehadd 51:c03f63c6f930 673 old_err = 0.0f,
adehadd 51:c03f63c6f930 674 err_diff,
adehadd 51:c03f63c6f930 675 time_diff,
adehadd 51:c03f63c6f930 676 hold_pos = 0,
adehadd 51:c03f63c6f930 677 cur_time = 0.0f,
adehadd 51:c03f63c6f930 678 old_time = 0.0f,
adehadd 51:c03f63c6f930 679 cur_speed = 0.0f;
CallumAlder 42:121148278dae 680
CallumAlder 42:121148278dae 681
adehadd 50:d1b983a0dd6f 682
adehadd 50:d1b983a0dd6f 683 enum { // To nearest Hz
adehadd 50:d1b983a0dd6f 684 NOTE_C = 261, // C4
adehadd 50:d1b983a0dd6f 685 NOTE_D = 293, // D4
adehadd 50:d1b983a0dd6f 686 NOTE_E = 330, // E4
adehadd 50:d1b983a0dd6f 687 NOTE_F = 349, // F4
adehadd 50:d1b983a0dd6f 688 NOTE_G = 392, // G4
adehadd 50:d1b983a0dd6f 689 NOTE_A = 440, // A4
adehadd 50:d1b983a0dd6f 690 NOTE_B = 494 // B4
adehadd 50:d1b983a0dd6f 691 };
adehadd 50:d1b983a0dd6f 692
CallumAlder 42:121148278dae 693 m_timer.start();
CallumAlder 42:121148278dae 694
CallumAlder 42:121148278dae 695 while (_RUN) {
CallumAlder 48:b2afe48ced0d 696 _tMotorCtrl.signal_wait((int32_t)0x1);
adehadd 49:ae8dedfe2d0f 697 // _pComm->_tCommIn.signal_set(0x01);
iachinweze1 23:ab1cb51527d1 698
CallumAlder 48:b2afe48ced0d 699 core_util_critical_section_enter(); // Access shared variables here
CallumAlder 48:b2afe48ced0d 700 cpyModeBitfield = _pComm->_modeBitField;
adehadd 51:c03f63c6f930 701 // std::copy(_stateCount, _stateCount+3, cpyStateCount);
adehadd 51:c03f63c6f930 702 // cpyCurrentState = _currentState;
adehadd 51:c03f63c6f930 703 // for (int i = 0; i < 3; ++i) {
adehadd 51:c03f63c6f930 704 // _stateCount[i] = 0;
adehadd 51:c03f63c6f930 705 // }
CallumAlder 42:121148278dae 706 core_util_critical_section_exit();
adehadd 20:c60f4785b556 707
CallumAlder 48:b2afe48ced0d 708
adehadd 51:c03f63c6f930 709 // cur_time = m_timer.read(); // Read state & timestamp
CallumAlder 42:121148278dae 710
adehadd 51:c03f63c6f930 711 // time_diff = cur_time - old_time;
CallumAlder 42:121148278dae 712 // cur_speed = (cur_pos - old_pos) / time_diff;
CallumAlder 48:b2afe48ced0d 713
adehadd 51:c03f63c6f930 714 // old_time = cur_time; // prep values for next time through loop
adehadd 51:c03f63c6f930 715 // old_pos = cpyCurrentState;
CallumAlder 42:121148278dae 716
CallumAlder 47:21bf4096faa1 717 // Hence we make the value positive,// and instead set the direction to the opposite one
CallumAlder 42:121148278dae 718
adehadd 51:c03f63c6f930 719 // iterElementMax = std::max_element(cpyStateCount, cpyStateCount+3) - cpyStateCount;
adehadd 20:c60f4785b556 720
adehadd 51:c03f63c6f930 721 // totalDegrees = ting[0] * cpyStateCount[iterElementMax];
adehadd 51:c03f63c6f930 722 // stateDiff = _theStates[iterElementMax]-cpyCurrentState;
adehadd 51:c03f63c6f930 723 // stateDiff >= 0 ? totalDegrees = totalDegrees + (ting[1]* stateDiff) : \
adehadd 51:c03f63c6f930 724 // totalDegrees = totalDegrees + (ting[1]* stateDiff *-1);
CallumAlder 42:121148278dae 725
CallumAlder 47:21bf4096faa1 726 if ((cpyModeBitfield & 0x01)|(cpyModeBitfield & 0x02)) {// Speed, torque control and PID
adehadd 51:c03f63c6f930 727 if (abs(cur_speed) <= 120 && (!attached)) {
adehadd 51:c03f63c6f930 728 eError = 1.0f;
adehadd 51:c03f63c6f930 729 attached = true;
adehadd 51:c03f63c6f930 730
adehadd 51:c03f63c6f930 731 CHA.rise(callback(this, &Motor::encISR0));
adehadd 51:c03f63c6f930 732 CHB.fall(callback(this, &Motor::encISR3));
adehadd 51:c03f63c6f930 733 CHB.rise(callback(this, &Motor::encISR1));
adehadd 51:c03f63c6f930 734 CHA.fall(callback(this, &Motor::encISR2));
adehadd 51:c03f63c6f930 735
adehadd 51:c03f63c6f930 736 encCount = 0;
adehadd 51:c03f63c6f930 737 encTotal = 0;
adehadd 51:c03f63c6f930 738 //_comm->printf("old error:%.4f quadratureStates:%u", old_err, quadratureStates);
adehadd 51:c03f63c6f930 739 encRemain = old_err * quadratureStates; // Number of encs remaining
adehadd 51:c03f63c6f930 740 //_pComm->_pc.printf("old error:%.4f quadratureStates:%u encRemain:%.4f", old_err, quadratureStates, encRemain);
adehadd 51:c03f63c6f930 741 }
adehadd 51:c03f63c6f930 742
adehadd 51:c03f63c6f930 743 else if (abs(cur_speed) > 120 && (attached)) {
adehadd 51:c03f63c6f930 744 eError = 0.0f;
adehadd 51:c03f63c6f930 745 attached = false;
adehadd 51:c03f63c6f930 746
adehadd 51:c03f63c6f930 747 CHA.rise(NULL);
adehadd 51:c03f63c6f930 748 CHB.fall(NULL);
adehadd 51:c03f63c6f930 749 CHB.rise(NULL);
adehadd 51:c03f63c6f930 750 CHA.fall(NULL);
adehadd 51:c03f63c6f930 751
adehadd 51:c03f63c6f930 752 encCount = 0;
adehadd 51:c03f63c6f930 753 encTotal = 0;
adehadd 51:c03f63c6f930 754 eError = 0;
adehadd 51:c03f63c6f930 755
adehadd 51:c03f63c6f930 756 // Store the number of rotations that have been done up to the point where the channels are activated
adehadd 51:c03f63c6f930 757 hold_pos = (cur_pos / 6.0f);
adehadd 51:c03f63c6f930 758 _pComm->_pc.printf("HPos:%f\n", hold_pos);
adehadd 51:c03f63c6f930 759
adehadd 51:c03f63c6f930 760 }
CallumAlder 42:121148278dae 761
adehadd 51:c03f63c6f930 762 // read state & timestamp
adehadd 51:c03f63c6f930 763 cur_pos = _pComm->_motor_pos;
adehadd 51:c03f63c6f930 764 if (cur_pos < 2) {
adehadd 51:c03f63c6f930 765 rIntegral = 0.0f;
adehadd 51:c03f63c6f930 766 sIntegral = 0.0f;
adehadd 51:c03f63c6f930 767 declared = false;
adehadd 51:c03f63c6f930 768 }
adehadd 51:c03f63c6f930 769 cur_time = m_timer.read();
adehadd 51:c03f63c6f930 770
adehadd 51:c03f63c6f930 771 // compute speed
adehadd 51:c03f63c6f930 772 time_diff = cur_time - old_time;
adehadd 51:c03f63c6f930 773 cur_speed = (cur_pos - old_pos) / time_diff;
adehadd 51:c03f63c6f930 774
adehadd 51:c03f63c6f930 775 // prep values for next time through loop
adehadd 51:c03f63c6f930 776 old_time = cur_time;
adehadd 51:c03f63c6f930 777 old_pos = cur_pos;
adehadd 51:c03f63c6f930 778
adehadd 51:c03f63c6f930 779 // Position error
adehadd 51:c03f63c6f930 780 // if (!attached) {
adehadd 51:c03f63c6f930 781 rError = (_pComm->_targetRot) - (cur_pos/6.0f);
adehadd 51:c03f63c6f930 782 // }
adehadd 51:c03f63c6f930 783 // else{
adehadd 51:c03f63c6f930 784 // rErrorOld = RError;
adehadd 51:c03f63c6f930 785 // rError = (_pComm->_targetRot) - (hold_pos + ((encRemain - encTotal)/quadratureStates));
adehadd 51:c03f63c6f930 786 // }
adehadd 51:c03f63c6f930 787
adehadd 51:c03f63c6f930 788
adehadd 51:c03f63c6f930 789 if ((cur_speed != 0)) {
adehadd 51:c03f63c6f930 790 rIntegral += rError * time_diff;
adehadd 51:c03f63c6f930 791 }
adehadd 51:c03f63c6f930 792 err_diff = rError - old_err;
adehadd 51:c03f63c6f930 793 old_err = rError;
adehadd 51:c03f63c6f930 794
adehadd 51:c03f63c6f930 795 // Speed error - Convert curr_speed from states per time to rotations per time
adehadd 51:c03f63c6f930 796 // TODO: Check the direction that CPos goes in when _targetVel < 0
adehadd 51:c03f63c6f930 797 sError = (_pComm->_targetVel) - (abs(cur_speed/6.0f)); //Read global variable _targetVel updated by interrupt and calculate error between target and reality
adehadd 51:c03f63c6f930 798 if ((cur_speed != 0) && (torque != _MAXPWM_PRD)) {
adehadd 51:c03f63c6f930 799 sIntegral += sError * time_diff;
adehadd 51:c03f63c6f930 800 if (abs(sIntegral * Kis) >= 2000) {
adehadd 51:c03f63c6f930 801 sIntegral -= sError * time_diff;
adehadd 51:c03f63c6f930 802 }
adehadd 51:c03f63c6f930 803 }
adehadd 51:c03f63c6f930 804
adehadd 51:c03f63c6f930 805 Ts = (int32_t)(((Kp1 * sError) + (Kis * sIntegral))); // * sgn(cur_pos));
adehadd 51:c03f63c6f930 806 Tr = (int32_t)(Kp2*rError + ((Kd/time_diff) * err_diff) + (Kir * rIntegral));
CallumAlder 42:121148278dae 807
adehadd 51:c03f63c6f930 808 if (cpyModeBitfield & 0x01) {
adehadd 51:c03f63c6f930 809 // Speed control
adehadd 51:c03f63c6f930 810 if (_pComm->_targetVel == 0) { //Check if user entered V0,
adehadd 51:c03f63c6f930 811 torque = _MAXPWM_PRD; //and set the output to maximum as specified
adehadd 51:c03f63c6f930 812 }
adehadd 51:c03f63c6f930 813
adehadd 51:c03f63c6f930 814 else {
adehadd 51:c03f63c6f930 815 // select minimum absolute value torque
adehadd 51:c03f63c6f930 816 /*
adehadd 51:c03f63c6f930 817 if (cur_speed < 0) {
adehadd 51:c03f63c6f930 818 torque = max(Ts, Tr);
adehadd 51:c03f63c6f930 819 }
adehadd 51:c03f63c6f930 820
adehadd 51:c03f63c6f930 821 else {
adehadd 51:c03f63c6f930 822 torque = min(Ts, Tr);
adehadd 51:c03f63c6f930 823 } */
adehadd 51:c03f63c6f930 824
adehadd 51:c03f63c6f930 825 torque = Ts;
adehadd 51:c03f63c6f930 826 if (abs(sError) > 0.6) {
adehadd 51:c03f63c6f930 827 // torque = abs(Tr) < 1000 ? 1000*sgn(Tr) : Tr;
adehadd 51:c03f63c6f930 828 torque = abs(torque) < MINPWM_PRD ? MINPWM_PRD*sgn(torque) : torque;
adehadd 51:c03f63c6f930 829 _pComm->_pc.printf("Speed:%f\r\n", (cur_speed/6.0f));
adehadd 51:c03f63c6f930 830 } else {
adehadd 51:c03f63c6f930 831 torque = 0;
adehadd 51:c03f63c6f930 832 }
adehadd 51:c03f63c6f930 833
adehadd 51:c03f63c6f930 834 torque = abs(torque) < MINPWM_PRD ? MINPWM_PRD*sgn(torque) : torque;
adehadd 51:c03f63c6f930 835 // torque = Ts;
adehadd 51:c03f63c6f930 836 }
adehadd 51:c03f63c6f930 837 }
adehadd 51:c03f63c6f930 838
adehadd 51:c03f63c6f930 839 else if (cpyModeBitfield & 0x02) {
adehadd 51:c03f63c6f930 840 // select minimum absolute value torque
adehadd 51:c03f63c6f930 841 if (_pComm->_targetRot == 0) {
adehadd 51:c03f63c6f930 842 // Not spinning at max speed
adehadd 51:c03f63c6f930 843 torque = 0.7 * _MAXPWM_PRD;
adehadd 51:c03f63c6f930 844 }
CallumAlder 42:121148278dae 845
adehadd 51:c03f63c6f930 846 else {
adehadd 51:c03f63c6f930 847 /*
adehadd 51:c03f63c6f930 848 // TODO: Handle sign or Tr for negative rotations
adehadd 51:c03f63c6f930 849 if (Tr > MINPWM_PRD || Tr < 100) {
adehadd 51:c03f63c6f930 850 torque = Tr;
adehadd 51:c03f63c6f930 851 } else if (Tr >= 100 && Tr <= MINPWM_PRD) {
adehadd 51:c03f63c6f930 852 torque = MINPWM_PRD;
adehadd 51:c03f63c6f930 853 } */
adehadd 51:c03f63c6f930 854
adehadd 51:c03f63c6f930 855 // select minimum absolute value torque
adehadd 51:c03f63c6f930 856 if (cur_speed < 0) {
adehadd 51:c03f63c6f930 857 torque = max(Ts, Tr);
adehadd 51:c03f63c6f930 858 } else {
adehadd 51:c03f63c6f930 859 torque = min(Ts, Tr);
adehadd 51:c03f63c6f930 860 }
adehadd 51:c03f63c6f930 861
adehadd 51:c03f63c6f930 862 if (abs(rError) > 0.2 && (rError > 0)) {
adehadd 51:c03f63c6f930 863 // torque = abs(Tr) < 1000 ? 1000*sgn(Tr) : Tr;
adehadd 51:c03f63c6f930 864 torque = abs(torque) < MINPWM_PRD ? MINPWM_PRD*sgn(torque) : torque;
adehadd 51:c03f63c6f930 865 } else {
adehadd 51:c03f63c6f930 866 torque = 0;
adehadd 51:c03f63c6f930 867 if (!declared) {
adehadd 51:c03f63c6f930 868 _pComm->_pc.printf("Remaining Turns:%f\n", rError);
adehadd 51:c03f63c6f930 869 declared = true;
adehadd 51:c03f63c6f930 870 }
adehadd 51:c03f63c6f930 871 //attached = false;
adehadd 51:c03f63c6f930 872 }
adehadd 51:c03f63c6f930 873
adehadd 51:c03f63c6f930 874 }
adehadd 51:c03f63c6f930 875 }
adehadd 51:c03f63c6f930 876
adehadd 51:c03f63c6f930 877 if (torque < 0) {
adehadd 51:c03f63c6f930 878 torque = -torque;
adehadd 51:c03f63c6f930 879 _lead = -2;
adehadd 51:c03f63c6f930 880 } else {
adehadd 51:c03f63c6f930 881 _lead = 2;
adehadd 51:c03f63c6f930 882 }
adehadd 51:c03f63c6f930 883
adehadd 51:c03f63c6f930 884 torque = torque > _MAXPWM_PRD ? _MAXPWM_PRD : torque; // Set a cap on torque
adehadd 51:c03f63c6f930 885
adehadd 51:c03f63c6f930 886 _pComm->_motorTorque = torque;
adehadd 51:c03f63c6f930 887 pwmCtrl.pulsewidth_us(torque);
adehadd 51:c03f63c6f930 888 // _pComm->_pc.printf("EError:%.4f, tot:%d, encRemain:%.4f || RError:%.4f || SError:%.4f, CSpeed:%f, Torque:%i\r\n", eError, encTotal, encRemain, rError, sError, (cur_speed/6.0f), torque);
adehadd 51:c03f63c6f930 889
adehadd 51:c03f63c6f930 890 //_pComm->_pc.printf("EError:%.4f, remain:%.4f, tot%d || RError:%.4f || Torque:%i \r\n", eError, encRemain, encTotal, rError, torque);
adehadd 51:c03f63c6f930 891
adehadd 51:c03f63c6f930 892
adehadd 51:c03f63c6f930 893 // Give the motor a kick
adehadd 51:c03f63c6f930 894 stateUpdate();
CallumAlder 47:21bf4096faa1 895
CallumAlder 47:21bf4096faa1 896 }
CallumAlder 47:21bf4096faa1 897 else if (cpyModeBitfield & 0x04) { // If it is in torque mode, do no PID math, just set pulsewidth
CallumAlder 48:b2afe48ced0d 898 torque = (int32_t)_pComm->_motorTorque;
CallumAlder 47:21bf4096faa1 899 if (oldTorque != torque) {
CallumAlder 48:b2afe48ced0d 900 _pComm->putMessage((Comm::msgType)8, torque);
CallumAlder 47:21bf4096faa1 901 oldTorque = torque;
CallumAlder 42:121148278dae 902 }
CallumAlder 47:21bf4096faa1 903 }
adehadd 50:d1b983a0dd6f 904 else if (cpyModeBitfield & 0x08) { // If it is in Melody mode
adehadd 51:c03f63c6f930 905 uint32_t oldMtrPeriod = _mtrPeriod; // Backup of current motor period
adehadd 50:d1b983a0dd6f 906 float oldDutyC = _dutyC; // Backup of current duty cycle
adehadd 50:d1b983a0dd6f 907 uint32_t noteFreq, newDur, newPeriod =0,
adehadd 51:c03f63c6f930 908 oldPeriod = _mtrPeriod;
adehadd 50:d1b983a0dd6f 909
adehadd 50:d1b983a0dd6f 910 Timer testTimer;
adehadd 50:d1b983a0dd6f 911 testTimer.start();
adehadd 50:d1b983a0dd6f 912 _pComm->_pc.printf("rep:%i, len:%i \n\r", _pComm->_noteRep, _pComm->_noteLen);
adehadd 50:d1b983a0dd6f 913 for (int k = 0; k < _pComm->_noteRep; ++k) {
adehadd 50:d1b983a0dd6f 914
adehadd 50:d1b983a0dd6f 915 for (int i = 0; i < _pComm->_noteLen; ++i) {
adehadd 50:d1b983a0dd6f 916
adehadd 50:d1b983a0dd6f 917 noteFreq = 0;
adehadd 50:d1b983a0dd6f 918 newDur = (uint32_t)_pComm->_noteDur[i]*1e3; // ms
adehadd 50:d1b983a0dd6f 919
adehadd 50:d1b983a0dd6f 920 switch (_pComm->_notes[i]) {
adehadd 50:d1b983a0dd6f 921 case 'C':
adehadd 50:d1b983a0dd6f 922 noteFreq = NOTE_C;
adehadd 50:d1b983a0dd6f 923 break;
adehadd 50:d1b983a0dd6f 924 case 'D':
adehadd 50:d1b983a0dd6f 925 noteFreq = NOTE_D;
adehadd 50:d1b983a0dd6f 926 break;
adehadd 50:d1b983a0dd6f 927 case 'E':
adehadd 50:d1b983a0dd6f 928 noteFreq = NOTE_E;
adehadd 50:d1b983a0dd6f 929 break;
adehadd 50:d1b983a0dd6f 930 case 'F':
adehadd 50:d1b983a0dd6f 931 noteFreq = NOTE_F;
adehadd 50:d1b983a0dd6f 932 break;
adehadd 50:d1b983a0dd6f 933 case 'G':
adehadd 50:d1b983a0dd6f 934 noteFreq = NOTE_G;
adehadd 50:d1b983a0dd6f 935 break;
adehadd 50:d1b983a0dd6f 936 case 'A':
adehadd 50:d1b983a0dd6f 937 noteFreq = NOTE_A;
adehadd 50:d1b983a0dd6f 938 break;
adehadd 50:d1b983a0dd6f 939 case 'B':
adehadd 50:d1b983a0dd6f 940 noteFreq = NOTE_B;
adehadd 50:d1b983a0dd6f 941 break;
adehadd 50:d1b983a0dd6f 942 default:
adehadd 50:d1b983a0dd6f 943 break;
adehadd 50:d1b983a0dd6f 944 }
adehadd 50:d1b983a0dd6f 945
adehadd 51:c03f63c6f930 946 if (noteFreq == 0) {break;} // leave the loop if we get a wrong character
adehadd 51:c03f63c6f930 947
adehadd 50:d1b983a0dd6f 948 newPeriod = (uint32_t)(1e6/(uint32_t)noteFreq); // us
adehadd 50:d1b983a0dd6f 949
adehadd 50:d1b983a0dd6f 950 if (newPeriod>oldPeriod) { // Change Period First
adehadd 50:d1b983a0dd6f 951 _pComm->_pc.printf("Period:changed \n" );
adehadd 50:d1b983a0dd6f 952 pwmCtrl.period_us(newPeriod); //set frequency of PWM
adehadd 50:d1b983a0dd6f 953 pwmCtrl.pulsewidth_us((uint32_t)(newPeriod*0.8));
adehadd 50:d1b983a0dd6f 954 } else { // Change Pulse WidthFirst
adehadd 50:d1b983a0dd6f 955 pwmCtrl.pulsewidth_us((uint32_t)(newPeriod*0.8));
adehadd 50:d1b983a0dd6f 956 pwmCtrl.period_us(newPeriod); //set frequency of PWM
adehadd 50:d1b983a0dd6f 957 }
adehadd 50:d1b983a0dd6f 958 // stateUpdate();
adehadd 50:d1b983a0dd6f 959 oldPeriod = newPeriod;
adehadd 50:d1b983a0dd6f 960 testTimer.reset();
adehadd 50:d1b983a0dd6f 961 while (testTimer.read_ms() < newDur ) {} // Do nothing
adehadd 50:d1b983a0dd6f 962 _pComm->_pc.printf("Period:%d Dur:%d \n",newPeriod, newDur );
adehadd 50:d1b983a0dd6f 963 }
adehadd 50:d1b983a0dd6f 964
adehadd 50:d1b983a0dd6f 965 _pComm->_modeBitField = 0x04; // stop melody mode
adehadd 50:d1b983a0dd6f 966
adehadd 50:d1b983a0dd6f 967 }
adehadd 50:d1b983a0dd6f 968 // After playing notes, go back to the old speed
adehadd 50:d1b983a0dd6f 969 torque = (int32_t)_pComm->_motorTorque;
adehadd 50:d1b983a0dd6f 970 pwmCtrl.pulsewidth_us((uint32_t)(torque*0.8));
adehadd 50:d1b983a0dd6f 971 // And reset the motor period and duty cycle
adehadd 50:d1b983a0dd6f 972
adehadd 50:d1b983a0dd6f 973 }
CallumAlder 47:21bf4096faa1 974 else{
CallumAlder 47:21bf4096faa1 975 torque = _MAXPWM_PRD * 0.5; // Run at 50% duty cycle if argument not properly defined
adehadd 27:ce05fed3c1ea 976
CallumAlder 42:121148278dae 977 }
adehadd 27:ce05fed3c1ea 978
adehadd 51:c03f63c6f930 979 // torque < 0 ? _lead = -2 : _lead = +2;
adehadd 51:c03f63c6f930 980 // torque = abs(torque);
adehadd 26:fb6151e5907d 981
adehadd 51:c03f63c6f930 982 // if(torque > _MAXPWM_PRD) torque = _MAXPWM_PRD; // In case the calculated PWM is higher than our maximum 50% allowance,
adehadd 51:c03f63c6f930 983 // // Set it to our max.
adehadd 51:c03f63c6f930 984 // _pComm->_motorTorque = torque;
adehadd 51:c03f63c6f930 985 // pwmCtrl.pulsewidth_us(torque);
CallumAlder 47:21bf4096faa1 986
CallumAlder 42:121148278dae 987 }
CallumAlder 42:121148278dae 988 }
CallumAlder 42:121148278dae 989
CallumAlder 42:121148278dae 990 void motorCtrlTick(){
CallumAlder 48:b2afe48ced0d 991 _tMotorCtrl.signal_set(0x1);
CallumAlder 42:121148278dae 992 }
CallumAlder 42:121148278dae 993 };
CallumAlder 42:121148278dae 994
CallumAlder 42:121148278dae 995
adehadd 27:ce05fed3c1ea 996 int main() {
adehadd 26:fb6151e5907d 997
CallumAlder 42:121148278dae 998 // Declare Objects
CallumAlder 42:121148278dae 999 Comm comm_port;
CallumAlder 42:121148278dae 1000 SHA256 miner;
CallumAlder 42:121148278dae 1001 Motor motor;
adehadd 27:ce05fed3c1ea 1002
CallumAlder 42:121148278dae 1003 // Start Motor and Comm Port
CallumAlder 42:121148278dae 1004 motor.motorStart(&comm_port);
CallumAlder 42:121148278dae 1005 comm_port.start_comm();
adehadd 27:ce05fed3c1ea 1006
CallumAlder 42:121148278dae 1007 // Declare Hash Variables
CallumAlder 47:21bf4096faa1 1008 uint8_t sequence[] = {0x45,0x6D,0x62,0x65,0x64,0x64,0x65,0x64,
CallumAlder 47:21bf4096faa1 1009 0x20,0x53,0x79,0x73,0x74,0x65,0x6D,0x73,
CallumAlder 47:21bf4096faa1 1010 0x20,0x61,0x72,0x65,0x20,0x66,0x75,0x6E,
CallumAlder 47:21bf4096faa1 1011 0x20,0x61,0x6E,0x64,0x20,0x64,0x6F,0x20,
CallumAlder 47:21bf4096faa1 1012 0x61,0x77,0x65,0x73,0x6F,0x6D,0x65,0x20,
CallumAlder 47:21bf4096faa1 1013 0x74,0x68,0x69,0x6E,0x67,0x73,0x21,0x20,
CallumAlder 47:21bf4096faa1 1014 0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,
CallumAlder 47:21bf4096faa1 1015 0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00};
CallumAlder 47:21bf4096faa1 1016 uint8_t hash[32];
CallumAlder 47:21bf4096faa1 1017 uint32_t length64 = 64;
CallumAlder 47:21bf4096faa1 1018 uint32_t hashCounter = 0;
CallumAlder 47:21bf4096faa1 1019 uint64_t* nonce = (uint64_t*)((int)sequence + 56);
adehadd 27:ce05fed3c1ea 1020 uint64_t* key = (uint64_t*)((int)sequence + 48);
iachinweze1 23:ab1cb51527d1 1021
CallumAlder 42:121148278dae 1022 // Begin Main Timer
CallumAlder 42:121148278dae 1023 Timer timer;
CallumAlder 42:121148278dae 1024 timer.start();
adehadd 27:ce05fed3c1ea 1025
CallumAlder 47:21bf4096faa1 1026 // Loop Program
CallumAlder 42:121148278dae 1027 while (1) {
adehadd 26:fb6151e5907d 1028
CallumAlder 47:21bf4096faa1 1029 //try{
CallumAlder 47:21bf4096faa1 1030
CallumAlder 47:21bf4096faa1 1031 // Mutex For Access Control
adehadd 49:ae8dedfe2d0f 1032 comm_port._newKeyMutex.lock();
CallumAlder 47:21bf4096faa1 1033 *key = comm_port._newKey;
adehadd 49:ae8dedfe2d0f 1034 comm_port._newKeyMutex.unlock();
adehadd 20:c60f4785b556 1035
CallumAlder 47:21bf4096faa1 1036 // Compute Hash and Counter
CallumAlder 47:21bf4096faa1 1037 miner.computeHash(hash, sequence, length64);
CallumAlder 47:21bf4096faa1 1038 hashCounter++;
CallumAlder 47:21bf4096faa1 1039
CallumAlder 47:21bf4096faa1 1040 // Enum Casting and Condition
CallumAlder 47:21bf4096faa1 1041 if (hash[0]==0 && hash[1]==0)
CallumAlder 47:21bf4096faa1 1042 comm_port.putMessage((Comm::msgType)7, *nonce);
adehadd 20:c60f4785b556 1043
CallumAlder 47:21bf4096faa1 1044 // Try Nonce
CallumAlder 47:21bf4096faa1 1045 (*nonce)++;
adehadd 26:fb6151e5907d 1046
CallumAlder 47:21bf4096faa1 1047 // Display via Comm Port
CallumAlder 47:21bf4096faa1 1048 if (timer.read() >= 1){
CallumAlder 47:21bf4096faa1 1049 comm_port.putMessage((Comm::msgType)5, hashCounter);
CallumAlder 47:21bf4096faa1 1050 hashCounter=0;
CallumAlder 47:21bf4096faa1 1051 timer.reset();
CallumAlder 47:21bf4096faa1 1052 }
CallumAlder 47:21bf4096faa1 1053 //}
adehadd 26:fb6151e5907d 1054
CallumAlder 47:21bf4096faa1 1055 //catch(...){
CallumAlder 47:21bf4096faa1 1056 // break;
CallumAlder 47:21bf4096faa1 1057 //}
CallumAlder 47:21bf4096faa1 1058
CallumAlder 15:2f95f2fb68e3 1059 }
CallumAlder 42:121148278dae 1060
CallumAlder 42:121148278dae 1061 return 0;
adehadd 27:ce05fed3c1ea 1062 }