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)));
     }
 }