Skad00sh / Mbed OS ES_CW2_Dev

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

Dependencies:   Crypto

main.cpp@46:b9081aa50bda, 2019-03-20 (annotated)

Committer:
adehadd
Date:
Wed Mar 20 22:31:15 2019 +0000
Revision:
46:b9081aa50bda
Parent:
45:402a8a9423b9
Child:
47:21bf4096faa1
Child:
52:8339cd5c6105
20/03/19 - print error is finally fixed - not ideal but it is fine

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