Yao Xu
Embedded System CW2
Dependencies: Crypto_light mbed-rtos mbed
Fork of
ES_CW2_Starter
by 
Edward Stott
main.cpp@4:bcd27085832d, 2018-03-23 (annotated)
- Committer:
- ylx15
- Date:
- Fri Mar 23 20:39:06 2018 +0000
- Revision:
- 4:bcd27085832d
- Parent:
- 3:569b35e2a602
Who changed what in which revision?
| User | Revision | Line number | New contents of line |
|---|---|---|---|
| estott | 0:de4320f74764 | 1 | #include "mbed.h" |
| ylx15 | 4:bcd27085832d | 2 | #include "SHA256.h" |
| ylx15 | 4:bcd27085832d | 3 | #include "rtos.h" |
| ylx15 | 4:bcd27085832d | 4 | #include <algorithm> |
| estott | 0:de4320f74764 | 5 | |
| estott | 0:de4320f74764 | 6 | //Photointerrupter input pins |
| estott | 0:de4320f74764 | 7 | #define I1pin D2 |
| estott | 2:4e88faab6988 | 8 | #define I2pin D11 |
| estott | 2:4e88faab6988 | 9 | #define I3pin D12 |
| estott | 2:4e88faab6988 | 10 | |
| estott | 2:4e88faab6988 | 11 | //Incremental encoder input pins |
| estott | 2:4e88faab6988 | 12 | #define CHA D7 |
| estott | 2:4e88faab6988 | 13 | #define CHB D8 |
| estott | 0:de4320f74764 | 14 | |
| estott | 0:de4320f74764 | 15 | //Motor Drive output pins //Mask in output byte |
| estott | 2:4e88faab6988 | 16 | #define L1Lpin D4 //0x01 |
| estott | 2:4e88faab6988 | 17 | #define L1Hpin D5 //0x02 |
| estott | 2:4e88faab6988 | 18 | #define L2Lpin D3 //0x04 |
| estott | 2:4e88faab6988 | 19 | #define L2Hpin D6 //0x08 |
| estott | 2:4e88faab6988 | 20 | #define L3Lpin D9 //0x10 |
| estott | 0:de4320f74764 | 21 | #define L3Hpin D10 //0x20 |
| estott | 0:de4320f74764 | 22 | |
| estott | 0:de4320f74764 | 23 | //Mapping from sequential drive states to motor phase outputs |
| estott | 0:de4320f74764 | 24 | /* |
| estott | 0:de4320f74764 | 25 | State L1 L2 L3 |
| estott | 0:de4320f74764 | 26 | 0 H - L |
| estott | 0:de4320f74764 | 27 | 1 - H L |
| estott | 0:de4320f74764 | 28 | 2 L H - |
| estott | 0:de4320f74764 | 29 | 3 L - H |
| estott | 0:de4320f74764 | 30 | 4 - L H |
| estott | 0:de4320f74764 | 31 | 5 H L - |
| estott | 0:de4320f74764 | 32 | 6 - - - |
| estott | 0:de4320f74764 | 33 | 7 - - - |
| estott | 0:de4320f74764 | 34 | */ |
| estott | 0:de4320f74764 | 35 | //Drive state to output table |
| estott | 0:de4320f74764 | 36 | const int8_t driveTable[] = {0x12,0x18,0x09,0x21,0x24,0x06,0x00,0x00}; |
| estott | 2:4e88faab6988 | 37 | |
| estott | 0:de4320f74764 | 38 | //Mapping from interrupter inputs to sequential rotor states. 0x00 and 0x07 are not valid |
| estott | 2:4e88faab6988 | 39 | const int8_t stateMap[] = {0x07,0x05,0x03,0x04,0x01,0x00,0x02,0x07}; |
| estott | 2:4e88faab6988 | 40 | //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 | 41 | |
| estott | 2:4e88faab6988 | 42 | //Phase lead to make motor spin |
| ylx15 | 4:bcd27085832d | 43 | int8_t lead = 2; //2 for forwards, -2 for backwards. (used to be const) |
| estott | 0:de4320f74764 | 44 | |
| estott | 0:de4320f74764 | 45 | //Status LED |
| estott | 0:de4320f74764 | 46 | DigitalOut led1(LED1); |
| estott | 0:de4320f74764 | 47 | |
| estott | 0:de4320f74764 | 48 | //Photointerrupter inputs |
| ylx15 | 4:bcd27085832d | 49 | InterruptIn I1(I1pin); |
| ylx15 | 4:bcd27085832d | 50 | InterruptIn I2(I2pin); |
| ylx15 | 4:bcd27085832d | 51 | InterruptIn I3(I3pin); |
| estott | 0:de4320f74764 | 52 | |
| estott | 0:de4320f74764 | 53 | //Motor Drive outputs |
| ylx15 | 4:bcd27085832d | 54 | PwmOut L1L(L1Lpin); |
| estott | 0:de4320f74764 | 55 | DigitalOut L1H(L1Hpin); |
| ylx15 | 4:bcd27085832d | 56 | PwmOut L2L(L2Lpin); |
| estott | 0:de4320f74764 | 57 | DigitalOut L2H(L2Hpin); |
| ylx15 | 4:bcd27085832d | 58 | PwmOut L3L(L3Lpin); |
| estott | 0:de4320f74764 | 59 | DigitalOut L3H(L3Hpin); |
| estott | 0:de4320f74764 | 60 | |
| ylx15 | 4:bcd27085832d | 61 | |
| ylx15 | 4:bcd27085832d | 62 | ////////////////////////////////////////////////////////// Print to serial port |
| ylx15 | 4:bcd27085832d | 63 | |
| ylx15 | 4:bcd27085832d | 64 | //Class Mail |
| ylx15 | 4:bcd27085832d | 65 | typedef struct{ |
| ylx15 | 4:bcd27085832d | 66 | char label1; |
| ylx15 | 4:bcd27085832d | 67 | char label2; |
| ylx15 | 4:bcd27085832d | 68 | uint8_t code; |
| ylx15 | 4:bcd27085832d | 69 | uint32_t data; |
| ylx15 | 4:bcd27085832d | 70 | } message_t; |
| ylx15 | 4:bcd27085832d | 71 | Mail<message_t, 16> outMessages; |
| ylx15 | 4:bcd27085832d | 72 | |
| ylx15 | 4:bcd27085832d | 73 | //Add message to the queue |
| ylx15 | 4:bcd27085832d | 74 | void putMessage(char label1, char label2, uint8_t code, uint32_t data) { |
| ylx15 | 4:bcd27085832d | 75 | message_t *pMessage = outMessages.alloc(); |
| ylx15 | 4:bcd27085832d | 76 | pMessage->label1 = label1; |
| ylx15 | 4:bcd27085832d | 77 | pMessage->label2 = label2; |
| ylx15 | 4:bcd27085832d | 78 | pMessage->code = code; |
| ylx15 | 4:bcd27085832d | 79 | pMessage->data = data; |
| ylx15 | 4:bcd27085832d | 80 | outMessages.put(pMessage); |
| ylx15 | 4:bcd27085832d | 81 | } |
| ylx15 | 4:bcd27085832d | 82 | |
| ylx15 | 4:bcd27085832d | 83 | //Initialise the serial port |
| ylx15 | 4:bcd27085832d | 84 | RawSerial pc(SERIAL_TX, SERIAL_RX); |
| ylx15 | 4:bcd27085832d | 85 | |
| ylx15 | 4:bcd27085832d | 86 | Thread commOutT(osPriorityHigh,1024); |
| ylx15 | 4:bcd27085832d | 87 | |
| ylx15 | 4:bcd27085832d | 88 | void commOutFn(){ |
| ylx15 | 4:bcd27085832d | 89 | while (1){ |
| ylx15 | 4:bcd27085832d | 90 | // wait for a message to be available in the queue from outmessage |
| ylx15 | 4:bcd27085832d | 91 | osEvent newEvent = outMessages.get(); |
| ylx15 | 4:bcd27085832d | 92 | message_t *pMessage= (message_t*)newEvent.value.p; |
| ylx15 | 4:bcd27085832d | 93 | if ((pMessage -> code == 2) || (pMessage -> code == 9)){ |
| ylx15 | 4:bcd27085832d | 94 | // output hex values only for bitcoin key and nonce |
| ylx15 | 4:bcd27085832d | 95 | pc.printf("Message %d with data 0x%016x\n\r", pMessage -> code, pMessage -> data); |
| ylx15 | 4:bcd27085832d | 96 | }else{ |
| ylx15 | 4:bcd27085832d | 97 | pc.printf("Message %d : %c - %c with data %i\n\r", pMessage -> code, pMessage ->label1, pMessage->label2, pMessage -> data); |
| ylx15 | 4:bcd27085832d | 98 | } |
| ylx15 | 4:bcd27085832d | 99 | outMessages.free(pMessage); // empty previously allocated memory |
| ylx15 | 4:bcd27085832d | 100 | } |
| ylx15 | 4:bcd27085832d | 101 | } |
| ylx15 | 4:bcd27085832d | 102 | |
| ylx15 | 4:bcd27085832d | 103 | ////////////////////////////////////////////////////////////////////// Decoding |
| ylx15 | 4:bcd27085832d | 104 | //Initialize variables |
| ylx15 | 4:bcd27085832d | 105 | Mutex newKey_mutex; |
| ylx15 | 4:bcd27085832d | 106 | volatile uint64_t newKey = 0; |
| ylx15 | 4:bcd27085832d | 107 | // where possible make integers of size less than 32 to assure atomic access |
| ylx15 | 4:bcd27085832d | 108 | volatile uint32_t motorPower = 1000; |
| ylx15 | 4:bcd27085832d | 109 | volatile int32_t newVelocity = 0; |
| ylx15 | 4:bcd27085832d | 110 | volatile int32_t desiredPosition = 0; |
| ylx15 | 4:bcd27085832d | 111 | volatile int32_t desiredRotations = 0; |
| ylx15 | 4:bcd27085832d | 112 | |
| ylx15 | 4:bcd27085832d | 113 | Queue<void, 8> inCharQ; |
| ylx15 | 4:bcd27085832d | 114 | |
| ylx15 | 4:bcd27085832d | 115 | int32_t motorPosition; |
| ylx15 | 4:bcd27085832d | 116 | |
| ylx15 | 4:bcd27085832d | 117 | // serialISR receive each incoming byte and place it in the queue |
| ylx15 | 4:bcd27085832d | 118 | void serialISR(){ |
| ylx15 | 4:bcd27085832d | 119 | // receive incoming byte. called to prevent the interrupt from retriggering |
| ylx15 | 4:bcd27085832d | 120 | uint8_t newChar = pc.getc(); |
| ylx15 | 4:bcd27085832d | 121 | inCharQ.put((void*)newChar); // place it in a queue |
| ylx15 | 4:bcd27085832d | 122 | } |
| ylx15 | 4:bcd27085832d | 123 | |
| ylx15 | 4:bcd27085832d | 124 | // Code for the decoding thread |
| ylx15 | 4:bcd27085832d | 125 | const uint8_t arr_size = 25; |
| ylx15 | 4:bcd27085832d | 126 | char newCmd [arr_size]; // create array large enough -> depends on commands |
| ylx15 | 4:bcd27085832d | 127 | uint8_t buffPosition = 0; |
| ylx15 | 4:bcd27085832d | 128 | |
| ylx15 | 4:bcd27085832d | 129 | Thread ReceiveBytesT(osPriorityNormal,1024); |
| ylx15 | 4:bcd27085832d | 130 | |
| ylx15 | 4:bcd27085832d | 131 | void ReceiveBytesFn(){ |
| ylx15 | 4:bcd27085832d | 132 | pc.attach(&serialISR); |
| ylx15 | 4:bcd27085832d | 133 | while(1) { |
| ylx15 | 4:bcd27085832d | 134 | osEvent newEvent = inCharQ.get(); |
| ylx15 | 4:bcd27085832d | 135 | uint8_t newChar = (uint8_t)newEvent.value.p; |
| ylx15 | 4:bcd27085832d | 136 | |
| ylx15 | 4:bcd27085832d | 137 | if (buffPosition < arr_size) { |
| ylx15 | 4:bcd27085832d | 138 | // determine when the command is finished |
| ylx15 | 4:bcd27085832d | 139 | if (newChar == '\r'){ // equivalent of the enter key |
| ylx15 | 4:bcd27085832d | 140 | newCmd[buffPosition] = '\0'; // signs the end for sscanf |
| ylx15 | 4:bcd27085832d | 141 | |
| ylx15 | 4:bcd27085832d | 142 | //Bitcoin Key |
| ylx15 | 4:bcd27085832d | 143 | if (newCmd[0]=='K'){ |
| ylx15 | 4:bcd27085832d | 144 | newKey_mutex.lock(); |
| ylx15 | 4:bcd27085832d | 145 | //decode the command and set the new key value |
| ylx15 | 4:bcd27085832d | 146 | sscanf(newCmd, "K%x", &newKey); |
| ylx15 | 4:bcd27085832d | 147 | newKey_mutex.unlock(); |
| ylx15 | 4:bcd27085832d | 148 | } |
| ylx15 | 4:bcd27085832d | 149 | |
| ylx15 | 4:bcd27085832d | 150 | //Set motor Torque/ motorPwer |
| ylx15 | 4:bcd27085832d | 151 | if (newCmd[0]=='T'){ |
| ylx15 | 4:bcd27085832d | 152 | // no need for mutex because uint32_t access is atomic |
| ylx15 | 4:bcd27085832d | 153 | sscanf(newCmd, "T%i", &motorPower); |
| ylx15 | 4:bcd27085832d | 154 | } |
| ylx15 | 4:bcd27085832d | 155 | |
| ylx15 | 4:bcd27085832d | 156 | //Set rotational speed |
| ylx15 | 4:bcd27085832d | 157 | if (newCmd[0]=='V'){ |
| ylx15 | 4:bcd27085832d | 158 | sscanf(newCmd, "V%i", &newVelocity); |
| ylx15 | 4:bcd27085832d | 159 | } |
| ylx15 | 4:bcd27085832d | 160 | |
| ylx15 | 4:bcd27085832d | 161 | //Set Position Control |
| ylx15 | 4:bcd27085832d | 162 | if (newCmd[0]=='P'){ |
| ylx15 | 4:bcd27085832d | 163 | sscanf(newCmd, "P%i", &desiredPosition); |
| ylx15 | 4:bcd27085832d | 164 | } |
| ylx15 | 4:bcd27085832d | 165 | |
| ylx15 | 4:bcd27085832d | 166 | //Set Rotation Control |
| ylx15 | 4:bcd27085832d | 167 | if (newCmd[0]=='R'){ |
| ylx15 | 4:bcd27085832d | 168 | sscanf(newCmd, "R%i", &desiredRotations); |
| ylx15 | 4:bcd27085832d | 169 | desiredPosition = motorPosition + desiredRotations*6; |
| ylx15 | 4:bcd27085832d | 170 | } |
| ylx15 | 4:bcd27085832d | 171 | |
| ylx15 | 4:bcd27085832d | 172 | buffPosition = 0; // reset the buff position |
| ylx15 | 4:bcd27085832d | 173 | } |
| ylx15 | 4:bcd27085832d | 174 | else{ |
| ylx15 | 4:bcd27085832d | 175 | newCmd[buffPosition] = newChar; |
| ylx15 | 4:bcd27085832d | 176 | buffPosition++; |
| ylx15 | 4:bcd27085832d | 177 | } |
| ylx15 | 4:bcd27085832d | 178 | } |
| ylx15 | 4:bcd27085832d | 179 | } |
| ylx15 | 4:bcd27085832d | 180 | } |
| ylx15 | 4:bcd27085832d | 181 | |
| ylx15 | 4:bcd27085832d | 182 | ///////////////////////////////////////////////////////////////// Motor Control |
| ylx15 | 4:bcd27085832d | 183 | |
| estott | 0:de4320f74764 | 184 | //Set a given drive state |
| ylx15 | 4:bcd27085832d | 185 | void motorOut(int8_t driveState, uint32_t motor_torque){ |
| estott | 0:de4320f74764 | 186 | |
| estott | 2:4e88faab6988 | 187 | //Lookup the output byte from the drive state. |
| estott | 2:4e88faab6988 | 188 | int8_t driveOut = driveTable[driveState & 0x07]; |
| estott | 2:4e88faab6988 | 189 | |
| estott | 2:4e88faab6988 | 190 | //Turn off first |
| ylx15 | 4:bcd27085832d | 191 | if (~driveOut & 0x01) L1L.pulsewidth_us(0); |
| estott | 2:4e88faab6988 | 192 | if (~driveOut & 0x02) L1H = 1; |
| ylx15 | 4:bcd27085832d | 193 | if (~driveOut & 0x04) L2L.pulsewidth_us(0); |
| estott | 2:4e88faab6988 | 194 | if (~driveOut & 0x08) L2H = 1; |
| ylx15 | 4:bcd27085832d | 195 | if (~driveOut & 0x10) L3L.pulsewidth_us(0); |
| estott | 2:4e88faab6988 | 196 | if (~driveOut & 0x20) L3H = 1; |
| estott | 2:4e88faab6988 | 197 | |
| estott | 2:4e88faab6988 | 198 | //Then turn on |
| ylx15 | 4:bcd27085832d | 199 | if (driveOut & 0x01) L1L.pulsewidth_us(motor_torque); |
| estott | 2:4e88faab6988 | 200 | if (driveOut & 0x02) L1H = 0; |
| ylx15 | 4:bcd27085832d | 201 | if (driveOut & 0x04) L2L.pulsewidth_us(motor_torque); |
| estott | 2:4e88faab6988 | 202 | if (driveOut & 0x08) L2H = 0; |
| ylx15 | 4:bcd27085832d | 203 | if (driveOut & 0x10) L3L.pulsewidth_us(motor_torque); |
| estott | 2:4e88faab6988 | 204 | if (driveOut & 0x20) L3H = 0; |
| estott | 0:de4320f74764 | 205 | } |
| estott | 0:de4320f74764 | 206 | |
| estott | 2:4e88faab6988 | 207 | //Convert photointerrupter inputs to a rotor state |
| estott | 0:de4320f74764 | 208 | inline int8_t readRotorState(){ |
| estott | 2:4e88faab6988 | 209 | return stateMap[I1 + 2*I2 + 4*I3]; |
| estott | 0:de4320f74764 | 210 | } |
| estott | 0:de4320f74764 | 211 | |
| estott | 0:de4320f74764 | 212 | //Basic synchronisation routine |
| estott | 2:4e88faab6988 | 213 | int8_t motorHome() { |
| estott | 0:de4320f74764 | 214 | //Put the motor in drive state 0 and wait for it to stabilise |
| ylx15 | 4:bcd27085832d | 215 | motorOut(0, 1000); |
| estott | 3:569b35e2a602 | 216 | wait(2.0); |
| estott | 0:de4320f74764 | 217 | |
| estott | 0:de4320f74764 | 218 | //Get the rotor state |
| estott | 2:4e88faab6988 | 219 | return readRotorState(); |
| estott | 0:de4320f74764 | 220 | } |
| ylx15 | 4:bcd27085832d | 221 | int8_t orState = 0; //Rotot offset at motor state 0 |
| ylx15 | 4:bcd27085832d | 222 | |
| ylx15 | 4:bcd27085832d | 223 | volatile float ys = 0; |
| ylx15 | 4:bcd27085832d | 224 | volatile float yr = 0; |
| ylx15 | 4:bcd27085832d | 225 | volatile float y_tmp = 0; |
| ylx15 | 4:bcd27085832d | 226 | volatile int32_t v = 0; |
| ylx15 | 4:bcd27085832d | 227 | |
| ylx15 | 4:bcd27085832d | 228 | void motorISR(){ |
| estott | 0:de4320f74764 | 229 | |
| ylx15 | 4:bcd27085832d | 230 | // choose between ys and yr according to the positions |
| ylx15 | 4:bcd27085832d | 231 | if (v>=0){ |
| ylx15 | 4:bcd27085832d | 232 | if(min(ys,yr)==yr){ |
| ylx15 | 4:bcd27085832d | 233 | y_tmp = yr; |
| ylx15 | 4:bcd27085832d | 234 | }else{ |
| ylx15 | 4:bcd27085832d | 235 | y_tmp = ys; |
| ylx15 | 4:bcd27085832d | 236 | } |
| ylx15 | 4:bcd27085832d | 237 | }else{ |
| ylx15 | 4:bcd27085832d | 238 | if(max(ys,yr)==yr){ |
| ylx15 | 4:bcd27085832d | 239 | y_tmp = yr; |
| ylx15 | 4:bcd27085832d | 240 | }else{ |
| ylx15 | 4:bcd27085832d | 241 | y_tmp = ys; |
| ylx15 | 4:bcd27085832d | 242 | } |
| ylx15 | 4:bcd27085832d | 243 | } |
| ylx15 | 4:bcd27085832d | 244 | |
| ylx15 | 4:bcd27085832d | 245 | |
| ylx15 | 4:bcd27085832d | 246 | // V0 overrides everything |
| ylx15 | 4:bcd27085832d | 247 | if ((abs(y_tmp) > 1000)||(newVelocity == 0)){ |
| ylx15 | 4:bcd27085832d | 248 | motorPower = 1000; |
| ylx15 | 4:bcd27085832d | 249 | }else{ |
| ylx15 | 4:bcd27085832d | 250 | motorPower = abs(y_tmp); |
| ylx15 | 4:bcd27085832d | 251 | } |
| ylx15 | 4:bcd27085832d | 252 | if ((y_tmp>=0)||(newVelocity ==0)){ |
| ylx15 | 4:bcd27085832d | 253 | lead = 2; |
| ylx15 | 4:bcd27085832d | 254 | } |
| ylx15 | 4:bcd27085832d | 255 | else { |
| ylx15 | 4:bcd27085832d | 256 | lead = -2; |
| ylx15 | 4:bcd27085832d | 257 | } |
| ylx15 | 4:bcd27085832d | 258 | |
| ylx15 | 4:bcd27085832d | 259 | |
| ylx15 | 4:bcd27085832d | 260 | // a count of 6 will equal one revolution |
| ylx15 | 4:bcd27085832d | 261 | static int8_t oldRotorState; |
| ylx15 | 4:bcd27085832d | 262 | int8_t rotorState = readRotorState(); |
| ylx15 | 4:bcd27085832d | 263 | motorOut((rotorState-orState+lead+6)%6, motorPower); |
| ylx15 | 4:bcd27085832d | 264 | if (rotorState - oldRotorState == 5) motorPosition--; |
| ylx15 | 4:bcd27085832d | 265 | else if (rotorState - oldRotorState == -5) motorPosition++; |
| ylx15 | 4:bcd27085832d | 266 | else motorPosition += (rotorState - oldRotorState); |
| ylx15 | 4:bcd27085832d | 267 | oldRotorState = rotorState; |
| ylx15 | 4:bcd27085832d | 268 | |
| ylx15 | 4:bcd27085832d | 269 | } |
| ylx15 | 4:bcd27085832d | 270 | |
| ylx15 | 4:bcd27085832d | 271 | |
| ylx15 | 4:bcd27085832d | 272 | |
| ylx15 | 4:bcd27085832d | 273 | Thread motorCtrlT (osPriorityNormal,512); // 1024 too big |
| ylx15 | 4:bcd27085832d | 274 | |
| ylx15 | 4:bcd27085832d | 275 | void motorCtrlTick(){ |
| ylx15 | 4:bcd27085832d | 276 | motorCtrlT.signal_set(0x1); |
| ylx15 | 4:bcd27085832d | 277 | } |
| ylx15 | 4:bcd27085832d | 278 | |
| ylx15 | 4:bcd27085832d | 279 | void motorCtrlFn(){ |
| ylx15 | 4:bcd27085832d | 280 | int32_t motorposition_old = 0; |
| ylx15 | 4:bcd27085832d | 281 | int32_t v_calc_counter = 0; |
| ylx15 | 4:bcd27085832d | 282 | int32_t kp = 23; |
| ylx15 | 4:bcd27085832d | 283 | int32_t kd = 18; |
| ylx15 | 4:bcd27085832d | 284 | int32_t localMotorPosition = 0; |
| ylx15 | 4:bcd27085832d | 285 | int32_t localDesiredPosition = 0; |
| ylx15 | 4:bcd27085832d | 286 | int32_t local_s = 0; |
| ylx15 | 4:bcd27085832d | 287 | int32_t Er = 0; |
| ylx15 | 4:bcd27085832d | 288 | int32_t Er_old = 0; |
| ylx15 | 4:bcd27085832d | 289 | int32_t dErdt = 0; |
| ylx15 | 4:bcd27085832d | 290 | |
| ylx15 | 4:bcd27085832d | 291 | Ticker motorCtrlTicker; |
| ylx15 | 4:bcd27085832d | 292 | motorCtrlTicker.attach_us(&motorCtrlTick,100000); // 100ms = 100000us |
| ylx15 | 4:bcd27085832d | 293 | while(1){ |
| ylx15 | 4:bcd27085832d | 294 | Timer t; |
| ylx15 | 4:bcd27085832d | 295 | t.start(); |
| ylx15 | 4:bcd27085832d | 296 | motorCtrlT.signal_wait(0x1); // from here onward it executes once 0.1s |
| ylx15 | 4:bcd27085832d | 297 | |
| ylx15 | 4:bcd27085832d | 298 | // make local copies to compute computation |
| ylx15 | 4:bcd27085832d | 299 | // becasue these variables can be updated at anytime |
| ylx15 | 4:bcd27085832d | 300 | localDesiredPosition = desiredPosition; |
| ylx15 | 4:bcd27085832d | 301 | localMotorPosition = motorPosition; |
| ylx15 | 4:bcd27085832d | 302 | local_s = newVelocity; |
| ylx15 | 4:bcd27085832d | 303 | Er = localDesiredPosition-localMotorPosition; |
| ylx15 | 4:bcd27085832d | 304 | |
| ylx15 | 4:bcd27085832d | 305 | // below is the velocity per second since this runs every 0.1s |
| ylx15 | 4:bcd27085832d | 306 | v = (localMotorPosition-motorposition_old)/(6*t.read()); |
| ylx15 | 4:bcd27085832d | 307 | dErdt = (Er-Er_old)/t.read(); |
| ylx15 | 4:bcd27085832d | 308 | t.reset(); |
| ylx15 | 4:bcd27085832d | 309 | |
| ylx15 | 4:bcd27085832d | 310 | motorposition_old = localMotorPosition; |
| ylx15 | 4:bcd27085832d | 311 | Er_old = Er; |
| ylx15 | 4:bcd27085832d | 312 | |
| ylx15 | 4:bcd27085832d | 313 | v_calc_counter ++; |
| ylx15 | 4:bcd27085832d | 314 | |
| ylx15 | 4:bcd27085832d | 315 | if (v_calc_counter >= 10){ |
| ylx15 | 4:bcd27085832d | 316 | //putMessage('c','v',3, v); //current velocity |
| ylx15 | 4:bcd27085832d | 317 | v_calc_counter = 0; |
| ylx15 | 4:bcd27085832d | 318 | } |
| ylx15 | 4:bcd27085832d | 319 | |
| ylx15 | 4:bcd27085832d | 320 | yr = kp*Er + kd*dErdt; |
| ylx15 | 4:bcd27085832d | 321 | |
| ylx15 | 4:bcd27085832d | 322 | // there were problems with the function abs(). use if-else instead |
| ylx15 | 4:bcd27085832d | 323 | if (v>=0){ |
| ylx15 | 4:bcd27085832d | 324 | if (Er >= 0){ |
| ylx15 | 4:bcd27085832d | 325 | ys = kp*(local_s - v); |
| ylx15 | 4:bcd27085832d | 326 | }else{ |
| ylx15 | 4:bcd27085832d | 327 | ys = -1*kp*(local_s - v); |
| ylx15 | 4:bcd27085832d | 328 | } |
| ylx15 | 4:bcd27085832d | 329 | }else{ |
| ylx15 | 4:bcd27085832d | 330 | if (Er >= 0){ |
| ylx15 | 4:bcd27085832d | 331 | ys = kp*(local_s + v); |
| ylx15 | 4:bcd27085832d | 332 | }else{ |
| ylx15 | 4:bcd27085832d | 333 | ys = -1*kp*(local_s + v); |
| ylx15 | 4:bcd27085832d | 334 | } |
| ylx15 | 4:bcd27085832d | 335 | } |
| ylx15 | 4:bcd27085832d | 336 | |
| ylx15 | 4:bcd27085832d | 337 | } |
| ylx15 | 4:bcd27085832d | 338 | } |
| ylx15 | 4:bcd27085832d | 339 | |
| ylx15 | 4:bcd27085832d | 340 | |
| estott | 0:de4320f74764 | 341 | //Main |
| estott | 0:de4320f74764 | 342 | int main() { |
| estott | 2:4e88faab6988 | 343 | |
| ylx15 | 4:bcd27085832d | 344 | L1L.period_us(2000); |
| ylx15 | 4:bcd27085832d | 345 | L2L.period_us(2000); |
| ylx15 | 4:bcd27085832d | 346 | L3L.period_us(2000); |
| ylx15 | 4:bcd27085832d | 347 | |
| estott | 0:de4320f74764 | 348 | pc.printf("Hello\n\r"); |
| estott | 0:de4320f74764 | 349 | |
| estott | 0:de4320f74764 | 350 | //Run the motor synchronisation |
| estott | 2:4e88faab6988 | 351 | orState = motorHome(); |
| estott | 2:4e88faab6988 | 352 | pc.printf("Rotor origin: %x\n\r",orState); |
| estott | 2:4e88faab6988 | 353 | //orState is subtracted from future rotor state inputs to align rotor and motor states |
| estott | 0:de4320f74764 | 354 | |
| ylx15 | 4:bcd27085832d | 355 | I1.rise(&motorISR); |
| ylx15 | 4:bcd27085832d | 356 | I1.fall(&motorISR); |
| ylx15 | 4:bcd27085832d | 357 | I2.rise(&motorISR); |
| ylx15 | 4:bcd27085832d | 358 | I2.fall(&motorISR); |
| ylx15 | 4:bcd27085832d | 359 | I3.rise(&motorISR); |
| ylx15 | 4:bcd27085832d | 360 | I3.fall(&motorISR); |
| ylx15 | 4:bcd27085832d | 361 | |
| ylx15 | 4:bcd27085832d | 362 | commOutT.start(commOutFn); |
| ylx15 | 4:bcd27085832d | 363 | ReceiveBytesT.start(ReceiveBytesFn); |
| ylx15 | 4:bcd27085832d | 364 | motorCtrlT.start(motorCtrlFn); |
| ylx15 | 4:bcd27085832d | 365 | |
| ylx15 | 4:bcd27085832d | 366 | SHA256 tmp; |
| ylx15 | 4:bcd27085832d | 367 | |
| ylx15 | 4:bcd27085832d | 368 | uint8_t sequence[] = {0x45,0x6D,0x62,0x65,0x64,0x64,0x65,0x64,0x20,0x53,0x79,0x73,0x74,0x65,0x6D,0x73,0x20,0x61,0x72,0x65,0x20,0x66,0x75,0x6E,0x20,0x61,0x6E,0x64,0x20,0x64,0x6F,0x20,0x61,0x77,0x65,0x73,0x6F,0x6D,0x65,0x20,0x74,0x68,0x69,0x6E,0x67,0x73,0x21,0x20,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00}; |
| ylx15 | 4:bcd27085832d | 369 | uint64_t* key = (uint64_t*)((int)sequence + 48); |
| ylx15 | 4:bcd27085832d | 370 | uint64_t* nonce = (uint64_t*)((int)sequence + 56); |
| ylx15 | 4:bcd27085832d | 371 | uint8_t hash[32]; |
| ylx15 | 4:bcd27085832d | 372 | |
| ylx15 | 4:bcd27085832d | 373 | Timer t; |
| ylx15 | 4:bcd27085832d | 374 | t.start(); |
| ylx15 | 4:bcd27085832d | 375 | uint32_t hashcount = 0; |
| ylx15 | 4:bcd27085832d | 376 | |
| estott | 0:de4320f74764 | 377 | //Poll the rotor state and set the motor outputs accordingly to spin the motor |
| estott | 1:184cb0870c04 | 378 | while (1) { |
| ylx15 | 4:bcd27085832d | 379 | newKey_mutex.lock(); |
| ylx15 | 4:bcd27085832d | 380 | *key = newKey; |
| ylx15 | 4:bcd27085832d | 381 | newKey_mutex.unlock(); |
| ylx15 | 4:bcd27085832d | 382 | |
| ylx15 | 4:bcd27085832d | 383 | tmp.computeHash(hash, sequence, 64); |
| ylx15 | 4:bcd27085832d | 384 | *nonce = *nonce + 1; |
| ylx15 | 4:bcd27085832d | 385 | // increase the hashcount after every computeHash |
| ylx15 | 4:bcd27085832d | 386 | hashcount ++; |
| ylx15 | 4:bcd27085832d | 387 | |
| ylx15 | 4:bcd27085832d | 388 | |
| ylx15 | 4:bcd27085832d | 389 | if (t.read()>=1){ |
| ylx15 | 4:bcd27085832d | 390 | putMessage('H','R',1, hashcount); // hash rate per second |
| ylx15 | 4:bcd27085832d | 391 | hashcount = 0; |
| ylx15 | 4:bcd27085832d | 392 | t.reset(); |
| ylx15 | 4:bcd27085832d | 393 | putMessage('c','v',3, v); //current velocity in rotation per second |
| ylx15 | 4:bcd27085832d | 394 | putMessage('T','V',4, newVelocity); // Target Velocity in rotation per second |
| ylx15 | 4:bcd27085832d | 395 | putMessage('c','p',5, motorPosition); // current position |
| ylx15 | 4:bcd27085832d | 396 | putMessage('T','P',6, desiredPosition); // Target Position |
| ylx15 | 4:bcd27085832d | 397 | //putMessage('K','K',9, *key); |
| estott | 0:de4320f74764 | 398 | } |
| ylx15 | 4:bcd27085832d | 399 | |
| ylx15 | 4:bcd27085832d | 400 | if ((hash[0]==0)&&(hash[1]==0)){ |
| ylx15 | 4:bcd27085832d | 401 | putMessage('B','N',2,*nonce); // Bitcoin Nonce |
| ylx15 | 4:bcd27085832d | 402 | } |
| ylx15 | 4:bcd27085832d | 403 | |
| ylx15 | 4:bcd27085832d | 404 | |
| estott | 2:4e88faab6988 | 405 | } |
| ylx15 | 4:bcd27085832d | 406 | |
| ylx15 | 4:bcd27085832d | 407 | } |