Georgios Solidakis / Mbed 2 deprecated IndiCorpEmbedded

Embedded coursework 2.

Dependencies:   IndiCorp mbed-rtos mbed

main.cpp@0:f3f1e48b3e4b, 2018-03-23 (annotated)

Committer:
trod
Date:
Fri Mar 23 20:05:08 2018 +0000
Revision:
0:f3f1e48b3e4b
Final version;

Who changed what in which revision?

UserRevisionLine numberNew contents of line
trod 0:f3f1e48b3e4b 1 //~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~INCLUDES~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
trod 0:f3f1e48b3e4b 2 #include "mbed.h"
trod 0:f3f1e48b3e4b 3 #include "Crypto.h" // Library used for Bitcoin mining.
trod 0:f3f1e48b3e4b 4 #include "rtos.h" // Real time operating system library for threads etc.
trod 0:f3f1e48b3e4b 5
trod 0:f3f1e48b3e4b 6
trod 0:f3f1e48b3e4b 7
trod 0:f3f1e48b3e4b 8
trod 0:f3f1e48b3e4b 9 //~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~DEFINITIONS~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
trod 0:f3f1e48b3e4b 10
trod 0:f3f1e48b3e4b 11 //~~~~~~~~~~~~~~Photointerrupter pins~~~~~~~~~~~~~~
trod 0:f3f1e48b3e4b 12 #define I1pin D2
trod 0:f3f1e48b3e4b 13 #define I2pin D11
trod 0:f3f1e48b3e4b 14 #define I3pin D12
trod 0:f3f1e48b3e4b 15
trod 0:f3f1e48b3e4b 16
trod 0:f3f1e48b3e4b 17 ////~~~~~~~~~~Incremental encoder pins//~~~~~~~~~~~
trod 0:f3f1e48b3e4b 18 #define CHA D7
trod 0:f3f1e48b3e4b 19 #define CHB D8
trod 0:f3f1e48b3e4b 20
trod 0:f3f1e48b3e4b 21
trod 0:f3f1e48b3e4b 22 //~~Motor Drive output pins~/Mask in output byte~~~
trod 0:f3f1e48b3e4b 23 #define L1Lpin D4 //0x01
trod 0:f3f1e48b3e4b 24 #define L1Hpin D5 //0x02
trod 0:f3f1e48b3e4b 25 #define L2Lpin D3 //0x04
trod 0:f3f1e48b3e4b 26 #define L2Hpin D6 //0x08
trod 0:f3f1e48b3e4b 27 #define L3Lpin D9 //0x10
trod 0:f3f1e48b3e4b 28 #define L3Hpin D10 //0x20
trod 0:f3f1e48b3e4b 29
trod 0:f3f1e48b3e4b 30
trod 0:f3f1e48b3e4b 31 //~~~~~~~~Maximum command length accepted~~~~~~~~~~~
trod 0:f3f1e48b3e4b 32 #define MAXCMDLENGTH 18
trod 0:f3f1e48b3e4b 33
trod 0:f3f1e48b3e4b 34
trod 0:f3f1e48b3e4b 35 //~~~~~~~~Maximum PWM allowed due to 50% restriction
trod 0:f3f1e48b3e4b 36 #define MAXPWM 1000
trod 0:f3f1e48b3e4b 37
trod 0:f3f1e48b3e4b 38
trod 0:f3f1e48b3e4b 39 //~~~~~~~Enumeration of message identifiers~~~~~~~~~
trod 0:f3f1e48b3e4b 40 enum MsgCode {Msg_motorState, Msg_hashRate, Msg_nonceMatch, Msg_keyAdded, Msg_velocityOut, Msg_velocityIn, Msg_positionIn, Msg_positionOut, Msg_rotations, Msg_torque, Msg_error};
trod 0:f3f1e48b3e4b 41
trod 0:f3f1e48b3e4b 42
trod 0:f3f1e48b3e4b 43 //~~~~~~~New data type to carry the messages~~~~~~~~
trod 0:f3f1e48b3e4b 44 typedef struct {
trod 0:f3f1e48b3e4b 45 MsgCode code;
trod 0:f3f1e48b3e4b 46 uint32_t data;
trod 0:f3f1e48b3e4b 47 } message_t;
trod 0:f3f1e48b3e4b 48
trod 0:f3f1e48b3e4b 49
trod 0:f3f1e48b3e4b 50
trod 0:f3f1e48b3e4b 51 //~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~Global Variables~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
trod 0:f3f1e48b3e4b 52
trod 0:f3f1e48b3e4b 53 //Mapping from sequential drive states to motor phase outputs
trod 0:f3f1e48b3e4b 54 /*
trod 0:f3f1e48b3e4b 55 State L1 L2 L3
trod 0:f3f1e48b3e4b 56 0 H - L
trod 0:f3f1e48b3e4b 57 1 - H L
trod 0:f3f1e48b3e4b 58 2 L H -
trod 0:f3f1e48b3e4b 59 3 L - H
trod 0:f3f1e48b3e4b 60 4 - L H
trod 0:f3f1e48b3e4b 61 5 H L -
trod 0:f3f1e48b3e4b 62 6 - - -
trod 0:f3f1e48b3e4b 63 7 - - -
trod 0:f3f1e48b3e4b 64 */
trod 0:f3f1e48b3e4b 65
trod 0:f3f1e48b3e4b 66
trod 0:f3f1e48b3e4b 67 //~~~~~~~~~~~Drive state to output table~~~~~~~~~~~~
trod 0:f3f1e48b3e4b 68 const int8_t driveTable[] = {0x12,0x18,0x09,0x21,0x24,0x06,0x00,0x00};
trod 0:f3f1e48b3e4b 69
trod 0:f3f1e48b3e4b 70
trod 0:f3f1e48b3e4b 71 //Mapping from interrupter inputs to sequential rotor states. 0x00 and 0x07 are not valid.
trod 0:f3f1e48b3e4b 72 const int8_t stateMap[] = {0x07,0x05,0x03,0x04,0x01,0x00,0x02,0x07};
trod 0:f3f1e48b3e4b 73 //Alternative if phase order of input or drive is reversed.
trod 0:f3f1e48b3e4b 74 //const int8_t stateMap[] = {0x07,0x01,0x03,0x02,0x05,0x00,0x04,0x07};
trod 0:f3f1e48b3e4b 75
trod 0:f3f1e48b3e4b 76
trod 0:f3f1e48b3e4b 77 ////~~~~~~~~~Phase lead to make motor spin~~~~~~~~~
trod 0:f3f1e48b3e4b 78 int8_t lead = 2; //2 for forwards, -2 for backwards
trod 0:f3f1e48b3e4b 79
trod 0:f3f1e48b3e4b 80
trod 0:f3f1e48b3e4b 81 //~~~~~~~~~~~~~~~~~~Rotor states~~~~~~~~~~~~~~~~~~~
trod 0:f3f1e48b3e4b 82 int8_t orState = 0; // Rotor offset at motor state 0
trod 0:f3f1e48b3e4b 83 volatile int8_t intStateOld = 0; // Motor old state. Type is volatile since
trod 0:f3f1e48b3e4b 84 // its value may change in ISR
trod 0:f3f1e48b3e4b 85
trod 0:f3f1e48b3e4b 86
trod 0:f3f1e48b3e4b 87 //~~~~~~~~~~~~~~~~~~~Status LED~~~~~~~~~~~~~~~~~~~~
trod 0:f3f1e48b3e4b 88 DigitalOut led1(LED1);
trod 0:f3f1e48b3e4b 89
trod 0:f3f1e48b3e4b 90
trod 0:f3f1e48b3e4b 91 //~~~~~~~~~~~~~Photointerrupter inputs~~~~~~~~~~~~~
trod 0:f3f1e48b3e4b 92 InterruptIn I1(I1pin);
trod 0:f3f1e48b3e4b 93 InterruptIn I2(I2pin);
trod 0:f3f1e48b3e4b 94 InterruptIn I3(I3pin);
trod 0:f3f1e48b3e4b 95
trod 0:f3f1e48b3e4b 96
trod 0:f3f1e48b3e4b 97 //~~~~~~~~~~~~~~Motor Drive outputs~~~~~~~~~~~~~~~~
trod 0:f3f1e48b3e4b 98 PwmOut L1L(L1Lpin);
trod 0:f3f1e48b3e4b 99 DigitalOut L1H(L1Hpin);
trod 0:f3f1e48b3e4b 100 PwmOut L2L(L2Lpin);
trod 0:f3f1e48b3e4b 101 DigitalOut L2H(L2Hpin);
trod 0:f3f1e48b3e4b 102 PwmOut L3L(L3Lpin);
trod 0:f3f1e48b3e4b 103 DigitalOut L3H(L3Hpin);
trod 0:f3f1e48b3e4b 104
trod 0:f3f1e48b3e4b 105
trod 0:f3f1e48b3e4b 106 //~Dats structure to pass information between threads~
trod 0:f3f1e48b3e4b 107 Mail<message_t,16> outMessages;
trod 0:f3f1e48b3e4b 108
trod 0:f3f1e48b3e4b 109
trod 0:f3f1e48b3e4b 110 //~~~~~~~~~~~~~~~~~~~~Queue~~~~~~~~~~~~~~~~~~~~~~~~
trod 0:f3f1e48b3e4b 111 Queue<void, 8> inCharQ;
trod 0:f3f1e48b3e4b 112
trod 0:f3f1e48b3e4b 113
trod 0:f3f1e48b3e4b 114 //~~~~~~~~~~~~Serial command buffer~~~~~~~~~~~~~~~
trod 0:f3f1e48b3e4b 115 char newCmd[MAXCMDLENGTH];
trod 0:f3f1e48b3e4b 116 volatile uint8_t cmdIndx = 0;
trod 0:f3f1e48b3e4b 117
trod 0:f3f1e48b3e4b 118
trod 0:f3f1e48b3e4b 119 //~~~~~~~~~~Key to be passed for mining~~~~~~~~~~~
trod 0:f3f1e48b3e4b 120 volatile uint64_t newKey; // Key
trod 0:f3f1e48b3e4b 121 Mutex newKey_mutex; // Restrict access to prevent deadlock.
trod 0:f3f1e48b3e4b 122
trod 0:f3f1e48b3e4b 123
trod 0:f3f1e48b3e4b 124 //~~~~~~~~~~~~~~Initial conditions~~~~~~~~~~~~~~~~
trod 0:f3f1e48b3e4b 125 volatile uint32_t motorPower = 300; // motor toque
trod 0:f3f1e48b3e4b 126 volatile float targetVel = 45.0;
trod 0:f3f1e48b3e4b 127 volatile float targetRot = 459.0;
trod 0:f3f1e48b3e4b 128
trod 0:f3f1e48b3e4b 129
trod 0:f3f1e48b3e4b 130 //~~~~~~~~~~~Motor position variable~~~~~~~~~~~~~~
trod 0:f3f1e48b3e4b 131 volatile int32_t motorPos; // Motor position updated by interrupt.
trod 0:f3f1e48b3e4b 132
trod 0:f3f1e48b3e4b 133
trod 0:f3f1e48b3e4b 134 //~~~~~~~~~~Serial port connection~~~~~~~~~~~~~~~~
trod 0:f3f1e48b3e4b 135 RawSerial pc(SERIAL_TX, SERIAL_RX);
trod 0:f3f1e48b3e4b 136
trod 0:f3f1e48b3e4b 137
trod 0:f3f1e48b3e4b 138
trod 0:f3f1e48b3e4b 139
trod 0:f3f1e48b3e4b 140 //~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~Threads~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
trod 0:f3f1e48b3e4b 141
trod 0:f3f1e48b3e4b 142 Thread commOutT(osPriorityAboveNormal,1024); // Output to serial port.
trod 0:f3f1e48b3e4b 143 Thread commInT(osPriorityAboveNormal,1024); // Input from serial port.
trod 0:f3f1e48b3e4b 144 Thread motorCtrlT(osPriorityNormal,1024); // Motor control thread.
trod 0:f3f1e48b3e4b 145
trod 0:f3f1e48b3e4b 146
trod 0:f3f1e48b3e4b 147
trod 0:f3f1e48b3e4b 148
trod 0:f3f1e48b3e4b 149 //~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~Function declarations~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
trod 0:f3f1e48b3e4b 150
trod 0:f3f1e48b3e4b 151 void motorOut(int8_t driveState, uint32_t pw);
trod 0:f3f1e48b3e4b 152 inline int8_t readRotorState();
trod 0:f3f1e48b3e4b 153 int8_t motorHome();
trod 0:f3f1e48b3e4b 154 void motorISR();
trod 0:f3f1e48b3e4b 155 void cmdParser();
trod 0:f3f1e48b3e4b 156 void commOutFn();
trod 0:f3f1e48b3e4b 157 void putMessage(MsgCode code, uint32_t data);
trod 0:f3f1e48b3e4b 158 void serialISR();
trod 0:f3f1e48b3e4b 159 void commInFn();
trod 0:f3f1e48b3e4b 160 void motorCtrlFn();
trod 0:f3f1e48b3e4b 161 void motorCtrlTick();
trod 0:f3f1e48b3e4b 162
trod 0:f3f1e48b3e4b 163
trod 0:f3f1e48b3e4b 164
trod 0:f3f1e48b3e4b 165
trod 0:f3f1e48b3e4b 166 //~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~Main~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
trod 0:f3f1e48b3e4b 167
trod 0:f3f1e48b3e4b 168 int main() {
trod 0:f3f1e48b3e4b 169 //~~~~~~~~~~~~~Initial serial prints~~~~~~~~~~~~~
trod 0:f3f1e48b3e4b 170 pc.printf("\n\r\n\r Hello \n\r");
trod 0:f3f1e48b3e4b 171 pc.printf("\n\r\n\rGroup: IndiCorp \n\r");
trod 0:f3f1e48b3e4b 172 pc.printf("Initial hardcoded conditions:\n\r");
trod 0:f3f1e48b3e4b 173 pc.printf("\tVelocity:\t%f\n\r", targetVel);
trod 0:f3f1e48b3e4b 174 pc.printf("\tRotation:\t%f\n\r", targetRot);
trod 0:f3f1e48b3e4b 175
trod 0:f3f1e48b3e4b 176
trod 0:f3f1e48b3e4b 177 //~~~~~~~~~~~~~~~Start all threads~~~~~~~~~~~~~~~
trod 0:f3f1e48b3e4b 178 commOutT.start(commOutFn);
trod 0:f3f1e48b3e4b 179 commInT.start(commInFn);
trod 0:f3f1e48b3e4b 180 motorCtrlT.start(motorCtrlFn);
trod 0:f3f1e48b3e4b 181
trod 0:f3f1e48b3e4b 182
trod 0:f3f1e48b3e4b 183 //~~~~~~~~~~~~~~Attach ISR to serial~~~~~~~~~~~~
trod 0:f3f1e48b3e4b 184 pc.attach(&serialISR);
trod 0:f3f1e48b3e4b 185
trod 0:f3f1e48b3e4b 186
trod 0:f3f1e48b3e4b 187 //~~~~~~~~Attach ISR to photointerrupters~~~~~~~
trod 0:f3f1e48b3e4b 188 I1.rise(&motorISR);
trod 0:f3f1e48b3e4b 189 I1.fall(&motorISR);
trod 0:f3f1e48b3e4b 190 I2.rise(&motorISR);
trod 0:f3f1e48b3e4b 191 I2.fall(&motorISR);
trod 0:f3f1e48b3e4b 192 I3.rise(&motorISR);
trod 0:f3f1e48b3e4b 193 I3.fall(&motorISR);
trod 0:f3f1e48b3e4b 194
trod 0:f3f1e48b3e4b 195
trod 0:f3f1e48b3e4b 196 //~~~~~~~~~Declare Bitcoin Variables~~~~~~~~~~~
trod 0:f3f1e48b3e4b 197 SHA256 sha256Inst;
trod 0:f3f1e48b3e4b 198 uint8_t sequence[] = {\
trod 0:f3f1e48b3e4b 199 0x45,0x6D,0x62,0x65,0x64,0x64,0x65,0x64,\
trod 0:f3f1e48b3e4b 200 0x20,0x53,0x79,0x73,0x74,0x65,0x6D,0x73,\
trod 0:f3f1e48b3e4b 201 0x20,0x61,0x72,0x65,0x20,0x66,0x75,0x6E,\
trod 0:f3f1e48b3e4b 202 0x20,0x61,0x6E,0x64,0x20,0x64,0x6F,0x20,\
trod 0:f3f1e48b3e4b 203 0x61,0x77,0x65,0x73,0x6F,0x6D,0x65,0x20,\
trod 0:f3f1e48b3e4b 204 0x74,0x68,0x69,0x6E,0x67,0x73,0x21,0x20,\
trod 0:f3f1e48b3e4b 205 0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,\
trod 0:f3f1e48b3e4b 206 0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00
trod 0:f3f1e48b3e4b 207 };
trod 0:f3f1e48b3e4b 208 uint64_t* key = (uint64_t*)((int)sequence + 48);
trod 0:f3f1e48b3e4b 209 uint64_t* nonce = (uint64_t*)((int)sequence + 56);
trod 0:f3f1e48b3e4b 210 uint8_t hash[32];
trod 0:f3f1e48b3e4b 211 uint32_t sequenceLength = 64;
trod 0:f3f1e48b3e4b 212 uint32_t hashCounter = 0;
trod 0:f3f1e48b3e4b 213 Timer bitcoinTimer;
trod 0:f3f1e48b3e4b 214
trod 0:f3f1e48b3e4b 215
trod 0:f3f1e48b3e4b 216 //Set PWM period to max 2000 due to hardware limitations
trod 0:f3f1e48b3e4b 217 L1L.period_us(2000);
trod 0:f3f1e48b3e4b 218 L2L.period_us(2000);
trod 0:f3f1e48b3e4b 219 L3L.period_us(2000);
trod 0:f3f1e48b3e4b 220
trod 0:f3f1e48b3e4b 221
trod 0:f3f1e48b3e4b 222 /* Run the motor synchronisation: orState is subtracted from future rotor
trod 0:f3f1e48b3e4b 223 state inputs to align rotor and motor states */
trod 0:f3f1e48b3e4b 224 orState = motorHome();
trod 0:f3f1e48b3e4b 225 pc.printf("Rotor origin: %x\n\r", orState); //Print state for debugging purposes.
trod 0:f3f1e48b3e4b 226
trod 0:f3f1e48b3e4b 227
trod 0:f3f1e48b3e4b 228 //~~~~~~Give the motor a kick to begin~~~~~~~~
trod 0:f3f1e48b3e4b 229 motorISR();
trod 0:f3f1e48b3e4b 230
trod 0:f3f1e48b3e4b 231
trod 0:f3f1e48b3e4b 232
trod 0:f3f1e48b3e4b 233 //~~~~~~~~~~~~~~~~Mining loop~~~~~~~~~~~~~~~~~
trod 0:f3f1e48b3e4b 234 bitcoinTimer.start(); // start timer
trod 0:f3f1e48b3e4b 235 while (1) {
trod 0:f3f1e48b3e4b 236 newKey_mutex.lock();
trod 0:f3f1e48b3e4b 237 (*key) = newKey;
trod 0:f3f1e48b3e4b 238 newKey_mutex.unlock();
trod 0:f3f1e48b3e4b 239 sha256Inst.computeHash(hash, sequence, sequenceLength);
trod 0:f3f1e48b3e4b 240 hashCounter++;
trod 0:f3f1e48b3e4b 241 if ((hash[0]==0) && (hash[1]==0)){
trod 0:f3f1e48b3e4b 242 putMessage(Msg_nonceMatch, *nonce); // matching nonce
trod 0:f3f1e48b3e4b 243 }
trod 0:f3f1e48b3e4b 244
trod 0:f3f1e48b3e4b 245 (*nonce)++;
trod 0:f3f1e48b3e4b 246
trod 0:f3f1e48b3e4b 247 if (bitcoinTimer.read() >= 1){
trod 0:f3f1e48b3e4b 248 putMessage(Msg_hashRate, hashCounter);
trod 0:f3f1e48b3e4b 249 hashCounter=0;
trod 0:f3f1e48b3e4b 250 bitcoinTimer.reset();
trod 0:f3f1e48b3e4b 251 }
trod 0:f3f1e48b3e4b 252 }
trod 0:f3f1e48b3e4b 253 }
trod 0:f3f1e48b3e4b 254
trod 0:f3f1e48b3e4b 255 //~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~Functions Definitions~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
trod 0:f3f1e48b3e4b 256
trod 0:f3f1e48b3e4b 257 //~~~~~~~~~~~~Set a given drive state~~~~~~~~~~~~
trod 0:f3f1e48b3e4b 258 void motorOut(int8_t driveState, uint32_t pw){
trod 0:f3f1e48b3e4b 259
trod 0:f3f1e48b3e4b 260 //Lookup the output byte from the drive state.
trod 0:f3f1e48b3e4b 261 int8_t driveOut = driveTable[driveState & 0x07];
trod 0:f3f1e48b3e4b 262
trod 0:f3f1e48b3e4b 263 //Turn off first
trod 0:f3f1e48b3e4b 264 if (~driveOut & 0x01) L1L.pulsewidth_us(0);
trod 0:f3f1e48b3e4b 265 if (~driveOut & 0x02) L1H = 1;
trod 0:f3f1e48b3e4b 266 if (~driveOut & 0x04) L2L.pulsewidth_us(0);
trod 0:f3f1e48b3e4b 267 if (~driveOut & 0x08) L2H = 1;
trod 0:f3f1e48b3e4b 268 if (~driveOut & 0x10) L3L.pulsewidth_us(0);
trod 0:f3f1e48b3e4b 269 if (~driveOut & 0x20) L3H = 1;
trod 0:f3f1e48b3e4b 270
trod 0:f3f1e48b3e4b 271 //Then turn on
trod 0:f3f1e48b3e4b 272 if (driveOut & 0x01) L1L.pulsewidth_us(pw);
trod 0:f3f1e48b3e4b 273 if (driveOut & 0x02) L1H = 0;
trod 0:f3f1e48b3e4b 274 if (driveOut & 0x04) L2L.pulsewidth_us(pw);
trod 0:f3f1e48b3e4b 275 if (driveOut & 0x08) L2H = 0;
trod 0:f3f1e48b3e4b 276 if (driveOut & 0x10) L3L.pulsewidth_us(pw);
trod 0:f3f1e48b3e4b 277 if (driveOut & 0x20) L3H = 0;
trod 0:f3f1e48b3e4b 278 }
trod 0:f3f1e48b3e4b 279
trod 0:f3f1e48b3e4b 280
trod 0:f3f1e48b3e4b 281 //~Convert photointerrupter inputs to a rotor state~
trod 0:f3f1e48b3e4b 282 inline int8_t readRotorState(){
trod 0:f3f1e48b3e4b 283 return stateMap[I1 + 2*I2 + 4*I3];
trod 0:f3f1e48b3e4b 284 }
trod 0:f3f1e48b3e4b 285
trod 0:f3f1e48b3e4b 286 //~~~~~~Basic motor synchronisation routine~~~~~~
trod 0:f3f1e48b3e4b 287 int8_t motorHome() {
trod 0:f3f1e48b3e4b 288 //Put the motor in drive state 0 and wait for it to stabilise
trod 0:f3f1e48b3e4b 289 motorOut(0, MAXPWM); // set to max PWM
trod 0:f3f1e48b3e4b 290 wait(2.0);
trod 0:f3f1e48b3e4b 291
trod 0:f3f1e48b3e4b 292 //Get the rotor state
trod 0:f3f1e48b3e4b 293 return readRotorState();
trod 0:f3f1e48b3e4b 294 }
trod 0:f3f1e48b3e4b 295
trod 0:f3f1e48b3e4b 296
trod 0:f3f1e48b3e4b 297 //~~~~~~~~~Motor ISR (photointerrupters)~~~~~~~~~
trod 0:f3f1e48b3e4b 298 void motorISR() {
trod 0:f3f1e48b3e4b 299 static int8_t oldRotorState;
trod 0:f3f1e48b3e4b 300 int8_t rotorState = readRotorState();
trod 0:f3f1e48b3e4b 301
trod 0:f3f1e48b3e4b 302 motorOut((rotorState-orState+lead+6)%6,motorPower);
trod 0:f3f1e48b3e4b 303
trod 0:f3f1e48b3e4b 304 // update motorPosition and oldRotorState
trod 0:f3f1e48b3e4b 305 if (rotorState - oldRotorState == 5) motorPos--;
trod 0:f3f1e48b3e4b 306 else if (rotorState - oldRotorState == -5) motorPos++;
trod 0:f3f1e48b3e4b 307 else motorPos += (rotorState - oldRotorState);
trod 0:f3f1e48b3e4b 308 oldRotorState = rotorState;
trod 0:f3f1e48b3e4b 309 }
trod 0:f3f1e48b3e4b 310
trod 0:f3f1e48b3e4b 311
trod 0:f3f1e48b3e4b 312 //~~~~~Decode messages to print on serial port~~~~~
trod 0:f3f1e48b3e4b 313 void commOutFn() {
trod 0:f3f1e48b3e4b 314 while(1) {
trod 0:f3f1e48b3e4b 315 osEvent newEvent = outMessages.get();
trod 0:f3f1e48b3e4b 316 message_t *pMessage = (message_t*)newEvent.value.p;
trod 0:f3f1e48b3e4b 317
trod 0:f3f1e48b3e4b 318 //Case switch to choose serial output based on incoming message
trod 0:f3f1e48b3e4b 319 switch(pMessage->code) {
trod 0:f3f1e48b3e4b 320 case Msg_motorState:
trod 0:f3f1e48b3e4b 321 pc.printf("The motor is currently in state %x\n\r", pMessage->data);
trod 0:f3f1e48b3e4b 322 break;
trod 0:f3f1e48b3e4b 323 case Msg_hashRate:
trod 0:f3f1e48b3e4b 324 pc.printf("Mining at a rate of %.2f Hash/s\n\r", (int32_t)pMessage->data);
trod 0:f3f1e48b3e4b 325 break;
trod 0:f3f1e48b3e4b 326 case Msg_nonceMatch:
trod 0:f3f1e48b3e4b 327 pc.printf("Nonce found: %x\n\r", pMessage->data);
trod 0:f3f1e48b3e4b 328 break;
trod 0:f3f1e48b3e4b 329 case Msg_keyAdded:
trod 0:f3f1e48b3e4b 330 pc.printf("New key added:\t0x%016x\n\r", pMessage->data);
trod 0:f3f1e48b3e4b 331 break;
trod 0:f3f1e48b3e4b 332 case Msg_torque:
trod 0:f3f1e48b3e4b 333 pc.printf("Motor torque set to:\t%d\n\r", pMessage->data);
trod 0:f3f1e48b3e4b 334 break;
trod 0:f3f1e48b3e4b 335 case Msg_velocityIn:
trod 0:f3f1e48b3e4b 336 pc.printf("Target velocity set to:\t%.2f\n\r", targetVel);
trod 0:f3f1e48b3e4b 337 break;
trod 0:f3f1e48b3e4b 338 case Msg_velocityOut:
trod 0:f3f1e48b3e4b 339 pc.printf("Current Velocity:\t%.2f\n\r", \
trod 0:f3f1e48b3e4b 340 (float)((int32_t)pMessage->data / 6));
trod 0:f3f1e48b3e4b 341 break;
trod 0:f3f1e48b3e4b 342 case Msg_positionIn:
trod 0:f3f1e48b3e4b 343 pc.printf("Target rotation set to:\t%.2f\n\r", \
trod 0:f3f1e48b3e4b 344 (float)((int32_t)pMessage->data / 6));
trod 0:f3f1e48b3e4b 345 break;
trod 0:f3f1e48b3e4b 346 case Msg_positionOut:
trod 0:f3f1e48b3e4b 347 pc.printf("Current position:\t%.2f\n\r", \
trod 0:f3f1e48b3e4b 348 (float)((int32_t)pMessage->data / 6));
trod 0:f3f1e48b3e4b 349 break;
trod 0:f3f1e48b3e4b 350 case Msg_error:
trod 0:f3f1e48b3e4b 351 pc.printf("Debugging position:%x\n\r", pMessage->data);
trod 0:f3f1e48b3e4b 352 break;
trod 0:f3f1e48b3e4b 353 default:
trod 0:f3f1e48b3e4b 354 pc.printf("Unknown Error. Data: %x\n\r", pMessage->data);
trod 0:f3f1e48b3e4b 355 break;
trod 0:f3f1e48b3e4b 356 }
trod 0:f3f1e48b3e4b 357 outMessages.free(pMessage);
trod 0:f3f1e48b3e4b 358 }
trod 0:f3f1e48b3e4b 359 }
trod 0:f3f1e48b3e4b 360
trod 0:f3f1e48b3e4b 361
trod 0:f3f1e48b3e4b 362 //~~~~~~~~~Put message in Mail queue~~~~~~~~~~~
trod 0:f3f1e48b3e4b 363 void putMessage(MsgCode code, uint32_t data){
trod 0:f3f1e48b3e4b 364 message_t *pMessage = outMessages.alloc();
trod 0:f3f1e48b3e4b 365 pMessage->code = code;
trod 0:f3f1e48b3e4b 366 pMessage->data = data;
trod 0:f3f1e48b3e4b 367 outMessages.put(pMessage);
trod 0:f3f1e48b3e4b 368 }
trod 0:f3f1e48b3e4b 369
trod 0:f3f1e48b3e4b 370
trod 0:f3f1e48b3e4b 371 //~~~~Receive & decode serial input command~~~~~
trod 0:f3f1e48b3e4b 372 void commInFn() {
trod 0:f3f1e48b3e4b 373 while (1) {
trod 0:f3f1e48b3e4b 374 osEvent newEvent = inCharQ.get();
trod 0:f3f1e48b3e4b 375 uint8_t newChar = *((uint8_t*)(&newEvent.value.p));
trod 0:f3f1e48b3e4b 376 pc.putc(newChar);
trod 0:f3f1e48b3e4b 377 if(cmdIndx >= MAXCMDLENGTH){ //Make sure there is no overflow in comand.
trod 0:f3f1e48b3e4b 378 cmdIndx = 0;
trod 0:f3f1e48b3e4b 379 putMessage(Msg_error, 1);
trod 0:f3f1e48b3e4b 380 }
trod 0:f3f1e48b3e4b 381 else{
trod 0:f3f1e48b3e4b 382 if(newChar != '\r'){ //While the command is not over,
trod 0:f3f1e48b3e4b 383 newCmd[cmdIndx] = newChar; //save input character and
trod 0:f3f1e48b3e4b 384 cmdIndx++; //advance index
trod 0:f3f1e48b3e4b 385 }
trod 0:f3f1e48b3e4b 386 else{
trod 0:f3f1e48b3e4b 387 newCmd[cmdIndx] = '\0'; //When the command is finally over,
trod 0:f3f1e48b3e4b 388 cmdIndx = 0; //reset index and
trod 0:f3f1e48b3e4b 389 cmdParser(); //parse the command for decoding.
trod 0:f3f1e48b3e4b 390 }
trod 0:f3f1e48b3e4b 391 }
trod 0:f3f1e48b3e4b 392 }
trod 0:f3f1e48b3e4b 393 }
trod 0:f3f1e48b3e4b 394
trod 0:f3f1e48b3e4b 395
trod 0:f3f1e48b3e4b 396
trod 0:f3f1e48b3e4b 397 //~~~~~~~~~~~~~Decode the command~~~~~~~~~~~
trod 0:f3f1e48b3e4b 398 void cmdParser(){
trod 0:f3f1e48b3e4b 399 switch(newCmd[0]) {
trod 0:f3f1e48b3e4b 400 case 'K':
trod 0:f3f1e48b3e4b 401 newKey_mutex.lock(); //Ensure there is no deadlock
trod 0:f3f1e48b3e4b 402 sscanf(newCmd, "K%x", &newKey); //Find desired the Key code
trod 0:f3f1e48b3e4b 403 putMessage(Msg_keyAdded, newKey); //Print it out
trod 0:f3f1e48b3e4b 404 newKey_mutex.unlock();
trod 0:f3f1e48b3e4b 405 break;
trod 0:f3f1e48b3e4b 406 case 'V':
trod 0:f3f1e48b3e4b 407 sscanf(newCmd, "V%f", &targetVel); //Find desired the target velocity
trod 0:f3f1e48b3e4b 408 putMessage(Msg_velocityIn, targetVel); //Print it out
trod 0:f3f1e48b3e4b 409 break;
trod 0:f3f1e48b3e4b 410 case 'R':
trod 0:f3f1e48b3e4b 411 sscanf(newCmd, "R%f", &targetRot); //Find desired target rotation
trod 0:f3f1e48b3e4b 412 putMessage(Msg_positionIn, targetRot); //Print it out
trod 0:f3f1e48b3e4b 413 break;
trod 0:f3f1e48b3e4b 414 case 'T':
trod 0:f3f1e48b3e4b 415 sscanf(newCmd, "T%d", &motorPower); //Find desired target torque
trod 0:f3f1e48b3e4b 416 putMessage(Msg_torque, motorPower); //Print it out
trod 0:f3f1e48b3e4b 417 break;
trod 0:f3f1e48b3e4b 418 default: break;
trod 0:f3f1e48b3e4b 419 }
trod 0:f3f1e48b3e4b 420 }
trod 0:f3f1e48b3e4b 421
trod 0:f3f1e48b3e4b 422
trod 0:f3f1e48b3e4b 423 //~~~~~~~~~~~~~Serial ISR~~~~~~~~~~~~
trod 0:f3f1e48b3e4b 424 void serialISR() {
trod 0:f3f1e48b3e4b 425 uint8_t newChar = pc.getc();
trod 0:f3f1e48b3e4b 426 inCharQ.put((void*)newChar);
trod 0:f3f1e48b3e4b 427 }
trod 0:f3f1e48b3e4b 428
trod 0:f3f1e48b3e4b 429
trod 0:f3f1e48b3e4b 430 //~~~~~~ISR triggered by Ticker~~~~~~
trod 0:f3f1e48b3e4b 431 void motorCtrlTick(){
trod 0:f3f1e48b3e4b 432 motorCtrlT.signal_set(0x1); //Set signal to motor control thread which carries out calculations to avoid CPU blocking
trod 0:f3f1e48b3e4b 433 }
trod 0:f3f1e48b3e4b 434
trod 0:f3f1e48b3e4b 435
trod 0:f3f1e48b3e4b 436 //~~~~~~~~~~~~~Motor control function with proportional controller~~~~~~~~~~~
trod 0:f3f1e48b3e4b 437 void motorCtrlFn() {
trod 0:f3f1e48b3e4b 438
trod 0:f3f1e48b3e4b 439 //~~~~~~~~~~~~~Variables~~~~~~~~~~~~~~~~
trod 0:f3f1e48b3e4b 440 Ticker motorCtrlTicker; //Ticker to ba attached to callback function
trod 0:f3f1e48b3e4b 441 int32_t velocity; //Variable for local velocity calculation
trod 0:f3f1e48b3e4b 442 int32_t locMotorPos; //Local copy of motor position
trod 0:f3f1e48b3e4b 443 static int32_t oldMotorPos = 0; //Old motor position used for calculations
trod 0:f3f1e48b3e4b 444 static uint8_t motorCtrlCounter = 0; //Counter to be reset every 10 iterations to get velocity calculation in seconds
trod 0:f3f1e48b3e4b 445 int32_t torque; //Local variable to set motor torque
trod 0:f3f1e48b3e4b 446 float sError; //Velocity error between target and reality
trod 0:f3f1e48b3e4b 447 float rError; //Rotation error between target and reality
trod 0:f3f1e48b3e4b 448 static float rErrorOld; //Old rotation error used for calculation
trod 0:f3f1e48b3e4b 449
trod 0:f3f1e48b3e4b 450 //~~~Controller constants~~~~
trod 0:f3f1e48b3e4b 451 int32_t Kp1=22; //Proportional controller constants
trod 0:f3f1e48b3e4b 452 int32_t Kp2=22; //Calculated by trial and error to give optimal accuracy
trod 0:f3f1e48b3e4b 453 float Kd=15.5;
trod 0:f3f1e48b3e4b 454
trod 0:f3f1e48b3e4b 455
trod 0:f3f1e48b3e4b 456 //Attach ticker to callback function that will run every 100 ms
trod 0:f3f1e48b3e4b 457 motorCtrlTicker.attach_us(&motorCtrlTick,100000);
trod 0:f3f1e48b3e4b 458
trod 0:f3f1e48b3e4b 459
trod 0:f3f1e48b3e4b 460
trod 0:f3f1e48b3e4b 461 while(1) {
trod 0:f3f1e48b3e4b 462 motorCtrlT.signal_wait(0x1); // Wait for thread signal.
trod 0:f3f1e48b3e4b 463
trod 0:f3f1e48b3e4b 464 //Initial velocity calculation and report
trod 0:f3f1e48b3e4b 465 locMotorPos = motorPos; //Read global variable motorPos which is updated by interrupt and store it in local variable
trod 0:f3f1e48b3e4b 466 velocity = (locMotorPos - oldMotorPos) * 10; //Proceed with calculation
trod 0:f3f1e48b3e4b 467 oldMotorPos = locMotorPos; //Update old motor position
trod 0:f3f1e48b3e4b 468 motorCtrlCounter++; //Advance counter
trod 0:f3f1e48b3e4b 469 if (motorCtrlCounter >= 10) { //Every 10th iteration
trod 0:f3f1e48b3e4b 470 motorCtrlCounter = 0; //Reset counter
trod 0:f3f1e48b3e4b 471 putMessage(Msg_velocityOut, velocity); //Report the current velocity
trod 0:f3f1e48b3e4b 472 putMessage(Msg_positionOut, locMotorPos); //Report the current position
trod 0:f3f1e48b3e4b 473 }
trod 0:f3f1e48b3e4b 474
trod 0:f3f1e48b3e4b 475 //~~~~~Speed controller~~~~~~
trod 0:f3f1e48b3e4b 476 sError = (targetVel * 6) - abs(velocity); //Read global variable targetVel updated by interrupt and calculate error between target and reality
trod 0:f3f1e48b3e4b 477 int32_t Ys; //Initialise controller output Ys
trod 0:f3f1e48b3e4b 478 if (sError == -abs(velocity)) { //Check if user entered V0,
trod 0:f3f1e48b3e4b 479 Ys = MAXPWM; //and set the output to maximum as specified
trod 0:f3f1e48b3e4b 480 }
trod 0:f3f1e48b3e4b 481 else {
trod 0:f3f1e48b3e4b 482 Ys = (int)(Kp1 * sError); //If the user didn't enter V0 implement controller transfer function: Ys = Kp * (s -|v|) where,
trod 0:f3f1e48b3e4b 483 } //Ys = controller output, Kp = prop controller constant, s = target velocity and v is the measured velocity
trod 0:f3f1e48b3e4b 484
trod 0:f3f1e48b3e4b 485 //~~~~~Rotation control~~~~~~
trod 0:f3f1e48b3e4b 486 rError = targetRot - (locMotorPos/6); //Read global variable targetRot updated by interrupt and calculate the rotation error.
trod 0:f3f1e48b3e4b 487 int32_t Yr; //Initialise controller output Yr
trod 0:f3f1e48b3e4b 488 Yr = Kp2*rError + Kd*(rError - rErrorOld); //Implement controller transfer function Ys= Kp*Er + Kd* (dEr/dt)
trod 0:f3f1e48b3e4b 489 rErrorOld = rError; //Update rotation error
trod 0:f3f1e48b3e4b 490 if(rError < 0){ //Use the sign of the error to set controller wrt direction of rotation
trod 0:f3f1e48b3e4b 491 Ys = -Ys;
trod 0:f3f1e48b3e4b 492 }
trod 0:f3f1e48b3e4b 493
trod 0:f3f1e48b3e4b 494 if((velocity>=0 && Ys<Yr) || (velocity<0 && Ys>Yr)){ //Choose Ys or Yr based on distance from target value so that it takes
trod 0:f3f1e48b3e4b 495 torque = Ys; //appropriate steps in the right direction to reach target value
trod 0:f3f1e48b3e4b 496 }
trod 0:f3f1e48b3e4b 497 else{
trod 0:f3f1e48b3e4b 498 torque = Yr;
trod 0:f3f1e48b3e4b 499 }
trod 0:f3f1e48b3e4b 500 if(torque < 0){ //Variable torque cannot be negative since it sets the PWM
trod 0:f3f1e48b3e4b 501 torque = -torque; //Hence we make the value positive,
trod 0:f3f1e48b3e4b 502 lead = -2; //and instead set the direction to the opposite one
trod 0:f3f1e48b3e4b 503 }
trod 0:f3f1e48b3e4b 504 else{
trod 0:f3f1e48b3e4b 505 lead = 2;
trod 0:f3f1e48b3e4b 506 }
trod 0:f3f1e48b3e4b 507 if(torque > MAXPWM){ //In case the calculated PWM is higher than our maximum 50% allowance,
trod 0:f3f1e48b3e4b 508 torque = MAXPWM; //Set it to our max.
trod 0:f3f1e48b3e4b 509 }
trod 0:f3f1e48b3e4b 510 motorPower = torque; //Lastly, update global variable motorPower which is updated by interrupt
trod 0:f3f1e48b3e4b 511 }
trod 0:f3f1e48b3e4b 512 }