yiwen zou / Mbed OS ES_CW2_Final

Coursework 2 Motor Control

Dependencies:   Crypto

main.cpp@11:f101c1f3d3bd, 2019-03-22 (annotated)

Committer:
eavennnn
Date:
Fri Mar 22 23:43:49 2019 +0000
Revision:
11:f101c1f3d3bd
Parent:
10:a4b5723b6c9d
Child:
12:9b7a85e17146
CW2 FINAL

Who changed what in which revision?

UserRevisionLine numberNew 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 11:f101c1f3d3bd 35 #define MAX_PWM 10000
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