Olaf Sikorski / Mbed OS motor-mining

This is probably never gonna get done

Dependencies:   Crypto

main.cpp@28:613a88b88074, 2019-03-22 (annotated)

Committer:
Olaffo
Date:
Fri Mar 22 19:13:05 2019 +0000
Revision:
28:613a88b88074
Parent:
27:71914f339d6b
Child:
29:cb4db90cfd63
attempt at CHA CHB control

Who changed what in which revision?

UserRevisionLine numberNew contents of line
estott 0:de4320f74764 1 #include "mbed.h"
peterith 13:c51d828531d5 2 #include "Crypto.h"
estott 0:de4320f74764 3
estott 0:de4320f74764 4 //Photointerrupter input pins
estott 10:a4b5723b6c9d 5 #define I1pin D3
estott 10:a4b5723b6c9d 6 #define I2pin D6
estott 10:a4b5723b6c9d 7 #define I3pin D5
estott 2:4e88faab6988 8
estott 2:4e88faab6988 9 //Incremental encoder input pins
peterith 13:c51d828531d5 10 #define CHApin D12
peterith 13:c51d828531d5 11 #define CHBpin D11
estott 0:de4320f74764 12
estott 0:de4320f74764 13 //Motor Drive output pins //Mask in output byte
estott 10:a4b5723b6c9d 14 #define L1Lpin D1 //0x01
estott 10:a4b5723b6c9d 15 #define L1Hpin A3 //0x02
estott 10:a4b5723b6c9d 16 #define L2Lpin D0 //0x04
peterith 13:c51d828531d5 17 #define L2Hpin A6 //0x08
peterith 13:c51d828531d5 18 #define L3Lpin D10 //0x10
peterith 13:c51d828531d5 19 #define L3Hpin D2 //0x20
estott 10:a4b5723b6c9d 20
estott 10:a4b5723b6c9d 21 #define PWMpin D9
estott 5:08f338b5e4d9 22
estott 5:08f338b5e4d9 23 //Motor current sense
peterith 13:c51d828531d5 24 #define MCSPpin A1
peterith 13:c51d828531d5 25 #define MCSNpin A0
estott 0:de4320f74764 26
estott 0:de4320f74764 27 //Mapping from sequential drive states to motor phase outputs
estott 0:de4320f74764 28 /*
estott 0:de4320f74764 29 State L1 L2 L3
estott 0:de4320f74764 30 0 H - L
estott 0:de4320f74764 31 1 - H L
estott 0:de4320f74764 32 2 L H -
estott 0:de4320f74764 33 3 L - H
estott 0:de4320f74764 34 4 - L H
estott 0:de4320f74764 35 5 H L -
estott 0:de4320f74764 36 6 - - -
estott 0:de4320f74764 37 7 - - -
estott 0:de4320f74764 38 */
estott 0:de4320f74764 39 //Drive state to output table
estott 0:de4320f74764 40 const int8_t driveTable[] = {0x12,0x18,0x09,0x21,0x24,0x06,0x00,0x00};
estott 2:4e88faab6988 41
estott 0:de4320f74764 42 //Mapping from interrupter inputs to sequential rotor states. 0x00 and 0x07 are not valid
estott 2:4e88faab6988 43 const int8_t stateMap[] = {0x07,0x05,0x03,0x04,0x01,0x00,0x02,0x07};
estott 2:4e88faab6988 44 //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 45
estott 2:4e88faab6988 46 //Phase lead to make motor spin
tanyuzhuo 24:7dd4e187dd30 47 int8_t lead = -2; //2 for forwards, -2 for backwards
estott 0:de4320f74764 48
estott 0:de4320f74764 49 //Status LED
estott 0:de4320f74764 50 DigitalOut led1(LED1);
estott 0:de4320f74764 51
estott 0:de4320f74764 52 //Photointerrupter inputs
tanyuzhuo 12:899cd6bf9844 53 InterruptIn I1(I1pin);
tanyuzhuo 12:899cd6bf9844 54 InterruptIn I2(I2pin);
tanyuzhuo 12:899cd6bf9844 55 InterruptIn I3(I3pin);
tanyuzhuo 12:899cd6bf9844 56
Olaffo 28:613a88b88074 57 //Fine interrupts
Olaffo 28:613a88b88074 58 InterruptIn CHA(CHApin);
Olaffo 28:613a88b88074 59 InterruptIn CHB(CHBpin);
Olaffo 28:613a88b88074 60 uint8_t attached = 0; //interrupt state tracker
Olaffo 28:613a88b88074 61 DigitalOut IDC(Indicator);
Olaffo 28:613a88b88074 62
estott 0:de4320f74764 63 //Motor Drive outputs
tanyuzhuo 20:5bd9f9e406d1 64 DigitalOut L1L(L1Lpin);
estott 0:de4320f74764 65 DigitalOut L1H(L1Hpin);
tanyuzhuo 20:5bd9f9e406d1 66 DigitalOut L2L(L2Lpin);
estott 0:de4320f74764 67 DigitalOut L2H(L2Hpin);
tanyuzhuo 20:5bd9f9e406d1 68 DigitalOut L3L(L3Lpin);
estott 0:de4320f74764 69 DigitalOut L3H(L3Hpin);
tanyuzhuo 20:5bd9f9e406d1 70 PwmOut d9 (PWMpin);
estott 0:de4320f74764 71
peterith 13:c51d828531d5 72 int8_t orState = 0;
tanyuzhuo 20:5bd9f9e406d1 73 //int8_t intState = 0;
tanyuzhuo 20:5bd9f9e406d1 74 //int8_t intStateOld = 0;
tanyuzhuo 17:ff5300ba5442 75 int32_t revoCounter = 0; //Counts the number of revolutions
tanyuzhuo 18:e48c0910c71e 76 int32_t motorVelocity;
Olaffo 25:ca5ccbf55b07 77 uint8_t mode = 6;
Olaffo 28:613a88b88074 78
tanyuzhuo 18:e48c0910c71e 79 //Phase lead to make motor spin
tanyuzhuo 20:5bd9f9e406d1 80 //int8_t lead = -2; //2 for forwards, -2 for backwards
tanyuzhuo 12:899cd6bf9844 81
peterith 14:0481b606d10e 82 typedef struct {
tanyuzhuo 20:5bd9f9e406d1 83 int message;
tanyuzhuo 20:5bd9f9e406d1 84 uint64_t data;
tanyuzhuo 20:5bd9f9e406d1 85 float fdata;
peterith 14:0481b606d10e 86 } mail_t;
tanyuzhuo 20:5bd9f9e406d1 87
Olaffo 28:613a88b88074 88 Mail<mail_t, 1024> mail_box;
Olaffo 25:ca5ccbf55b07 89 Thread commsIn(osPriorityNormal,1024);
tanyuzhuo 20:5bd9f9e406d1 90 Thread bitcointhread(osPriorityNormal,1024);
tanyuzhuo 20:5bd9f9e406d1 91 Thread motorCtrlT(osPriorityNormal,1024);
tanyuzhuo 20:5bd9f9e406d1 92 Thread commsOut(osPriorityNormal,1024);
tanyuzhuo 20:5bd9f9e406d1 93
peterith 14:0481b606d10e 94 RawSerial pc(SERIAL_TX, SERIAL_RX);
peterith 14:0481b606d10e 95 Queue<void, 8> inCharQ;
peterith 14:0481b606d10e 96 Mutex newKey_mutex;
peterith 14:0481b606d10e 97 uint64_t newKey = 0;
peterith 14:0481b606d10e 98
tanyuzhuo 18:e48c0910c71e 99 volatile float newRev;
tanyuzhuo 23:904721445e7a 100 volatile float maxSpeed = 90;
tanyuzhuo 23:904721445e7a 101 float pulseWidth;
tanyuzhuo 18:e48c0910c71e 102 float motorPosition_command;
tanyuzhuo 18:e48c0910c71e 103 float motorPosition;
tanyuzhuo 23:904721445e7a 104 //float pulseWidth;
tanyuzhuo 18:e48c0910c71e 105
Olaffo 28:613a88b88074 106 uint32_t revCount,encCount,maxEncCount,minEncCount,badEdges,maxBadEdges;
Olaffo 28:613a88b88074 107 uint32_t encState,totalEncCount;
Olaffo 28:613a88b88074 108 uint32_t encArray[1024];
Olaffo 28:613a88b88074 109 int encIndex = 0;
Olaffo 28:613a88b88074 110
tanyuzhuo 20:5bd9f9e406d1 111 void serialISR() {
tanyuzhuo 20:5bd9f9e406d1 112 uint8_t newChar = pc.getc();
tanyuzhuo 20:5bd9f9e406d1 113 inCharQ.put((void*) newChar);
tanyuzhuo 20:5bd9f9e406d1 114 }
Olaffo 28:613a88b88074 115
Olaffo 28:613a88b88074 116
Olaffo 28:613a88b88074 117 /***************************************************************/
Olaffo 28:613a88b88074 118 //Fine control interrupt functions
Olaffo 28:613a88b88074 119
Olaffo 28:613a88b88074 120 void encISR0() {
Olaffo 28:613a88b88074 121 if (encState == 3) {encCount++;IDC = !IDC;}
Olaffo 28:613a88b88074 122 else badEdges++;
Olaffo 28:613a88b88074 123 encState = 0;
Olaffo 28:613a88b88074 124 }
Olaffo 28:613a88b88074 125
Olaffo 28:613a88b88074 126 // B Rise
Olaffo 28:613a88b88074 127 void encISR1() {
Olaffo 28:613a88b88074 128 if (encState == 0) {encCount++;IDC = !IDC;}
Olaffo 28:613a88b88074 129 else badEdges++;
Olaffo 28:613a88b88074 130 encState = 1;
Olaffo 28:613a88b88074 131 }
Olaffo 28:613a88b88074 132
Olaffo 28:613a88b88074 133 // A Fall
Olaffo 28:613a88b88074 134 void encISR2() {
Olaffo 28:613a88b88074 135 if (encState == 1) {encCount++;IDC = !IDC;}
Olaffo 28:613a88b88074 136 else badEdges++;
Olaffo 28:613a88b88074 137 encState = 2;
Olaffo 28:613a88b88074 138 }
Olaffo 28:613a88b88074 139
Olaffo 28:613a88b88074 140 // B Fall
Olaffo 28:613a88b88074 141 void encISR3() {
Olaffo 28:613a88b88074 142 if (encState == 2) {encCount++;IDC = !IDC;}
Olaffo 28:613a88b88074 143 else badEdges++;
Olaffo 28:613a88b88074 144 encState = 3;
Olaffo 28:613a88b88074 145 }
Olaffo 28:613a88b88074 146 //End of block 2
Olaffo 28:613a88b88074 147 /***************************************************************/
Olaffo 28:613a88b88074 148
tanyuzhuo 20:5bd9f9e406d1 149 /***************************************************************/
tanyuzhuo 20:5bd9f9e406d1 150 //The following block should be moved to a library, but I don't the time to figure this out atm.
tanyuzhuo 20:5bd9f9e406d1 151
tanyuzhuo 20:5bd9f9e406d1 152 //CommsOut.h
tanyuzhuo 20:5bd9f9e406d1 153
tanyuzhuo 20:5bd9f9e406d1 154 enum message_keys {
Olaffo 25:ca5ccbf55b07 155 KEY_DECLINED = 0,
Olaffo 25:ca5ccbf55b07 156 ROTATION_CMD = 1,
Olaffo 25:ca5ccbf55b07 157 ROTATION_FF_CMD = 2,
Olaffo 25:ca5ccbf55b07 158 MAX_SPEED_CMD = 3,
Olaffo 25:ca5ccbf55b07 159 KEY_ECHO = 4,
Olaffo 25:ca5ccbf55b07 160 TUNE_CMD = 5,
Olaffo 25:ca5ccbf55b07 161 STANDARD = 6,
Olaffo 25:ca5ccbf55b07 162 FOREVER_FORWARD = 7,
Olaffo 25:ca5ccbf55b07 163 LEAD = 9,
Olaffo 28:613a88b88074 164 FINE_TUNE = 21,
Olaffo 28:613a88b88074 165 T_ENC = 23,
Olaffo 28:613a88b88074 166 ENC = 24,
tanyuzhuo 20:5bd9f9e406d1 167
tanyuzhuo 20:5bd9f9e406d1 168
Olaffo 25:ca5ccbf55b07 169 INVALID_CMD = 10,
Olaffo 25:ca5ccbf55b07 170 MOTOR_POS = 11,
Olaffo 25:ca5ccbf55b07 171 MOTOR_SPEED = 12,
tanyuzhuo 20:5bd9f9e406d1 172
tanyuzhuo 20:5bd9f9e406d1 173
Olaffo 25:ca5ccbf55b07 174 NONCE_DETECT = 14,
Olaffo 25:ca5ccbf55b07 175 ROTOR_ORG = 15,
tanyuzhuo 20:5bd9f9e406d1 176
Olaffo 28:613a88b88074 177 WELCOME = 20,
Olaffo 28:613a88b88074 178
Olaffo 28:613a88b88074 179 GIVE_ENC = 100
Olaffo 25:ca5ccbf55b07 180
Olaffo 25:ca5ccbf55b07 181
tanyuzhuo 20:5bd9f9e406d1 182 };
tanyuzhuo 20:5bd9f9e406d1 183
tanyuzhuo 20:5bd9f9e406d1 184 void putMessage(int message, uint64_t data = 0, float fdata=0){
peterith 14:0481b606d10e 185 mail_t *mail = mail_box.alloc();
tanyuzhuo 20:5bd9f9e406d1 186 mail->message = message;
tanyuzhuo 20:5bd9f9e406d1 187 mail->data = data;
tanyuzhuo 20:5bd9f9e406d1 188 mail->fdata = fdata;
peterith 14:0481b606d10e 189 mail_box.put(mail);
peterith 14:0481b606d10e 190 }
peterith 14:0481b606d10e 191
tanyuzhuo 20:5bd9f9e406d1 192 void commsOutFunc() {
Olaffo 28:613a88b88074 193 int i;
Olaffo 28:613a88b88074 194
tanyuzhuo 20:5bd9f9e406d1 195 while (true) {
tanyuzhuo 20:5bd9f9e406d1 196 osEvent evt = mail_box.get();
tanyuzhuo 20:5bd9f9e406d1 197 if (evt.status == osEventMail) {
tanyuzhuo 20:5bd9f9e406d1 198 mail_t *mail = (mail_t*)evt.value.p;
tanyuzhuo 20:5bd9f9e406d1 199 switch (mail->message) {
tanyuzhuo 20:5bd9f9e406d1 200 case KEY_DECLINED :
tanyuzhuo 20:5bd9f9e406d1 201 pc.printf("Not a valid key!\n\r");
tanyuzhuo 20:5bd9f9e406d1 202 break;
tanyuzhuo 20:5bd9f9e406d1 203 case ROTATION_CMD :
tanyuzhuo 24:7dd4e187dd30 204 pc.printf("Rotation count: %3.2f\n\r", mail->fdata);
tanyuzhuo 20:5bd9f9e406d1 205 break;
Olaffo 25:ca5ccbf55b07 206 case ROTATION_FF_CMD :
Olaffo 25:ca5ccbf55b07 207 pc.printf("I will rotate forever...");
tanyuzhuo 20:5bd9f9e406d1 208 break;
Olaffo 25:ca5ccbf55b07 209 case MAX_SPEED_CMD :
Olaffo 25:ca5ccbf55b07 210 pc.printf("Max speed: %2.1f\n\r", mail->fdata);
Olaffo 25:ca5ccbf55b07 211 break;
tanyuzhuo 20:5bd9f9e406d1 212 case KEY_ECHO :
tanyuzhuo 20:5bd9f9e406d1 213 pc.printf("Received key: %016llx\n\r", mail->data);
Olaffo 25:ca5ccbf55b07 214 break;
Olaffo 25:ca5ccbf55b07 215 case TUNE_CMD :
Olaffo 25:ca5ccbf55b07 216 pc.printf("Tune command!\n\r");
tanyuzhuo 20:5bd9f9e406d1 217 break;
tanyuzhuo 20:5bd9f9e406d1 218 case INVALID_CMD :
tanyuzhuo 20:5bd9f9e406d1 219 pc.printf("Invalid command!\r\n");
tanyuzhuo 20:5bd9f9e406d1 220 break;
tanyuzhuo 20:5bd9f9e406d1 221 case MOTOR_POS :
tanyuzhuo 20:5bd9f9e406d1 222 pc.printf("Motor position: %f\r\n", mail->fdata);
Olaffo 25:ca5ccbf55b07 223 //pc.printf("{TIMEPLOT|%.2f|Motor Position}", mail->fdata);
tanyuzhuo 20:5bd9f9e406d1 224 break;
tanyuzhuo 20:5bd9f9e406d1 225 case MOTOR_SPEED :
tanyuzhuo 20:5bd9f9e406d1 226 pc.printf("Motor speed: %f\r\n", mail->fdata);
tanyuzhuo 20:5bd9f9e406d1 227 break;
tanyuzhuo 20:5bd9f9e406d1 228 case NONCE_DETECT :
tanyuzhuo 20:5bd9f9e406d1 229 pc.printf("Successful nonce: %016x\n\r", mail->data);
tanyuzhuo 20:5bd9f9e406d1 230 break;
tanyuzhuo 20:5bd9f9e406d1 231 case WELCOME :
Olaffo 25:ca5ccbf55b07 232 pc.printf("Hello Olaf\n\r");
tanyuzhuo 20:5bd9f9e406d1 233 break;
tanyuzhuo 20:5bd9f9e406d1 234 case ROTOR_ORG :
tanyuzhuo 20:5bd9f9e406d1 235 pc.printf("Rotor origin: %x\n\r", orState);
Olaffo 28:613a88b88074 236 break;
Olaffo 25:ca5ccbf55b07 237 case LEAD :
Olaffo 25:ca5ccbf55b07 238 pc.printf("Lead: %d\n\r\n\r", mail->data);
Olaffo 25:ca5ccbf55b07 239 break;
Olaffo 28:613a88b88074 240 case FINE_TUNE :
Olaffo 28:613a88b88074 241 pc.printf("Interrupts: %d\n\r\n\r", attached);
Olaffo 28:613a88b88074 242 break;
Olaffo 28:613a88b88074 243 case T_ENC :
Olaffo 28:613a88b88074 244 pc.printf("totalEncCount: %d\n\r", mail->data);
Olaffo 28:613a88b88074 245 break;
Olaffo 28:613a88b88074 246 case ENC :
Olaffo 28:613a88b88074 247 pc.printf("EncCount: %d\n\r", mail->data);
Olaffo 28:613a88b88074 248 break;
Olaffo 28:613a88b88074 249 case GIVE_ENC :
Olaffo 28:613a88b88074 250
Olaffo 28:613a88b88074 251 for (i=0; i<encIndex; i++ ){
Olaffo 28:613a88b88074 252 pc.printf("%d\n\r", encArray[i]);
Olaffo 28:613a88b88074 253 }
Olaffo 28:613a88b88074 254 //pc.printf("%d\n\r", encIndex);
Olaffo 28:613a88b88074 255 break;
tanyuzhuo 20:5bd9f9e406d1 256
tanyuzhuo 20:5bd9f9e406d1 257 }
tanyuzhuo 20:5bd9f9e406d1 258 mail_box.free(mail);
tanyuzhuo 20:5bd9f9e406d1 259 }
tanyuzhuo 20:5bd9f9e406d1 260 }
tanyuzhuo 20:5bd9f9e406d1 261 }
Olaffo 28:613a88b88074 262 //End of block 2
tanyuzhuo 20:5bd9f9e406d1 263 /***************************************************************/
tanyuzhuo 20:5bd9f9e406d1 264
tanyuzhuo 20:5bd9f9e406d1 265 /***************************************************************/
tanyuzhuo 20:5bd9f9e406d1 266 //The following block should also be moved to a library, but I still don't the time to figure this out atm.
tanyuzhuo 20:5bd9f9e406d1 267
tanyuzhuo 20:5bd9f9e406d1 268 //CommsIn.h
tanyuzhuo 20:5bd9f9e406d1 269
Olaffo 25:ca5ccbf55b07 270 void R_Decoder(char* command) {
tanyuzhuo 20:5bd9f9e406d1 271 int8_t sign =1;
tanyuzhuo 20:5bd9f9e406d1 272 uint8_t index = 1;
tanyuzhuo 20:5bd9f9e406d1 273 int intValue = 0;
tanyuzhuo 20:5bd9f9e406d1 274 float decimalValue = 0;
Olaffo 25:ca5ccbf55b07 275
Olaffo 25:ca5ccbf55b07 276 // Check sign
Olaffo 25:ca5ccbf55b07 277 if (command[1] == '-'){
Olaffo 25:ca5ccbf55b07 278 sign = -1;
Olaffo 25:ca5ccbf55b07 279 index++;
Olaffo 25:ca5ccbf55b07 280 }
Olaffo 25:ca5ccbf55b07 281 // Take first digit
Olaffo 25:ca5ccbf55b07 282 if (command[index] > ('0'-1) && command[index] < (1 + '9')) {
Olaffo 25:ca5ccbf55b07 283 intValue = command[index] - '0';
Olaffo 25:ca5ccbf55b07 284 index++;
Olaffo 25:ca5ccbf55b07 285 }
Olaffo 25:ca5ccbf55b07 286 else {
Olaffo 25:ca5ccbf55b07 287 putMessage(INVALID_CMD);
Olaffo 25:ca5ccbf55b07 288 return;
Olaffo 25:ca5ccbf55b07 289 }
tanyuzhuo 20:5bd9f9e406d1 290
Olaffo 25:ca5ccbf55b07 291 //Try taking 2 more digits
Olaffo 25:ca5ccbf55b07 292 if (command[index] > ('0'-1) && command[index] < (1 + '9')) {
Olaffo 25:ca5ccbf55b07 293 intValue = intValue*10 + command[index] - '0';
Olaffo 25:ca5ccbf55b07 294 index++;
Olaffo 25:ca5ccbf55b07 295 }
Olaffo 25:ca5ccbf55b07 296 if (command[index] > ('0'-1) && command[index] < (1 + '9')) {
Olaffo 25:ca5ccbf55b07 297 intValue = intValue*10 + command[index] - '0';
Olaffo 25:ca5ccbf55b07 298 index++;
Olaffo 25:ca5ccbf55b07 299 }
Olaffo 25:ca5ccbf55b07 300 //Check for '.'
Olaffo 25:ca5ccbf55b07 301 if (command[index] == '.') {
Olaffo 25:ca5ccbf55b07 302 index++;
Olaffo 25:ca5ccbf55b07 303 //Check for decimals
Olaffo 25:ca5ccbf55b07 304 if (command[index] > ('0'-1) && command[index] < (1 + '9')) {
Olaffo 25:ca5ccbf55b07 305 decimalValue = (command[index] - '0')/10;
Olaffo 25:ca5ccbf55b07 306 index++;
Olaffo 25:ca5ccbf55b07 307 }
Olaffo 25:ca5ccbf55b07 308 if (command[index] > ('0'-1) && command[index] < (1 + '9')) {
Olaffo 25:ca5ccbf55b07 309 decimalValue = (decimalValue/10 + command[index] - '0')/10;
Olaffo 25:ca5ccbf55b07 310 }
Olaffo 25:ca5ccbf55b07 311 }
Olaffo 25:ca5ccbf55b07 312 decimalValue = (decimalValue + intValue) * sign;
Olaffo 25:ca5ccbf55b07 313 if (decimalValue == 0){
Olaffo 25:ca5ccbf55b07 314 mode = FOREVER_FORWARD;
Olaffo 25:ca5ccbf55b07 315 putMessage(ROTATION_FF_CMD);
Olaffo 25:ca5ccbf55b07 316 }
Olaffo 25:ca5ccbf55b07 317 else {
Olaffo 25:ca5ccbf55b07 318 newRev = decimalValue;
Olaffo 25:ca5ccbf55b07 319 putMessage(ROTATION_CMD,0,decimalValue);
Olaffo 25:ca5ccbf55b07 320 }
Olaffo 25:ca5ccbf55b07 321 }
Olaffo 25:ca5ccbf55b07 322
Olaffo 25:ca5ccbf55b07 323 void V_Decoder(char* command) {
Olaffo 25:ca5ccbf55b07 324 uint8_t index = 1;
Olaffo 25:ca5ccbf55b07 325 int intValue = 0;
Olaffo 25:ca5ccbf55b07 326 float decimalValue = 0;
Olaffo 25:ca5ccbf55b07 327
Olaffo 25:ca5ccbf55b07 328 // Take first digit
Olaffo 25:ca5ccbf55b07 329 if (command[index] > ('0'-1) && command[index] < (1 + '9')) {
Olaffo 25:ca5ccbf55b07 330 intValue = command[index] - '0';
Olaffo 25:ca5ccbf55b07 331 index++;
Olaffo 25:ca5ccbf55b07 332 }
Olaffo 25:ca5ccbf55b07 333 else {
Olaffo 25:ca5ccbf55b07 334 putMessage(INVALID_CMD);
Olaffo 25:ca5ccbf55b07 335 return;
Olaffo 25:ca5ccbf55b07 336 }
tanyuzhuo 20:5bd9f9e406d1 337
Olaffo 25:ca5ccbf55b07 338 //Try taking 2 more digits
Olaffo 25:ca5ccbf55b07 339 if (command[index] > ('0'-1) && command[index] < (1 + '9')) {
Olaffo 25:ca5ccbf55b07 340 intValue = intValue*10 + command[index] - '0';
Olaffo 25:ca5ccbf55b07 341 index++;
Olaffo 25:ca5ccbf55b07 342 }
Olaffo 25:ca5ccbf55b07 343 if (command[index] > ('0'-1) && command[index] < (1 + '9')) {
Olaffo 25:ca5ccbf55b07 344 intValue = intValue*10 + command[index] - '0';
Olaffo 25:ca5ccbf55b07 345 index++;
Olaffo 25:ca5ccbf55b07 346 }
Olaffo 25:ca5ccbf55b07 347 //Check for '.'
Olaffo 25:ca5ccbf55b07 348 if (command[index] == '.') {
Olaffo 25:ca5ccbf55b07 349 index++;
Olaffo 25:ca5ccbf55b07 350 //Check for one decimal
Olaffo 25:ca5ccbf55b07 351 if (command[index] > ('0'-1) && command[index] < (1 + '9')) {
Olaffo 25:ca5ccbf55b07 352 decimalValue = (command[index] - '0')/10;
Olaffo 25:ca5ccbf55b07 353 }
tanyuzhuo 20:5bd9f9e406d1 354 }
Olaffo 25:ca5ccbf55b07 355 decimalValue = (decimalValue + intValue);
Olaffo 25:ca5ccbf55b07 356 maxSpeed = decimalValue;
Olaffo 25:ca5ccbf55b07 357 putMessage(MAX_SPEED_CMD,0,decimalValue);
peterith 14:0481b606d10e 358 }
peterith 14:0481b606d10e 359
peterith 14:0481b606d10e 360 void commandProcessor() {
peterith 14:0481b606d10e 361 pc.attach(&serialISR);
tanyuzhuo 17:ff5300ba5442 362 char command[19];
tanyuzhuo 17:ff5300ba5442 363 char* number;
peterith 14:0481b606d10e 364 //char k;
peterith 14:0481b606d10e 365 uint64_t receivedKey;
peterith 14:0481b606d10e 366 uint8_t index = 0;
peterith 14:0481b606d10e 367 while(1) {
peterith 14:0481b606d10e 368 osEvent newEvent = inCharQ.get();
peterith 14:0481b606d10e 369 uint8_t newChar = (uint8_t) newEvent.value.p;
peterith 14:0481b606d10e 370 command[index] = newChar;
peterith 14:0481b606d10e 371 index++;
tanyuzhuo 17:ff5300ba5442 372 if (newChar == '\r') {
peterith 14:0481b606d10e 373 command[17] = '\0';
tanyuzhuo 17:ff5300ba5442 374
tanyuzhuo 17:ff5300ba5442 375 if (command[0] == 'R') {
Olaffo 25:ca5ccbf55b07 376 R_Decoder(command);
tanyuzhuo 24:7dd4e187dd30 377 motorPosition_command = motorPosition;
tanyuzhuo 17:ff5300ba5442 378 }
tanyuzhuo 17:ff5300ba5442 379 else if (command[0] == 'V') {
Olaffo 25:ca5ccbf55b07 380 V_Decoder(command);
tanyuzhuo 17:ff5300ba5442 381 }
tanyuzhuo 17:ff5300ba5442 382 else if (command[0] == 'K') {
tanyuzhuo 17:ff5300ba5442 383 if (index == 18){ // when index is 18 means you entered K and 16 digits
tanyuzhuo 17:ff5300ba5442 384 number = command +1; //super bad solution, but I don't know how to work with strings in C
tanyuzhuo 17:ff5300ba5442 385 receivedKey = strtoull(number, NULL, 16);
tanyuzhuo 20:5bd9f9e406d1 386 putMessage(KEY_ECHO,receivedKey);
Olaffo 25:ca5ccbf55b07 387
tanyuzhuo 17:ff5300ba5442 388 newKey_mutex.lock();
tanyuzhuo 17:ff5300ba5442 389 newKey = receivedKey;
tanyuzhuo 17:ff5300ba5442 390 newKey_mutex.unlock();
tanyuzhuo 17:ff5300ba5442 391 } else {
tanyuzhuo 20:5bd9f9e406d1 392 putMessage(KEY_DECLINED);
tanyuzhuo 17:ff5300ba5442 393 };
tanyuzhuo 17:ff5300ba5442 394 }
tanyuzhuo 17:ff5300ba5442 395 else if (command[0] == 'T') {
Olaffo 25:ca5ccbf55b07 396 putMessage(TUNE_CMD);
tanyuzhuo 17:ff5300ba5442 397 }
Olaffo 28:613a88b88074 398 else if (command[0] == 'G') {
Olaffo 28:613a88b88074 399 putMessage(GIVE_ENC);
Olaffo 28:613a88b88074 400 }
peterith 14:0481b606d10e 401 memset(command, 0, sizeof(command));
peterith 14:0481b606d10e 402 index = 0;
peterith 14:0481b606d10e 403 } else {
tanyuzhuo 20:5bd9f9e406d1 404 pc.printf("Current command: %s\n\r", command); //Not sure how to go around this one cause it requires passing string
peterith 14:0481b606d10e 405 }
peterith 14:0481b606d10e 406 }
peterith 14:0481b606d10e 407 }
Olaffo 28:613a88b88074 408 //End of block 3
Olaffo 28:613a88b88074 409 /***************************************************************/
tanyuzhuo 17:ff5300ba5442 410
tanyuzhuo 16:10d53b056b17 411 void bitcoin(){
tanyuzhuo 16:10d53b056b17 412 while(1) {
tanyuzhuo 16:10d53b056b17 413 SHA256 sha;
tanyuzhuo 16:10d53b056b17 414 uint8_t sequence[] = {0x45,0x6D,0x62,0x65,0x64,0x64,0x65,0x64,
tanyuzhuo 16:10d53b056b17 415 0x20,0x53,0x79,0x73,0x74,0x65,0x6D,0x73,
tanyuzhuo 16:10d53b056b17 416 0x20,0x61,0x72,0x65,0x20,0x66,0x75,0x6E,
tanyuzhuo 16:10d53b056b17 417 0x20,0x61,0x6E,0x64,0x20,0x64,0x6F,0x20,
tanyuzhuo 16:10d53b056b17 418 0x61,0x77,0x65,0x73,0x6F,0x6D,0x65,0x20,
tanyuzhuo 16:10d53b056b17 419 0x74,0x68,0x69,0x6E,0x67,0x73,0x21,0x20,
tanyuzhuo 16:10d53b056b17 420 0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,
tanyuzhuo 16:10d53b056b17 421 0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00};
tanyuzhuo 16:10d53b056b17 422 uint64_t* key = (uint64_t*) ((int) sequence + 48);
tanyuzhuo 16:10d53b056b17 423 uint64_t* nonce = (uint64_t*) ((int) sequence + 56);
tanyuzhuo 16:10d53b056b17 424 uint8_t hash[32];
tanyuzhuo 16:10d53b056b17 425
tanyuzhuo 16:10d53b056b17 426 Timer t;
tanyuzhuo 16:10d53b056b17 427 t.start();
tanyuzhuo 16:10d53b056b17 428 unsigned currentTime = 0;
tanyuzhuo 20:5bd9f9e406d1 429 unsigned currentCount= 0;
tanyuzhuo 16:10d53b056b17 430
tanyuzhuo 16:10d53b056b17 431 for (unsigned i = 0; i <= UINT_MAX; i++) {
tanyuzhuo 16:10d53b056b17 432 (*nonce)++;
tanyuzhuo 16:10d53b056b17 433 newKey_mutex.lock();
tanyuzhuo 16:10d53b056b17 434 *key = newKey;
tanyuzhuo 16:10d53b056b17 435 newKey_mutex.unlock();
tanyuzhuo 16:10d53b056b17 436 sha.computeHash(hash, sequence, 64);
tanyuzhuo 16:10d53b056b17 437 if (hash[0] == 0 && hash[1] == 0) {
tanyuzhuo 16:10d53b056b17 438 //putMessage(nonce);
tanyuzhuo 16:10d53b056b17 439 pc.printf("Successful nonce: %016x\n\r", *nonce);
tanyuzhuo 16:10d53b056b17 440 }
tanyuzhuo 16:10d53b056b17 441 if ((unsigned) t.read() == currentTime) {
tanyuzhuo 16:10d53b056b17 442 //pc.printf("Hash rate: %d\n\r", i - currentCount);
tanyuzhuo 20:5bd9f9e406d1 443 //pc.printf("Current key: %016llx\n\r", *key);
tanyuzhuo 16:10d53b056b17 444 currentTime++;
tanyuzhuo 16:10d53b056b17 445 currentCount = i;
tanyuzhuo 16:10d53b056b17 446 }
tanyuzhuo 16:10d53b056b17 447 }
tanyuzhuo 16:10d53b056b17 448 t.stop();
tanyuzhuo 16:10d53b056b17 449 }
Olaffo 25:ca5ccbf55b07 450 }
tanyuzhuo 20:5bd9f9e406d1 451
tanyuzhuo 18:e48c0910c71e 452
tanyuzhuo 18:e48c0910c71e 453
estott 0:de4320f74764 454 //Set a given drive state
tanyuzhuo 23:904721445e7a 455 void motorOut(int8_t driveState, float pulseWidth){
estott 0:de4320f74764 456
estott 2:4e88faab6988 457 //Lookup the output byte from the drive state.
estott 2:4e88faab6988 458 int8_t driveOut = driveTable[driveState & 0x07];
estott 2:4e88faab6988 459
estott 2:4e88faab6988 460 //Turn off first
tanyuzhuo 20:5bd9f9e406d1 461 if (~driveOut & 0x01) L1L = 0;
estott 2:4e88faab6988 462 if (~driveOut & 0x02) L1H = 1;
tanyuzhuo 20:5bd9f9e406d1 463 if (~driveOut & 0x04) L2L = 0;
estott 2:4e88faab6988 464 if (~driveOut & 0x08) L2H = 1;
tanyuzhuo 20:5bd9f9e406d1 465 if (~driveOut & 0x10) L3L = 0;
estott 2:4e88faab6988 466 if (~driveOut & 0x20) L3H = 1;
estott 2:4e88faab6988 467
estott 2:4e88faab6988 468 //Then turn on
tanyuzhuo 20:5bd9f9e406d1 469 if (driveOut & 0x01) L1L = 1;
estott 2:4e88faab6988 470 if (driveOut & 0x02) L1H = 0;
tanyuzhuo 20:5bd9f9e406d1 471 if (driveOut & 0x04) L2L = 1;
estott 2:4e88faab6988 472 if (driveOut & 0x08) L2H = 0;
tanyuzhuo 20:5bd9f9e406d1 473 if (driveOut & 0x10) L3L = 1;
estott 2:4e88faab6988 474 if (driveOut & 0x20) L3H = 0;
tanyuzhuo 20:5bd9f9e406d1 475
tanyuzhuo 20:5bd9f9e406d1 476
tanyuzhuo 23:904721445e7a 477 //d9.write(1);
tanyuzhuo 23:904721445e7a 478 d9.pulsewidth_us(pulseWidth);
peterith 13:c51d828531d5 479 }
estott 0:de4320f74764 480
peterith 13:c51d828531d5 481 //Convert photointerrupter inputs to a rotor state
estott 0:de4320f74764 482 inline int8_t readRotorState(){
estott 2:4e88faab6988 483 return stateMap[I1 + 2*I2 + 4*I3];
peterith 13:c51d828531d5 484 }
estott 0:de4320f74764 485
estott 2:4e88faab6988 486 int8_t motorHome() {
tanyuzhuo 18:e48c0910c71e 487 //Put the motor in drive state 0 and wait for it to stabilize
tanyuzhuo 23:904721445e7a 488 motorOut(0,pulseWidth);
estott 3:569b35e2a602 489 wait(2.0);
estott 2:4e88faab6988 490 return readRotorState();
estott 0:de4320f74764 491 }
peterith 13:c51d828531d5 492
tanyuzhuo 20:5bd9f9e406d1 493
tanyuzhuo 18:e48c0910c71e 494 // ISR to handle the updating of the motor position
tanyuzhuo 18:e48c0910c71e 495 void motorISR() {
tanyuzhuo 18:e48c0910c71e 496 static int8_t oldRotorState;
tanyuzhuo 20:5bd9f9e406d1 497 //orState is subtracted from future rotor state inputs to align rotor and motor states
tanyuzhuo 20:5bd9f9e406d1 498
tanyuzhuo 18:e48c0910c71e 499 int8_t rotorState = readRotorState();
tanyuzhuo 23:904721445e7a 500 motorOut((rotorState-orState+lead+6)%6,pulseWidth); //+6 to make sure the remainder is positive
tanyuzhuo 18:e48c0910c71e 501 if (rotorState - oldRotorState == 5) motorPosition --;
tanyuzhuo 18:e48c0910c71e 502 else if (rotorState - oldRotorState == -5) motorPosition ++;
tanyuzhuo 18:e48c0910c71e 503 else motorPosition += (rotorState - oldRotorState);
tanyuzhuo 20:5bd9f9e406d1 504 //pc.printf ("motorpPosition %f\n\r", motorPosition);
tanyuzhuo 18:e48c0910c71e 505 oldRotorState = rotorState;
Olaffo 28:613a88b88074 506
Olaffo 28:613a88b88074 507 //putMessage(ENC,encCount);
Olaffo 28:613a88b88074 508
Olaffo 28:613a88b88074 509 if (attached == 1){
Olaffo 28:613a88b88074 510 encCount = 0;
Olaffo 28:613a88b88074 511 attached++;
Olaffo 28:613a88b88074 512 }
Olaffo 28:613a88b88074 513
Olaffo 28:613a88b88074 514 encArray[encIndex] = encCount;
Olaffo 28:613a88b88074 515 encIndex++;
Olaffo 28:613a88b88074 516
tanyuzhuo 18:e48c0910c71e 517 }
tanyuzhuo 18:e48c0910c71e 518 /*void push() {
peterith 13:c51d828531d5 519 intState = readRotorState();
peterith 13:c51d828531d5 520 if (intState != intStateOld) {
peterith 13:c51d828531d5 521 intStateOld = intState;
peterith 13:c51d828531d5 522 motorOut((intState - orState + lead +6) % 6); //+6 to make sure the remainder is positive
peterith 13:c51d828531d5 523 }
tanyuzhuo 18:e48c0910c71e 524 }*/
tanyuzhuo 20:5bd9f9e406d1 525 void motorCtrlTick(){
tanyuzhuo 20:5bd9f9e406d1 526 motorCtrlT.signal_set(0x1);
tanyuzhuo 20:5bd9f9e406d1 527 }
tanyuzhuo 18:e48c0910c71e 528 void motorCtrlFn(){
tanyuzhuo 18:e48c0910c71e 529 int32_t counter=0;
tanyuzhuo 18:e48c0910c71e 530 static int32_t oldmotorPosition;
tanyuzhuo 24:7dd4e187dd30 531 float Ts;
tanyuzhuo 24:7dd4e187dd30 532 float sError; //speed error
tanyuzhuo 24:7dd4e187dd30 533 float intError = 0; //integral of velError
tanyuzhuo 24:7dd4e187dd30 534 float Tr;
tanyuzhuo 24:7dd4e187dd30 535 float error;
tanyuzhuo 24:7dd4e187dd30 536 float olderror = 0;
tanyuzhuo 21:34f440ae0227 537 float Speed;
tanyuzhuo 24:7dd4e187dd30 538 float Rev;
Olaffo 27:71914f339d6b 539 float kps = 27;
Olaffo 28:613a88b88074 540 float kis = 0.4;
tanyuzhuo 24:7dd4e187dd30 541 float kpr = 35;
tanyuzhuo 24:7dd4e187dd30 542 float kdr = 14.75; //proportional constant for speed
tanyuzhuo 18:e48c0910c71e 543 // Timer to count time passed between ticks to calculate velocity
tanyuzhuo 18:e48c0910c71e 544 Timer motorTime;
tanyuzhuo 18:e48c0910c71e 545 motorTime.start();
tanyuzhuo 18:e48c0910c71e 546 float motorPos;
tanyuzhuo 24:7dd4e187dd30 547 float myTime;
Olaffo 28:613a88b88074 548
Olaffo 28:613a88b88074 549 CHA.rise(&encISR0);
Olaffo 28:613a88b88074 550 CHB.fall(&encISR3);
Olaffo 28:613a88b88074 551 CHB.rise(&encISR1);
Olaffo 28:613a88b88074 552 CHA.fall(&encISR2);
tanyuzhuo 24:7dd4e187dd30 553
tanyuzhuo 18:e48c0910c71e 554 Ticker motorCtrlTicker;
tanyuzhuo 18:e48c0910c71e 555 motorCtrlTicker.attach_us(&motorCtrlTick,100000);
tanyuzhuo 18:e48c0910c71e 556 while(1){
tanyuzhuo 18:e48c0910c71e 557 motorCtrlT.signal_wait(0x1);
tanyuzhuo 24:7dd4e187dd30 558
Olaffo 25:ca5ccbf55b07 559 // Convert variabls to int
tanyuzhuo 21:34f440ae0227 560 Speed = maxSpeed*6;
tanyuzhuo 24:7dd4e187dd30 561 Rev = newRev *6;
tanyuzhuo 18:e48c0910c71e 562 motorPos = motorPosition;
tanyuzhuo 24:7dd4e187dd30 563
tanyuzhuo 24:7dd4e187dd30 564 myTime = motorTime.read();
tanyuzhuo 24:7dd4e187dd30 565 motorVelocity = (motorPos - oldmotorPosition)/myTime;
tanyuzhuo 24:7dd4e187dd30 566 error = Rev + motorPosition_command - motorPos;
tanyuzhuo 18:e48c0910c71e 567 oldmotorPosition = motorPos;
tanyuzhuo 20:5bd9f9e406d1 568
Olaffo 28:613a88b88074 569 if ((motorVelocity < 40) && (attached == 0)){
Olaffo 28:613a88b88074 570 CHA.rise(&encISR0);
Olaffo 28:613a88b88074 571 CHB.fall(&encISR3);
Olaffo 28:613a88b88074 572 CHB.rise(&encISR1);
Olaffo 28:613a88b88074 573 CHA.fall(&encISR2);
Olaffo 28:613a88b88074 574
Olaffo 28:613a88b88074 575 attached = 1;
Olaffo 28:613a88b88074 576 }
Olaffo 28:613a88b88074 577 else if ((motorVelocity >= 60) && (attached > 0)){
Olaffo 28:613a88b88074 578 CHA.rise(NULL);
Olaffo 28:613a88b88074 579 CHB.fall(NULL);
Olaffo 28:613a88b88074 580 CHB.rise(NULL);
Olaffo 28:613a88b88074 581 CHA.fall(NULL);
Olaffo 28:613a88b88074 582
Olaffo 28:613a88b88074 583 attached = 0;
Olaffo 28:613a88b88074 584
Olaffo 28:613a88b88074 585 }
Olaffo 28:613a88b88074 586
Olaffo 28:613a88b88074 587
tanyuzhuo 20:5bd9f9e406d1 588 //equation for controls
tanyuzhuo 24:7dd4e187dd30 589 sError = Speed -abs(motorVelocity);
Olaffo 28:613a88b88074 590 if ((motorVelocity != 0) && (abs(sError)<25)){
Olaffo 28:613a88b88074 591 intError = intError + sError;
Olaffo 28:613a88b88074 592 }
Olaffo 28:613a88b88074 593 if (abs(intError*kis)>2000) {
Olaffo 28:613a88b88074 594 intError = intError - sError;
tanyuzhuo 24:7dd4e187dd30 595 }
Olaffo 25:ca5ccbf55b07 596 Ts = (kps * sError + kis * intError)*(error/abs(error));
Olaffo 25:ca5ccbf55b07 597 Tr = kpr*error+kdr*(error-olderror)/ myTime;
Olaffo 25:ca5ccbf55b07 598
Olaffo 25:ca5ccbf55b07 599 // Speed AND rotation control (aka standard mode)
Olaffo 25:ca5ccbf55b07 600 if (mode = STANDARD)
Olaffo 25:ca5ccbf55b07 601 {
Olaffo 25:ca5ccbf55b07 602 // Case for reverse rotation
Olaffo 25:ca5ccbf55b07 603 if (error < 0){
Olaffo 25:ca5ccbf55b07 604
Olaffo 25:ca5ccbf55b07 605 // Case for choosing rotational control
Olaffo 25:ca5ccbf55b07 606 if (Ts <=Tr ){
Olaffo 25:ca5ccbf55b07 607
Olaffo 25:ca5ccbf55b07 608 // Flip lead
Olaffo 25:ca5ccbf55b07 609 if ( Tr >= 0) { lead = -2; }
Olaffo 25:ca5ccbf55b07 610 else if ( Tr < 0) { lead = 2; }
Olaffo 25:ca5ccbf55b07 611
Olaffo 25:ca5ccbf55b07 612 // Assure torque is in bounds
Olaffo 25:ca5ccbf55b07 613 if (Tr > 2000){ Tr = 2000; }
Olaffo 25:ca5ccbf55b07 614 else if (Tr < 0){ Tr = -Tr; }
Olaffo 25:ca5ccbf55b07 615 else if (Tr < -2000){ Tr = 2000; }
Olaffo 25:ca5ccbf55b07 616
Olaffo 25:ca5ccbf55b07 617 // Pass torque
Olaffo 25:ca5ccbf55b07 618 pulseWidth = Tr;
Olaffo 25:ca5ccbf55b07 619 }
Olaffo 25:ca5ccbf55b07 620
Olaffo 25:ca5ccbf55b07 621 // Case for choosing speed control
Olaffo 25:ca5ccbf55b07 622 else {
Olaffo 25:ca5ccbf55b07 623 // Flip lead
Olaffo 25:ca5ccbf55b07 624 if ( Ts >= 0) { lead = -2; }
Olaffo 25:ca5ccbf55b07 625 else if ( Ts < 0) { lead = 2; }
Olaffo 25:ca5ccbf55b07 626
Olaffo 25:ca5ccbf55b07 627 // Assure torque is in bounds
Olaffo 25:ca5ccbf55b07 628 if (Ts > 2000){ Ts = 2000; }
Olaffo 25:ca5ccbf55b07 629 else if (Ts < 0){ Ts = -Ts; }
Olaffo 25:ca5ccbf55b07 630 else if (Ts < -2000){ Ts = 2000; }
Olaffo 25:ca5ccbf55b07 631
Olaffo 25:ca5ccbf55b07 632 // Pass torque
Olaffo 25:ca5ccbf55b07 633 pulseWidth = Ts;
Olaffo 25:ca5ccbf55b07 634 }
Olaffo 25:ca5ccbf55b07 635 }
Olaffo 25:ca5ccbf55b07 636
Olaffo 25:ca5ccbf55b07 637 // Case for forward rotation
Olaffo 25:ca5ccbf55b07 638 else if (error >= 0){
Olaffo 25:ca5ccbf55b07 639
Olaffo 25:ca5ccbf55b07 640 // Case for choosing speed control
Olaffo 25:ca5ccbf55b07 641 if (Ts <=Tr ){
Olaffo 25:ca5ccbf55b07 642 // Flip lead
Olaffo 25:ca5ccbf55b07 643 if ( Ts >= 0) { lead = -2; }
Olaffo 25:ca5ccbf55b07 644 else if ( Ts < 0) { lead = 2; }
Olaffo 25:ca5ccbf55b07 645
Olaffo 25:ca5ccbf55b07 646 // Assure torque is in bounds
Olaffo 25:ca5ccbf55b07 647 if (Ts > 2000){ Ts = 2000; }
Olaffo 25:ca5ccbf55b07 648 else if (Ts < 0){ Ts = -Ts; }
Olaffo 25:ca5ccbf55b07 649 else if (Ts < -2000){ Ts = 2000; }
Olaffo 25:ca5ccbf55b07 650
Olaffo 25:ca5ccbf55b07 651 // Pass torque
Olaffo 25:ca5ccbf55b07 652 pulseWidth = Ts;
Olaffo 25:ca5ccbf55b07 653 }
Olaffo 25:ca5ccbf55b07 654
Olaffo 25:ca5ccbf55b07 655 // Case for rotational speed control
Olaffo 25:ca5ccbf55b07 656 else {
Olaffo 25:ca5ccbf55b07 657
Olaffo 25:ca5ccbf55b07 658 // Flip lead
Olaffo 25:ca5ccbf55b07 659 if ( Tr >= 0) { lead = -2; }
Olaffo 25:ca5ccbf55b07 660 else if ( Tr < 0) { lead = 2; }
Olaffo 25:ca5ccbf55b07 661
Olaffo 25:ca5ccbf55b07 662 // Assure torque is in bounds
Olaffo 25:ca5ccbf55b07 663 if (Tr > 2000){ Tr = 2000; }
Olaffo 25:ca5ccbf55b07 664 else if (Tr < 0){ Tr = -Tr; }
Olaffo 25:ca5ccbf55b07 665 else if (Tr < -2000){ Tr = 2000; }
Olaffo 25:ca5ccbf55b07 666
Olaffo 25:ca5ccbf55b07 667 // Pass torque
Olaffo 25:ca5ccbf55b07 668 pulseWidth = Tr;
Olaffo 25:ca5ccbf55b07 669 }
Olaffo 25:ca5ccbf55b07 670 }
tanyuzhuo 24:7dd4e187dd30 671 }
tanyuzhuo 24:7dd4e187dd30 672
Olaffo 25:ca5ccbf55b07 673
Olaffo 25:ca5ccbf55b07 674 // ONLY speed control (aka forever forward mode)
Olaffo 28:613a88b88074 675 else if (mode == FOREVER_FORWARD){
Olaffo 25:ca5ccbf55b07 676
Olaffo 25:ca5ccbf55b07 677 if ( Ts >= 0) { lead = -2; }
Olaffo 25:ca5ccbf55b07 678 else if ( Ts < 0) { lead = 2; }
Olaffo 25:ca5ccbf55b07 679
Olaffo 25:ca5ccbf55b07 680 // Assure torque is in bounds
Olaffo 25:ca5ccbf55b07 681 if (Ts > 2000){ Ts = 2000; }
Olaffo 25:ca5ccbf55b07 682 else if (Ts < 0){ Ts = -Ts; }
Olaffo 25:ca5ccbf55b07 683 else if (Ts < -2000){ Ts = 2000; }
Olaffo 25:ca5ccbf55b07 684
tanyuzhuo 24:7dd4e187dd30 685 pulseWidth = Ts;
tanyuzhuo 24:7dd4e187dd30 686 }
tanyuzhuo 24:7dd4e187dd30 687
Olaffo 25:ca5ccbf55b07 688 counter++;
tanyuzhuo 18:e48c0910c71e 689 motorTime.reset();
tanyuzhuo 18:e48c0910c71e 690 // Serial output to monitor speed and position
tanyuzhuo 18:e48c0910c71e 691 if(counter == 10){
tanyuzhuo 18:e48c0910c71e 692 counter = 0;
tanyuzhuo 18:e48c0910c71e 693 //display velocity and motor position
tanyuzhuo 21:34f440ae0227 694 putMessage(MOTOR_POS,0,(float)(motorPosition/6.0));
tanyuzhuo 20:5bd9f9e406d1 695 putMessage(MOTOR_SPEED,0,(float)(motorVelocity/6.0));
Olaffo 28:613a88b88074 696 //putMessage(ENC,encCount);
Olaffo 28:613a88b88074 697 //putMessage(LEAD,lead);
Olaffo 28:613a88b88074 698 putMessage(FINE_TUNE);
tanyuzhuo 18:e48c0910c71e 699 }
tanyuzhuo 24:7dd4e187dd30 700 olderror=error;
tanyuzhuo 18:e48c0910c71e 701 }
tanyuzhuo 21:34f440ae0227 702 }
peterith 13:c51d828531d5 703 int main() {
tanyuzhuo 17:ff5300ba5442 704 //Initialise bincoin mining and communication
tanyuzhuo 20:5bd9f9e406d1 705
Olaffo 25:ca5ccbf55b07 706 // Start up checkup
tanyuzhuo 23:904721445e7a 707 d9.period(0.002f); //Set PWM period in seconds
Olaffo 25:ca5ccbf55b07 708 int8_t orState = motorHome();
Olaffo 25:ca5ccbf55b07 709 putMessage(WELCOME);
Olaffo 25:ca5ccbf55b07 710 putMessage(ROTOR_ORG);
Olaffo 28:613a88b88074 711
Olaffo 28:613a88b88074 712 IDC = 1;
peterith 14:0481b606d10e 713
Olaffo 25:ca5ccbf55b07 714 // Start all threads
Olaffo 25:ca5ccbf55b07 715 commsIn.start(commandProcessor);
tanyuzhuo 20:5bd9f9e406d1 716 commsOut.start(commsOutFunc);
tanyuzhuo 20:5bd9f9e406d1 717 motorCtrlT.start(motorCtrlFn);
tanyuzhuo 20:5bd9f9e406d1 718 bitcointhread.start(bitcoin);
estott 0:de4320f74764 719
Olaffo 25:ca5ccbf55b07 720 // Define motor ISRs
tanyuzhuo 20:5bd9f9e406d1 721 I1.rise(&motorISR);
tanyuzhuo 20:5bd9f9e406d1 722 I2.rise(&motorISR);
tanyuzhuo 20:5bd9f9e406d1 723 I3.rise(&motorISR);
peterith 13:c51d828531d5 724
tanyuzhuo 20:5bd9f9e406d1 725 I1.fall(&motorISR);
tanyuzhuo 20:5bd9f9e406d1 726 I2.fall(&motorISR);
tanyuzhuo 20:5bd9f9e406d1 727 I3.fall(&motorISR);
tanyuzhuo 20:5bd9f9e406d1 728
tanyuzhuo 16:10d53b056b17 729
estott 0:de4320f74764 730 }