Revision 7:04dff5b8490c, committed 2017-03-16

Comitter:

po514

Date:

Thu Mar 16 13:57:47 2017 +0000

Parent:

6:d881a556eb9a

Commit message:

constants updated

Changed in this revision

diff -r d881a556eb9a -r 04dff5b8490c main.cpp
--- a/main.cpp	Thu Mar 16 13:03:08 2017 +0000
+++ b/main.cpp	Thu Mar 16 13:57:47 2017 +0000
@@ -40,9 +40,11 @@
 const int8_t lead = 2;  //2 for forwards, -2 for backwards
 
 volatile float revTime;
-volatile float revPerMin;
+volatile float revPerSec;
+volatile float errorPrev = 0;
 volatile float dutyCycle;
 volatile float targetSpeed;
+volatile float acc;
 
 volatile float timeMap[] = {0.0 , 0.0 , 0.0 , 0.0 , 0.0 , 0.0 };
 
@@ -98,15 +100,19 @@
     led3 = !led3;
     float speedTime;
     speedTime = t.read();
-    revPerMin = 1.0/(speedTime - timeMap[I1 + 2*I2 + 4*I3]);
+    revPerSec = 1.0/(speedTime - timeMap[I1 + 2*I2 + 4*I3]);
     timeMap[I1 + 2*I2 + 4*I3] = speedTime;
 }   
 
 void controlLoop(void){
     while(true){
-        float error = targetSpeed - revPerMin;
-        float k = 10.0;
-        dutyCycle = k*error;
+        float error = targetSpeed - revPerSec;
+        float errorDer = (error - errorPrev)*20;
+        float k = 2.0;
+        float kd = 1;
+        dutyCycle = k*error + kd*errorDer;
+        
+        errorPrev = error; //remeber error
         wait(0.05);
     }
 }
@@ -165,7 +171,7 @@
     Serial pc(SERIAL_TX, SERIAL_RX);
     int8_t intState = 0;
     int8_t intStateOld = 0;
-    float per = 0.02f; 
+    float per = 0.01f; 
     targetSpeed = 3.0;
     
     L1L.period(per);