yiwen zou
/
ES_CW2_Final
Coursework 2 Motor Control
main.cpp@12:9b7a85e17146, 2019-03-23 (annotated)
- Committer:
- eavennnn
- Date:
- Sat Mar 23 14:37:38 2019 +0000
- Revision:
- 12:9b7a85e17146
- Parent:
- 11:f101c1f3d3bd
Edited Name
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
estott | 0:de4320f74764 | 1 | #include "mbed.h" |
eavennnn | 11:f101c1f3d3bd | 2 | #include "Crypto.h" |
eavennnn | 11:f101c1f3d3bd | 3 | #include "rtos.h" |
estott | 0:de4320f74764 | 4 | |
estott | 0:de4320f74764 | 5 | //Photointerrupter input pins |
estott | 10:a4b5723b6c9d | 6 | #define I1pin D3 |
estott | 10:a4b5723b6c9d | 7 | #define I2pin D6 |
estott | 10:a4b5723b6c9d | 8 | #define I3pin D5 |
estott | 2:4e88faab6988 | 9 | |
estott | 2:4e88faab6988 | 10 | //Incremental encoder input pins |
estott | 10:a4b5723b6c9d | 11 | #define CHApin D12 |
estott | 10:a4b5723b6c9d | 12 | #define CHBpin D11 |
estott | 0:de4320f74764 | 13 | |
estott | 0:de4320f74764 | 14 | //Motor Drive output pins //Mask in output byte |
estott | 10:a4b5723b6c9d | 15 | #define L1Lpin D1 //0x01 |
estott | 10:a4b5723b6c9d | 16 | #define L1Hpin A3 //0x02 |
estott | 10:a4b5723b6c9d | 17 | #define L2Lpin D0 //0x04 |
estott | 10:a4b5723b6c9d | 18 | #define L2Hpin A6 //0x08 |
estott | 10:a4b5723b6c9d | 19 | #define L3Lpin D10 //0x10 |
estott | 10:a4b5723b6c9d | 20 | #define L3Hpin D2 //0x20 |
estott | 10:a4b5723b6c9d | 21 | |
estott | 10:a4b5723b6c9d | 22 | #define PWMpin D9 |
estott | 5:08f338b5e4d9 | 23 | |
eavennnn | 11:f101c1f3d3bd | 24 | |
eavennnn | 11:f101c1f3d3bd | 25 | |
eavennnn | 11:f101c1f3d3bd | 26 | |
eavennnn | 11:f101c1f3d3bd | 27 | |
eavennnn | 11:f101c1f3d3bd | 28 | |
eavennnn | 11:f101c1f3d3bd | 29 | Thread motorCtrlT (osPriorityNormal, 1024); |
eavennnn | 11:f101c1f3d3bd | 30 | Thread OutputT (osPriorityNormal,1024); |
eavennnn | 11:f101c1f3d3bd | 31 | Thread DecodeT (osPriorityNormal,1024); |
eavennnn | 11:f101c1f3d3bd | 32 | RawSerial pc(SERIAL_TX, SERIAL_RX); |
eavennnn | 11:f101c1f3d3bd | 33 | |
eavennnn | 11:f101c1f3d3bd | 34 | //Global variables |
eavennnn | 12:9b7a85e17146 | 35 | #define MAX_PWM 2000 |
eavennnn | 11:f101c1f3d3bd | 36 | Queue<void,8> inCharQ; |
eavennnn | 11:f101c1f3d3bd | 37 | volatile uint64_t newKey; |
eavennnn | 11:f101c1f3d3bd | 38 | volatile float newRev = 0.001; //default zero rotation |
eavennnn | 11:f101c1f3d3bd | 39 | volatile float maxSpeed = 50; //1800 rotations per second |
eavennnn | 11:f101c1f3d3bd | 40 | volatile float motorPosition; |
eavennnn | 11:f101c1f3d3bd | 41 | volatile float originalmotorPosition; |
eavennnn | 11:f101c1f3d3bd | 42 | volatile float pulseWidth = MAX_PWM; |
eavennnn | 11:f101c1f3d3bd | 43 | int32_t motorVelocity; |
eavennnn | 11:f101c1f3d3bd | 44 | volatile float kps = 30; |
eavennnn | 11:f101c1f3d3bd | 45 | volatile float kis = 0.75; |
eavennnn | 11:f101c1f3d3bd | 46 | volatile float kpr = 20; |
eavennnn | 11:f101c1f3d3bd | 47 | volatile float kdr = 8.5; |
eavennnn | 11:f101c1f3d3bd | 48 | |
eavennnn | 11:f101c1f3d3bd | 49 | |
eavennnn | 11:f101c1f3d3bd | 50 | //Protect variable from being accessed by other threads |
eavennnn | 11:f101c1f3d3bd | 51 | Mutex newKey_mutex; |
eavennnn | 11:f101c1f3d3bd | 52 | |
eavennnn | 11:f101c1f3d3bd | 53 | |
eavennnn | 11:f101c1f3d3bd | 54 | |
eavennnn | 11:f101c1f3d3bd | 55 | typedef struct{ //define structure of mail |
eavennnn | 11:f101c1f3d3bd | 56 | uint8_t code; |
eavennnn | 11:f101c1f3d3bd | 57 | float data; |
eavennnn | 11:f101c1f3d3bd | 58 | uint64_t longData; |
eavennnn | 11:f101c1f3d3bd | 59 | } mail_t; |
eavennnn | 11:f101c1f3d3bd | 60 | |
eavennnn | 11:f101c1f3d3bd | 61 | Mail<mail_t,16> outMail; |
eavennnn | 11:f101c1f3d3bd | 62 | |
eavennnn | 11:f101c1f3d3bd | 63 | |
eavennnn | 11:f101c1f3d3bd | 64 | void serialISR(){ //Rawserial Interrupts |
eavennnn | 11:f101c1f3d3bd | 65 | uint8_t newChar = pc.getc(); |
eavennnn | 11:f101c1f3d3bd | 66 | inCharQ.put((void*)newChar); |
eavennnn | 11:f101c1f3d3bd | 67 | } |
eavennnn | 11:f101c1f3d3bd | 68 | |
eavennnn | 11:f101c1f3d3bd | 69 | void putMessage(uint8_t code, float data){ //Mail for queueing messages |
eavennnn | 11:f101c1f3d3bd | 70 | mail_t *pMail = outMail.alloc(); |
eavennnn | 11:f101c1f3d3bd | 71 | pMail -> code = code; |
eavennnn | 11:f101c1f3d3bd | 72 | pMail -> data = data; |
eavennnn | 11:f101c1f3d3bd | 73 | outMail.put(pMail); |
eavennnn | 11:f101c1f3d3bd | 74 | } |
eavennnn | 11:f101c1f3d3bd | 75 | |
eavennnn | 11:f101c1f3d3bd | 76 | // Overloaded version of putMessage for int versions of data |
eavennnn | 11:f101c1f3d3bd | 77 | void putMessage(uint8_t code, uint64_t data){ |
eavennnn | 11:f101c1f3d3bd | 78 | mail_t *pMail = outMail.alloc(); |
eavennnn | 11:f101c1f3d3bd | 79 | pMail -> code = code; |
eavennnn | 11:f101c1f3d3bd | 80 | pMail -> longData = data; |
eavennnn | 11:f101c1f3d3bd | 81 | outMail.put(pMail); |
eavennnn | 11:f101c1f3d3bd | 82 | } |
eavennnn | 11:f101c1f3d3bd | 83 | |
eavennnn | 11:f101c1f3d3bd | 84 | |
eavennnn | 11:f101c1f3d3bd | 85 | void Decode(){ //Decode User Input Command |
eavennnn | 11:f101c1f3d3bd | 86 | char newCommand[50]; //Array used to hold commands |
eavennnn | 11:f101c1f3d3bd | 87 | uint8_t index = 0; |
eavennnn | 11:f101c1f3d3bd | 88 | pc.attach(&serialISR); //Attach rawserial |
eavennnn | 11:f101c1f3d3bd | 89 | while(1) { |
eavennnn | 11:f101c1f3d3bd | 90 | osEvent newEvent = inCharQ.get(); //New event created when new character detected |
eavennnn | 11:f101c1f3d3bd | 91 | uint8_t newChar = (uint8_t)newEvent.value.p; |
eavennnn | 11:f101c1f3d3bd | 92 | newCommand[index] = newChar; |
eavennnn | 11:f101c1f3d3bd | 93 | |
eavennnn | 11:f101c1f3d3bd | 94 | if(index == 49) { //Checks whether buffer overflows |
eavennnn | 11:f101c1f3d3bd | 95 | pc.printf("Buffer Overflow!\n\r"); |
eavennnn | 11:f101c1f3d3bd | 96 | index = 0; |
eavennnn | 11:f101c1f3d3bd | 97 | } |
eavennnn | 11:f101c1f3d3bd | 98 | else if(newChar == '\r'){ // \r indicates end of command, checks first character of command and reset buffer |
eavennnn | 11:f101c1f3d3bd | 99 | newCommand[index] = '\0'; |
eavennnn | 11:f101c1f3d3bd | 100 | index = 0; |
eavennnn | 11:f101c1f3d3bd | 101 | if (newCommand[0] == 'K'){ // K -> New Key , case 3 |
eavennnn | 11:f101c1f3d3bd | 102 | newKey_mutex.lock(); |
eavennnn | 11:f101c1f3d3bd | 103 | sscanf(newCommand, "K%x", &newKey); |
eavennnn | 11:f101c1f3d3bd | 104 | putMessage(3,newKey); |
eavennnn | 11:f101c1f3d3bd | 105 | newKey_mutex.unlock(); |
eavennnn | 11:f101c1f3d3bd | 106 | } |
eavennnn | 11:f101c1f3d3bd | 107 | if(newCommand[0] == 'V'){ |
eavennnn | 11:f101c1f3d3bd | 108 | sscanf(newCommand,"V%f",&maxSpeed); |
eavennnn | 11:f101c1f3d3bd | 109 | putMessage(6,maxSpeed); |
eavennnn | 11:f101c1f3d3bd | 110 | if(maxSpeed > 20){ // if the target velocity is large enough |
eavennnn | 11:f101c1f3d3bd | 111 | kps = 30; |
eavennnn | 11:f101c1f3d3bd | 112 | kis = 0.75; |
eavennnn | 11:f101c1f3d3bd | 113 | kpr = 20; |
eavennnn | 11:f101c1f3d3bd | 114 | kdr = 8.5; |
eavennnn | 11:f101c1f3d3bd | 115 | } |
eavennnn | 11:f101c1f3d3bd | 116 | else{ // otherwise for small velocities change the parameters |
eavennnn | 11:f101c1f3d3bd | 117 | kps = 20; |
eavennnn | 11:f101c1f3d3bd | 118 | kis = 2; |
eavennnn | 11:f101c1f3d3bd | 119 | kpr = 18; |
eavennnn | 11:f101c1f3d3bd | 120 | kdr = 5; |
eavennnn | 11:f101c1f3d3bd | 121 | } |
eavennnn | 11:f101c1f3d3bd | 122 | } |
eavennnn | 11:f101c1f3d3bd | 123 | if(newCommand[0] == 'R'){ |
eavennnn | 11:f101c1f3d3bd | 124 | sscanf(newCommand, "R%f", &newRev); |
eavennnn | 11:f101c1f3d3bd | 125 | putMessage(7,newRev); |
eavennnn | 11:f101c1f3d3bd | 126 | originalmotorPosition = motorPosition; |
eavennnn | 11:f101c1f3d3bd | 127 | } |
eavennnn | 11:f101c1f3d3bd | 128 | } |
eavennnn | 11:f101c1f3d3bd | 129 | else { //Keep loading |
eavennnn | 11:f101c1f3d3bd | 130 | index++; |
eavennnn | 11:f101c1f3d3bd | 131 | } |
eavennnn | 11:f101c1f3d3bd | 132 | } |
eavennnn | 11:f101c1f3d3bd | 133 | } |
eavennnn | 11:f101c1f3d3bd | 134 | |
eavennnn | 11:f101c1f3d3bd | 135 | //Serial Outputs |
eavennnn | 11:f101c1f3d3bd | 136 | void OutputMail(){ |
eavennnn | 11:f101c1f3d3bd | 137 | while (1) { |
eavennnn | 11:f101c1f3d3bd | 138 | //New event created when user enters command |
eavennnn | 11:f101c1f3d3bd | 139 | osEvent newEvent = outMail.get(); |
eavennnn | 11:f101c1f3d3bd | 140 | mail_t *pMail = (mail_t*)newEvent.value.p; |
eavennnn | 11:f101c1f3d3bd | 141 | |
eavennnn | 11:f101c1f3d3bd | 142 | //Use switch to handle different cases, case defined by code |
eavennnn | 11:f101c1f3d3bd | 143 | switch(pMail->code){ |
eavennnn | 11:f101c1f3d3bd | 144 | case 1: |
eavennnn | 11:f101c1f3d3bd | 145 | pc.printf("Hash rate %.0f\n\r", pMail->data); |
eavennnn | 11:f101c1f3d3bd | 146 | break; |
eavennnn | 11:f101c1f3d3bd | 147 | case 2: |
eavennnn | 11:f101c1f3d3bd | 148 | pc.printf("Hash computed at 0x%016x\n\r", pMail->longData); |
eavennnn | 11:f101c1f3d3bd | 149 | break; |
eavennnn | 11:f101c1f3d3bd | 150 | case 3: |
eavennnn | 11:f101c1f3d3bd | 151 | pc.printf("Sequence key set to 0x%016llx\n\r", pMail->longData); |
eavennnn | 11:f101c1f3d3bd | 152 | break; |
eavennnn | 11:f101c1f3d3bd | 153 | case 4: |
eavennnn | 11:f101c1f3d3bd | 154 | pc.printf("Motor Position is %.2f\n\r", pMail->data); |
eavennnn | 11:f101c1f3d3bd | 155 | break; |
eavennnn | 11:f101c1f3d3bd | 156 | case 5: |
eavennnn | 11:f101c1f3d3bd | 157 | pc.printf("Motor Velocity is %.2f\n\r", pMail->data); |
eavennnn | 11:f101c1f3d3bd | 158 | break; |
eavennnn | 11:f101c1f3d3bd | 159 | case 6: |
eavennnn | 11:f101c1f3d3bd | 160 | pc.printf("Max Speed now is set to %.2f\n\r", pMail->data); |
eavennnn | 11:f101c1f3d3bd | 161 | break; |
eavennnn | 11:f101c1f3d3bd | 162 | case 7: |
eavennnn | 11:f101c1f3d3bd | 163 | pc.printf("Revolution now is set to %.2f\n\r", pMail->data); |
eavennnn | 11:f101c1f3d3bd | 164 | break; |
eavennnn | 11:f101c1f3d3bd | 165 | default: |
eavennnn | 11:f101c1f3d3bd | 166 | pc.printf("Message %d with data 0x%016x\n\r", pMail->code, pMail->data); |
eavennnn | 11:f101c1f3d3bd | 167 | } |
eavennnn | 11:f101c1f3d3bd | 168 | //free memory location allocated |
eavennnn | 11:f101c1f3d3bd | 169 | outMail.free(pMail); |
eavennnn | 11:f101c1f3d3bd | 170 | } |
eavennnn | 11:f101c1f3d3bd | 171 | } |
estott | 0:de4320f74764 | 172 | |
estott | 0:de4320f74764 | 173 | //Mapping from sequential drive states to motor phase outputs |
estott | 0:de4320f74764 | 174 | /* |
estott | 0:de4320f74764 | 175 | State L1 L2 L3 |
estott | 0:de4320f74764 | 176 | 0 H - L |
estott | 0:de4320f74764 | 177 | 1 - H L |
estott | 0:de4320f74764 | 178 | 2 L H - |
estott | 0:de4320f74764 | 179 | 3 L - H |
estott | 0:de4320f74764 | 180 | 4 - L H |
estott | 0:de4320f74764 | 181 | 5 H L - |
estott | 0:de4320f74764 | 182 | 6 - - - |
estott | 0:de4320f74764 | 183 | 7 - - - |
eavennnn | 11:f101c1f3d3bd | 184 | |
estott | 0:de4320f74764 | 185 | */ |
estott | 0:de4320f74764 | 186 | //Drive state to output table |
estott | 0:de4320f74764 | 187 | const int8_t driveTable[] = {0x12,0x18,0x09,0x21,0x24,0x06,0x00,0x00}; |
estott | 2:4e88faab6988 | 188 | |
estott | 0:de4320f74764 | 189 | //Mapping from interrupter inputs to sequential rotor states. 0x00 and 0x07 are not valid |
estott | 2:4e88faab6988 | 190 | const int8_t stateMap[] = {0x07,0x05,0x03,0x04,0x01,0x00,0x02,0x07}; |
estott | 2:4e88faab6988 | 191 | //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 | 192 | |
estott | 2:4e88faab6988 | 193 | //Phase lead to make motor spin |
eavennnn | 11:f101c1f3d3bd | 194 | int8_t lead = 2; //2 for forwards, -2 for backwards |
eavennnn | 11:f101c1f3d3bd | 195 | |
eavennnn | 11:f101c1f3d3bd | 196 | int8_t orState = 0; //Rotot offset at motor state 0 |
eavennnn | 11:f101c1f3d3bd | 197 | |
eavennnn | 11:f101c1f3d3bd | 198 | |
eavennnn | 11:f101c1f3d3bd | 199 | void motorCtrlTick(){ //call back function that starts motor control |
eavennnn | 11:f101c1f3d3bd | 200 | motorCtrlT.signal_set(0x1); |
eavennnn | 11:f101c1f3d3bd | 201 | } |
eavennnn | 11:f101c1f3d3bd | 202 | |
estott | 0:de4320f74764 | 203 | |
estott | 0:de4320f74764 | 204 | //Status LED |
estott | 0:de4320f74764 | 205 | DigitalOut led1(LED1); |
estott | 0:de4320f74764 | 206 | |
estott | 0:de4320f74764 | 207 | //Photointerrupter inputs |
eavennnn | 11:f101c1f3d3bd | 208 | InterruptIn I1(I1pin); |
eavennnn | 11:f101c1f3d3bd | 209 | InterruptIn I2(I2pin); |
eavennnn | 11:f101c1f3d3bd | 210 | InterruptIn I3(I3pin); |
estott | 0:de4320f74764 | 211 | |
estott | 0:de4320f74764 | 212 | //Motor Drive outputs |
estott | 0:de4320f74764 | 213 | DigitalOut L1L(L1Lpin); |
estott | 0:de4320f74764 | 214 | DigitalOut L1H(L1Hpin); |
estott | 0:de4320f74764 | 215 | DigitalOut L2L(L2Lpin); |
estott | 0:de4320f74764 | 216 | DigitalOut L2H(L2Hpin); |
estott | 0:de4320f74764 | 217 | DigitalOut L3L(L3Lpin); |
estott | 0:de4320f74764 | 218 | DigitalOut L3H(L3Hpin); |
estott | 0:de4320f74764 | 219 | |
eavennnn | 11:f101c1f3d3bd | 220 | //PwmOut Pin |
eavennnn | 11:f101c1f3d3bd | 221 | PwmOut PWMD9(PWMpin); |
eavennnn | 11:f101c1f3d3bd | 222 | |
eavennnn | 11:f101c1f3d3bd | 223 | |
estott | 0:de4320f74764 | 224 | //Set a given drive state |
eavennnn | 11:f101c1f3d3bd | 225 | void motorOut(int8_t driveState, uint32_t motorTorque){ |
estott | 0:de4320f74764 | 226 | |
estott | 2:4e88faab6988 | 227 | //Lookup the output byte from the drive state. |
estott | 2:4e88faab6988 | 228 | int8_t driveOut = driveTable[driveState & 0x07]; |
estott | 2:4e88faab6988 | 229 | |
eavennnn | 11:f101c1f3d3bd | 230 | |
eavennnn | 11:f101c1f3d3bd | 231 | |
estott | 2:4e88faab6988 | 232 | //Turn off first |
estott | 2:4e88faab6988 | 233 | if (~driveOut & 0x01) L1L = 0; |
estott | 2:4e88faab6988 | 234 | if (~driveOut & 0x02) L1H = 1; |
estott | 2:4e88faab6988 | 235 | if (~driveOut & 0x04) L2L = 0; |
estott | 2:4e88faab6988 | 236 | if (~driveOut & 0x08) L2H = 1; |
estott | 2:4e88faab6988 | 237 | if (~driveOut & 0x10) L3L = 0; |
estott | 2:4e88faab6988 | 238 | if (~driveOut & 0x20) L3H = 1; |
estott | 2:4e88faab6988 | 239 | |
estott | 2:4e88faab6988 | 240 | //Then turn on |
estott | 2:4e88faab6988 | 241 | if (driveOut & 0x01) L1L = 1; |
estott | 2:4e88faab6988 | 242 | if (driveOut & 0x02) L1H = 0; |
estott | 2:4e88faab6988 | 243 | if (driveOut & 0x04) L2L = 1; |
estott | 2:4e88faab6988 | 244 | if (driveOut & 0x08) L2H = 0; |
estott | 2:4e88faab6988 | 245 | if (driveOut & 0x10) L3L = 1; |
estott | 2:4e88faab6988 | 246 | if (driveOut & 0x20) L3H = 0; |
eavennnn | 11:f101c1f3d3bd | 247 | |
eavennnn | 11:f101c1f3d3bd | 248 | //d9.write(motorTorque); |
eavennnn | 11:f101c1f3d3bd | 249 | PWMD9.pulsewidth_us(motorTorque); |
eavennnn | 11:f101c1f3d3bd | 250 | |
estott | 0:de4320f74764 | 251 | } |
estott | 0:de4320f74764 | 252 | |
eavennnn | 11:f101c1f3d3bd | 253 | //Convert photointerrupter inputs to a rotor state |
estott | 0:de4320f74764 | 254 | inline int8_t readRotorState(){ |
estott | 2:4e88faab6988 | 255 | return stateMap[I1 + 2*I2 + 4*I3]; |
eavennnn | 11:f101c1f3d3bd | 256 | } |
estott | 0:de4320f74764 | 257 | |
estott | 0:de4320f74764 | 258 | //Basic synchronisation routine |
estott | 2:4e88faab6988 | 259 | int8_t motorHome() { |
eavennnn | 11:f101c1f3d3bd | 260 | //Put the motor in drive state 0 and wait for it to stabilize |
eavennnn | 11:f101c1f3d3bd | 261 | motorOut(0, 700000); |
eavennnn | 11:f101c1f3d3bd | 262 | //motorOut(0); |
estott | 3:569b35e2a602 | 263 | wait(2.0); |
estott | 0:de4320f74764 | 264 | |
estott | 0:de4320f74764 | 265 | //Get the rotor state |
estott | 2:4e88faab6988 | 266 | return readRotorState(); |
estott | 0:de4320f74764 | 267 | } |
eavennnn | 11:f101c1f3d3bd | 268 | |
eavennnn | 11:f101c1f3d3bd | 269 | |
eavennnn | 11:f101c1f3d3bd | 270 | void motorISR(){ |
eavennnn | 11:f101c1f3d3bd | 271 | static int8_t oldrotorState; |
eavennnn | 11:f101c1f3d3bd | 272 | int8_t rotorState = readRotorState(); |
eavennnn | 11:f101c1f3d3bd | 273 | //pc.printf("Im here"); |
eavennnn | 11:f101c1f3d3bd | 274 | motorOut((rotorState-orState+lead+6)%6,pulseWidth); //+6 to make sure the remainder is positive |
eavennnn | 11:f101c1f3d3bd | 275 | |
eavennnn | 11:f101c1f3d3bd | 276 | if (rotorState - oldrotorState == 5) motorPosition --; |
eavennnn | 11:f101c1f3d3bd | 277 | else if (rotorState - oldrotorState == -5) motorPosition ++; |
eavennnn | 11:f101c1f3d3bd | 278 | else motorPosition += (rotorState - oldrotorState); |
eavennnn | 11:f101c1f3d3bd | 279 | oldrotorState = rotorState; |
eavennnn | 11:f101c1f3d3bd | 280 | } |
eavennnn | 11:f101c1f3d3bd | 281 | |
eavennnn | 11:f101c1f3d3bd | 282 | /* |
eavennnn | 11:f101c1f3d3bd | 283 | void Torque(){ |
eavennnn | 11:f101c1f3d3bd | 284 | |
eavennnn | 11:f101c1f3d3bd | 285 | } |
eavennnn | 11:f101c1f3d3bd | 286 | */ |
eavennnn | 11:f101c1f3d3bd | 287 | |
eavennnn | 11:f101c1f3d3bd | 288 | |
eavennnn | 11:f101c1f3d3bd | 289 | void motorCtrlFn(){ |
eavennnn | 11:f101c1f3d3bd | 290 | int32_t counter = 0; |
eavennnn | 11:f101c1f3d3bd | 291 | static int32_t oldmotorPosition; |
eavennnn | 11:f101c1f3d3bd | 292 | Timer t; |
eavennnn | 11:f101c1f3d3bd | 293 | t.start(); |
eavennnn | 11:f101c1f3d3bd | 294 | |
eavennnn | 11:f101c1f3d3bd | 295 | //Define variables and parameters being used for motor control |
eavennnn | 11:f101c1f3d3bd | 296 | float motorPos; |
eavennnn | 11:f101c1f3d3bd | 297 | float Speed; |
eavennnn | 11:f101c1f3d3bd | 298 | float Revolution; |
eavennnn | 11:f101c1f3d3bd | 299 | float outputTorqueS; |
eavennnn | 11:f101c1f3d3bd | 300 | float outputTorqueR; |
eavennnn | 11:f101c1f3d3bd | 301 | float Torque; |
eavennnn | 11:f101c1f3d3bd | 302 | float rateofchangeofPositionError; |
eavennnn | 11:f101c1f3d3bd | 303 | float oldError; |
eavennnn | 11:f101c1f3d3bd | 304 | float errorSum; |
eavennnn | 11:f101c1f3d3bd | 305 | float speedError; |
eavennnn | 11:f101c1f3d3bd | 306 | float positionError; |
eavennnn | 11:f101c1f3d3bd | 307 | float c = 42.0; |
eavennnn | 11:f101c1f3d3bd | 308 | |
eavennnn | 11:f101c1f3d3bd | 309 | //Default Lead |
eavennnn | 11:f101c1f3d3bd | 310 | int8_t outputLeadS = 2; |
eavennnn | 11:f101c1f3d3bd | 311 | int8_t outputLeadR = 2; |
eavennnn | 11:f101c1f3d3bd | 312 | |
eavennnn | 11:f101c1f3d3bd | 313 | //Sign of direction |
eavennnn | 11:f101c1f3d3bd | 314 | int8_t errorSign = 1; |
eavennnn | 11:f101c1f3d3bd | 315 | |
eavennnn | 11:f101c1f3d3bd | 316 | //Define ticker to measure time interval |
eavennnn | 11:f101c1f3d3bd | 317 | Ticker motorCtrlTicker; |
eavennnn | 11:f101c1f3d3bd | 318 | motorCtrlTicker.attach_us(&motorCtrlTick,100000); |
eavennnn | 11:f101c1f3d3bd | 319 | |
eavennnn | 11:f101c1f3d3bd | 320 | while(1){ |
eavennnn | 11:f101c1f3d3bd | 321 | motorCtrlT.signal_wait(0x1); //executes every 100ms |
eavennnn | 11:f101c1f3d3bd | 322 | |
eavennnn | 11:f101c1f3d3bd | 323 | errorSum = 0; |
eavennnn | 11:f101c1f3d3bd | 324 | |
eavennnn | 11:f101c1f3d3bd | 325 | motorPos = motorPosition; |
eavennnn | 11:f101c1f3d3bd | 326 | Speed = maxSpeed*6; |
eavennnn | 11:f101c1f3d3bd | 327 | Revolution = newRev*6; |
eavennnn | 11:f101c1f3d3bd | 328 | |
eavennnn | 11:f101c1f3d3bd | 329 | //Calculate rate of change of position = velocity |
eavennnn | 11:f101c1f3d3bd | 330 | motorVelocity = (motorPos - oldmotorPosition)/t.read(); |
eavennnn | 11:f101c1f3d3bd | 331 | |
eavennnn | 11:f101c1f3d3bd | 332 | |
eavennnn | 11:f101c1f3d3bd | 333 | // |
eavennnn | 11:f101c1f3d3bd | 334 | if(motorVelocity >= 0) errorSign = 1; |
eavennnn | 11:f101c1f3d3bd | 335 | else errorSign = -1; |
eavennnn | 11:f101c1f3d3bd | 336 | |
eavennnn | 11:f101c1f3d3bd | 337 | //Calculate rate of change of position error = differential term |
eavennnn | 11:f101c1f3d3bd | 338 | positionError = Revolution + originalmotorPosition - motorPos; |
eavennnn | 11:f101c1f3d3bd | 339 | rateofchangeofPositionError = (positionError - oldError)/t.read(); |
eavennnn | 11:f101c1f3d3bd | 340 | |
eavennnn | 11:f101c1f3d3bd | 341 | //Calculate speed error |
eavennnn | 11:f101c1f3d3bd | 342 | speedError = Speed - abs(motorVelocity); |
eavennnn | 11:f101c1f3d3bd | 343 | |
eavennnn | 11:f101c1f3d3bd | 344 | |
eavennnn | 11:f101c1f3d3bd | 345 | oldmotorPosition = motorPos; |
eavennnn | 11:f101c1f3d3bd | 346 | |
eavennnn | 11:f101c1f3d3bd | 347 | //Calculate output Torque for speed and position |
eavennnn | 11:f101c1f3d3bd | 348 | |
eavennnn | 11:f101c1f3d3bd | 349 | if(Speed == 0) outputTorqueS = MAX_PWM; |
eavennnn | 11:f101c1f3d3bd | 350 | else outputTorqueS = (kps*((Speed+c)- abs(motorVelocity))+kis*errorSum)*errorSign; |
eavennnn | 11:f101c1f3d3bd | 351 | |
eavennnn | 11:f101c1f3d3bd | 352 | outputTorqueR = kpr*positionError + kdr*rateofchangeofPositionError; |
eavennnn | 11:f101c1f3d3bd | 353 | |
eavennnn | 11:f101c1f3d3bd | 354 | //set upper limit for integral term |
eavennnn | 11:f101c1f3d3bd | 355 | if(kis*errorSum <= MAX_PWM){ |
eavennnn | 11:f101c1f3d3bd | 356 | errorSum += speedError*t.read(); |
eavennnn | 11:f101c1f3d3bd | 357 | } |
eavennnn | 11:f101c1f3d3bd | 358 | |
eavennnn | 11:f101c1f3d3bd | 359 | t.reset(); //ticker reset |
eavennnn | 11:f101c1f3d3bd | 360 | |
eavennnn | 11:f101c1f3d3bd | 361 | //Determine output lead depending on sign of torque |
eavennnn | 11:f101c1f3d3bd | 362 | |
eavennnn | 11:f101c1f3d3bd | 363 | if(outputTorqueR >= 0) { |
eavennnn | 11:f101c1f3d3bd | 364 | outputLeadR = 2; |
eavennnn | 11:f101c1f3d3bd | 365 | } else { |
eavennnn | 11:f101c1f3d3bd | 366 | outputLeadR = -2; |
eavennnn | 11:f101c1f3d3bd | 367 | } |
eavennnn | 11:f101c1f3d3bd | 368 | if(Speed !=0 ){ |
eavennnn | 11:f101c1f3d3bd | 369 | if(outputTorqueS >= 0) { |
eavennnn | 11:f101c1f3d3bd | 370 | outputLeadS = 2; |
eavennnn | 11:f101c1f3d3bd | 371 | } |
eavennnn | 11:f101c1f3d3bd | 372 | else { |
eavennnn | 11:f101c1f3d3bd | 373 | outputLeadS = -2; |
eavennnn | 11:f101c1f3d3bd | 374 | } |
eavennnn | 11:f101c1f3d3bd | 375 | if(outputTorqueS > MAX_PWM) { |
eavennnn | 11:f101c1f3d3bd | 376 | outputTorqueS = MAX_PWM; |
eavennnn | 11:f101c1f3d3bd | 377 | } |
eavennnn | 11:f101c1f3d3bd | 378 | if(outputTorqueS < -MAX_PWM) { |
eavennnn | 11:f101c1f3d3bd | 379 | outputTorqueS = -MAX_PWM; |
eavennnn | 11:f101c1f3d3bd | 380 | } |
eavennnn | 11:f101c1f3d3bd | 381 | } |
eavennnn | 11:f101c1f3d3bd | 382 | else { |
eavennnn | 11:f101c1f3d3bd | 383 | outputTorqueS = MAX_PWM; |
eavennnn | 11:f101c1f3d3bd | 384 | } |
eavennnn | 11:f101c1f3d3bd | 385 | |
eavennnn | 11:f101c1f3d3bd | 386 | |
eavennnn | 11:f101c1f3d3bd | 387 | // pick the slower one to limit speed to maxSpeed |
eavennnn | 11:f101c1f3d3bd | 388 | if(newRev == 0){ |
eavennnn | 11:f101c1f3d3bd | 389 | pulseWidth = abs(outputTorqueS); |
eavennnn | 11:f101c1f3d3bd | 390 | } |
eavennnn | 11:f101c1f3d3bd | 391 | else{ |
eavennnn | 11:f101c1f3d3bd | 392 | if(motorVelocity < 0){ |
eavennnn | 11:f101c1f3d3bd | 393 | if(outputTorqueS >= outputTorqueR){ |
eavennnn | 11:f101c1f3d3bd | 394 | pulseWidth = abs(outputTorqueS); |
eavennnn | 11:f101c1f3d3bd | 395 | lead = outputLeadS; |
eavennnn | 11:f101c1f3d3bd | 396 | } |
eavennnn | 11:f101c1f3d3bd | 397 | else { |
eavennnn | 11:f101c1f3d3bd | 398 | pulseWidth = abs(outputTorqueR); |
eavennnn | 11:f101c1f3d3bd | 399 | lead = outputLeadR; |
eavennnn | 11:f101c1f3d3bd | 400 | } |
eavennnn | 11:f101c1f3d3bd | 401 | } |
eavennnn | 11:f101c1f3d3bd | 402 | else { |
eavennnn | 11:f101c1f3d3bd | 403 | if (outputTorqueS <= outputTorqueR){ |
eavennnn | 11:f101c1f3d3bd | 404 | pulseWidth = abs(outputTorqueS); |
eavennnn | 11:f101c1f3d3bd | 405 | lead = outputLeadS; |
eavennnn | 11:f101c1f3d3bd | 406 | } |
eavennnn | 11:f101c1f3d3bd | 407 | else { |
eavennnn | 11:f101c1f3d3bd | 408 | pulseWidth = abs(outputTorqueR); |
eavennnn | 11:f101c1f3d3bd | 409 | lead = outputLeadR; |
eavennnn | 11:f101c1f3d3bd | 410 | } |
eavennnn | 11:f101c1f3d3bd | 411 | } |
eavennnn | 11:f101c1f3d3bd | 412 | } |
eavennnn | 11:f101c1f3d3bd | 413 | |
eavennnn | 11:f101c1f3d3bd | 414 | if(motorVelocity == 0) motorISR(); |
eavennnn | 11:f101c1f3d3bd | 415 | |
eavennnn | 11:f101c1f3d3bd | 416 | |
eavennnn | 11:f101c1f3d3bd | 417 | //Output position and velocity when counter counts to 10 |
eavennnn | 11:f101c1f3d3bd | 418 | counter++; |
eavennnn | 11:f101c1f3d3bd | 419 | if(counter == 10){ |
eavennnn | 11:f101c1f3d3bd | 420 | counter = 0; |
eavennnn | 11:f101c1f3d3bd | 421 | putMessage(4,(float)(motorPosition/6.0)); |
eavennnn | 11:f101c1f3d3bd | 422 | putMessage(5,(float)(motorVelocity/6.0)); |
eavennnn | 11:f101c1f3d3bd | 423 | } |
eavennnn | 11:f101c1f3d3bd | 424 | |
eavennnn | 11:f101c1f3d3bd | 425 | //Redefine old position error |
eavennnn | 11:f101c1f3d3bd | 426 | oldError = positionError; |
eavennnn | 11:f101c1f3d3bd | 427 | } |
eavennnn | 11:f101c1f3d3bd | 428 | } |
eavennnn | 11:f101c1f3d3bd | 429 | |
eavennnn | 11:f101c1f3d3bd | 430 | |
eavennnn | 11:f101c1f3d3bd | 431 | float hashCount = 0; |
eavennnn | 11:f101c1f3d3bd | 432 | |
eavennnn | 11:f101c1f3d3bd | 433 | void calcHashRate(){ |
eavennnn | 11:f101c1f3d3bd | 434 | putMessage(1,hashCount); |
eavennnn | 11:f101c1f3d3bd | 435 | hashCount = 0; |
eavennnn | 11:f101c1f3d3bd | 436 | } |
eavennnn | 11:f101c1f3d3bd | 437 | |
eavennnn | 11:f101c1f3d3bd | 438 | |
eavennnn | 11:f101c1f3d3bd | 439 | |
estott | 0:de4320f74764 | 440 | |
estott | 0:de4320f74764 | 441 | //Main |
estott | 0:de4320f74764 | 442 | int main() { |
eavennnn | 11:f101c1f3d3bd | 443 | //set up pwm period |
eavennnn | 11:f101c1f3d3bd | 444 | PWMD9.period(0.002f); // 2ms second period |
eavennnn | 11:f101c1f3d3bd | 445 | PWMD9.write(1.0f); // 100% duty cycle, relative to period |
estott | 2:4e88faab6988 | 446 | orState = motorHome(); |
eavennnn | 11:f101c1f3d3bd | 447 | pc.printf("Rotor origin: %x\n\r", orState); |
eavennnn | 11:f101c1f3d3bd | 448 | |
eavennnn | 11:f101c1f3d3bd | 449 | motorCtrlT.start(motorCtrlFn); |
eavennnn | 11:f101c1f3d3bd | 450 | OutputT.start(OutputMail); |
eavennnn | 11:f101c1f3d3bd | 451 | DecodeT.start(Decode); |
eavennnn | 11:f101c1f3d3bd | 452 | |
eavennnn | 11:f101c1f3d3bd | 453 | // Run the motor synchronisation |
eavennnn | 11:f101c1f3d3bd | 454 | pc.printf("Rotor origin: %x\n\r", orState); |
eavennnn | 11:f101c1f3d3bd | 455 | |
eavennnn | 11:f101c1f3d3bd | 456 | // motor controlling interrupt routines |
eavennnn | 11:f101c1f3d3bd | 457 | I1.rise(&motorISR); |
eavennnn | 11:f101c1f3d3bd | 458 | I1.fall(&motorISR); |
eavennnn | 11:f101c1f3d3bd | 459 | I2.rise(&motorISR); |
eavennnn | 11:f101c1f3d3bd | 460 | I2.fall(&motorISR); |
eavennnn | 11:f101c1f3d3bd | 461 | I3.rise(&motorISR); |
eavennnn | 11:f101c1f3d3bd | 462 | I3.fall(&motorISR); |
estott | 0:de4320f74764 | 463 | |
eavennnn | 11:f101c1f3d3bd | 464 | // mining bitcoins |
eavennnn | 11:f101c1f3d3bd | 465 | SHA256 mine; |
eavennnn | 11:f101c1f3d3bd | 466 | uint8_t sequence[] = { |
eavennnn | 11:f101c1f3d3bd | 467 | 0x45,0x6D,0x62,0x65,0x64,0x64,0x65,0x64, |
eavennnn | 11:f101c1f3d3bd | 468 | 0x20,0x53,0x79,0x73,0x74,0x65,0x6D,0x73, |
eavennnn | 11:f101c1f3d3bd | 469 | 0x20,0x61,0x72,0x65,0x20,0x66,0x75,0x6E, |
eavennnn | 11:f101c1f3d3bd | 470 | 0x20,0x61,0x6E,0x64,0x20,0x64,0x6F,0x20, |
eavennnn | 11:f101c1f3d3bd | 471 | 0x61,0x77,0x65,0x73,0x6F,0x6D,0x65,0x20, |
eavennnn | 11:f101c1f3d3bd | 472 | 0x74,0x68,0x69,0x6E,0x67,0x73,0x21,0x20, |
eavennnn | 11:f101c1f3d3bd | 473 | 0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00, |
eavennnn | 11:f101c1f3d3bd | 474 | 0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00}; |
eavennnn | 11:f101c1f3d3bd | 475 | |
eavennnn | 11:f101c1f3d3bd | 476 | |
eavennnn | 11:f101c1f3d3bd | 477 | uint64_t* key = (uint64_t*)((int)sequence + 48); |
eavennnn | 11:f101c1f3d3bd | 478 | uint64_t* nonce = (uint64_t*)((int)sequence + 56); |
eavennnn | 11:f101c1f3d3bd | 479 | uint8_t hash[32]; |
eavennnn | 11:f101c1f3d3bd | 480 | |
eavennnn | 11:f101c1f3d3bd | 481 | // timer for hash rate |
eavennnn | 11:f101c1f3d3bd | 482 | Ticker t; |
eavennnn | 11:f101c1f3d3bd | 483 | t.attach(&calcHashRate, 1.0); |
eavennnn | 11:f101c1f3d3bd | 484 | |
eavennnn | 11:f101c1f3d3bd | 485 | |
estott | 1:184cb0870c04 | 486 | while (1) { |
eavennnn | 11:f101c1f3d3bd | 487 | //Protect key from other accesses |
eavennnn | 11:f101c1f3d3bd | 488 | newKey_mutex.lock(); |
eavennnn | 11:f101c1f3d3bd | 489 | *key = newKey; |
eavennnn | 11:f101c1f3d3bd | 490 | newKey_mutex.unlock(); |
eavennnn | 11:f101c1f3d3bd | 491 | //Computer Hash with correct conditions |
eavennnn | 11:f101c1f3d3bd | 492 | mine.computeHash(hash, sequence, 64); |
eavennnn | 11:f101c1f3d3bd | 493 | hashCount = hashCount + 1; |
eavennnn | 11:f101c1f3d3bd | 494 | if (hash[0] == 0 && hash[1] == 0){ |
eavennnn | 11:f101c1f3d3bd | 495 | putMessage(2, *nonce); |
eavennnn | 11:f101c1f3d3bd | 496 | //pc.printf("Key: 0x%016llx\n\r", *key); |
estott | 0:de4320f74764 | 497 | } |
eavennnn | 11:f101c1f3d3bd | 498 | *nonce = *nonce + 1; |
eavennnn | 11:f101c1f3d3bd | 499 | |
estott | 2:4e88faab6988 | 500 | } |
estott | 0:de4320f74764 | 501 | } |
estott | 0:de4320f74764 | 502 |