Skad00sh / Mbed OS ES_CW2_Dev

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

Dependencies:   Crypto

main.cpp@52:8339cd5c6105, 2019-03-22 (annotated)

Committer:
iachinweze1
Date:
Fri Mar 22 13:40:45 2019 +0000
Revision:
52:8339cd5c6105
Parent:
46:b9081aa50bda 
iz nice but speed control is fakked;

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