Skad00sh / Mbed OS ES_CW2_Dev

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

Dependencies:   Crypto

main.cpp@44:990b5aaf5198, 2019-03-20 (annotated)

Committer:
CallumAlder
Date:
Wed Mar 20 19:56:03 2019 +0000
Revision:
44:990b5aaf5198
Parent:
43:a6d20109b2f2 
compare prints

Who changed what in which revision?

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