![](/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:
- 23:58de87ec3997
- Parent:
- 22:7c94575d7792
- Child:
- 24:0a0e22d4d8d3
diff -r 7c94575d7792 -r 58de87ec3997 main.cpp --- a/main.cpp Tue Mar 20 17:47:53 2018 +0000 +++ b/main.cpp Tue Mar 20 18:25:44 2018 +0000 @@ -22,6 +22,7 @@ #define CHAR_ARR_SIZE 18 //Max length of input codes #define MAX_PWM_PERIOD 2000 #define MAX_TORQUE 1000 +#define KP 25 //Mapping from sequential drive states to motor phase outputs /* @@ -143,30 +144,31 @@ motorCtrlT.signal_set(0x1); } -volatile float currentSpeed = 0; +volatile float curSpeed = 0; void motorCtrlFn(){ - int32_t posStart = 0, posEnd = 0; + static int32_t curPos = 0, oldPos = 0; Ticker motorCtrlTicker; motorCtrlTicker.attach_us(&motorCtrlTick,100000); - putMessage(MSG_TEST, 0x505); - uint8_t cnt = 0; + uint8_t count = 0; + Timer timer; + timer.start(); + uint32_t curTime = 0, oldTime = 0, timeDiff; while(1){ - posStart = motorPosition; motorCtrlT.signal_wait(0x1); - posEnd = motorPosition; - currentSpeed = (posEnd - posStart)*10; - - if(cnt >= 9) { - putMessage(MSG_CUR_SPD, currentSpeed); - // putMessage(MSG_TEST, posEnd); - cnt = 0; + curTime = timer.read(); + timeDiff = curTime - oldTime; + oldTime = curTime; + curPos = motorPosition; + curSpeed = (curPos - oldPos) / timeDiff; + oldPos = curPos; + count = (count+1)%10; + if (!count) { + putMessage(MSG_CUR_SPD, curSpeed); } - else{ cnt++; } - - torque = (uint32_t)(25*(maxspeed-abs(currentSpeed))); + torque = (uint32_t)(KP*(maxspeed-abs(curSpeed))); } }