![](/media/cache/group/default_image.jpg.50x50_q85.jpg)
First Commit
Dependencies: mbed Crypto_light mbed-rtos
Spin it 2 win it
Diff: main.cpp
- Revision:
- 20:a435105305fe
- Parent:
- 19:526fd700e1b3
- Child:
- 21:0d1025dc6433
diff -r 526fd700e1b3 -r a435105305fe main.cpp --- a/main.cpp Tue Mar 20 15:10:08 2018 +0000 +++ b/main.cpp Tue Mar 20 16:51:38 2018 +0000 @@ -53,8 +53,8 @@ enum MSG {MSG_RESET, MSG_HASHCOUNT, MSG_NONCE_OK, - MSG_OVERFLOW, MSG_ROT_PEN, MSG_MAX_SPD, MSG_NEW_KEY, MSG_INP_ERR, - MSG_TEST, MSG_TORQUE}; + MSG_OVERFLOW, MSG_ROT_PEN, MSG_MAX_SPD, MSG_NEW_KEY, MSG_INP_ERR, MSG_TORQUE, + MSG_TEST, MSG_CUR_SPD}; //Instantiate the serial port RawSerial pc(SERIAL_TX, SERIAL_RX); @@ -77,9 +77,10 @@ + typedef struct { - uint8_t code; - int32_t data; + uint8_t code; + int32_t data; } message_t ; Mail<message_t,16> outMessages; @@ -92,7 +93,7 @@ outMessages.put(pMessage); } -Thread commOutT; +Thread commOutT (osPriorityNormal, 512); void commOutFn() { while(1) { @@ -121,15 +122,52 @@ uint8_t newChar = pc.getc(); inCharQ.put((void*)newChar); } +Thread motorCtrlT (osPriorityNormal, 2048); + +int8_t readRotorState(); +void motorOut(int8_t,uint32_t); +volatile int32_t motorPosition; + +void photointerrupter_isr() { + static int8_t oldRotorState = 0; + int8_t rotorState = readRotorState(); + motorOut((rotorState-orState+lead+6)%6, torque); //+6 to make sure the remainder is positive + if (rotorState - oldRotorState == 5) motorPosition--; + else if (rotorState - oldRotorState == -5) motorPosition++; + else motorPosition += (rotorState - oldRotorState); + oldRotorState = rotorState; +} + +//TODO Implement timer for greater accuracy +void motorCtrlTick(){ + motorCtrlT.signal_set(0x1); +} + +float currentSpeed = 0; -void photointerrupter_isr() -{ - int8_t intState = readRotorState(); - motorOut((intState-orState+lead+6)%6, torque); //+6 to make sure the remainder is positive +void motorCtrlFn(){ + int32_t posStart, posEnd; + Ticker motorCtrlTicker; + motorCtrlTicker.attach_us(&motorCtrlTick,100000); + // putMessage(MSG_TEST, 0x505); + uint8_t cnt = 0; + + while(1){ + posStart = motorPosition; + motorCtrlT.signal_wait(0x1); + posEnd = motorPosition; + currentSpeed = (posEnd-posStart)*10; + + if(cnt >= 9) { + // putMessage(MSG_CUR_SPD, currentSpeed); + cnt = 0; + } + else{ cnt++; } + } } -Thread decodeT; + void setNewCmd(char s[CHAR_ARR_SIZE]) { @@ -143,27 +181,32 @@ rotations_pending_mutex.unlock(); putMessage(MSG_ROT_PEN, rotations_pending); //V - } else if (sscanf(s, "V%f", &maxspeed_)) { + } + else if (sscanf(s, "V%f", &maxspeed_)) { maxspeed_mutex.lock(); maxspeed = maxspeed_; maxspeed_mutex.unlock(); putMessage(MSG_MAX_SPD, maxspeed); //K - } else if (sscanf(s, "K%llx", &newKey_)) { + } + else if (sscanf(s, "K%llx", &newKey_)) { newKey_mutex.lock(); newKey = newKey_; newKey_mutex.unlock(); putMessage(MSG_NEW_KEY, newKey); - } else if (sscanf(s, "T%u", &torque_)) { + } + else if (sscanf(s, "T%u", &torque_)) { torque = torque_; photointerrupter_isr(); //Give it a kick putMessage(MSG_TORQUE, torque); //ERROR - } else - putMessage(MSG_INP_ERR, 0x404); + } + else{ + putMessage(MSG_INP_ERR, 0x404); + } } - +Thread decodeT (osPriorityNormal, 512); void decodeFn() { pc.attach(&serialISR); @@ -193,10 +236,6 @@ } } - - - - volatile uint16_t hashcount = 0; void do_hashcount() { @@ -250,16 +289,15 @@ putMessage(MSG_RESET, 0); + commOutT.start(&commOutFn); - commOutT.start(&commOutFn); + motorCtrlT.start(&motorCtrlFn); decodeT.start(&decodeFn); -// pc.printf("Hello\n\r"); - //Run the motor synchronisation orState = motorHome(); -// pc.printf("Rotor origin: %x\n\r",orState); + //orState is subtracted from future rotor state inputs to align rotor and motor states I1.rise(&photointerrupter_isr); @@ -302,9 +340,6 @@ *key = newKey; - - - sha256.computeHash(hash, sequence, 64); if (hash[0] == 0 && hash[1] == 0) {