![](/media/cache/img/default_profile.jpg.50x50_q85.jpg)
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?
User | Revision | Line number | New 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 | } |