Diff: main.cpp

Revision:

4:bdd445879de2

Parent:

3:69c670ed6382

Child:

5:8269e635183f

--- a/main.cpp	Thu Mar 16 11:39:12 2017 +0000
+++ b/main.cpp	Thu Mar 16 12:20:01 2017 +0000
@@ -42,6 +42,7 @@
 volatile float revTime;
 volatile float revPerMin;
 volatile float dutyCycle;
+volatile float targetSpeed;
 
 //Status LED
 DigitalOut led1(LED1);
@@ -56,7 +57,7 @@
 
 Ticker tick;
 Thread th1;
-Thread th2;
+Thread controlLoopThread;
 Timer t;
 
 Serial pc(SERIAL_TX,SERIAL_RX);
@@ -88,15 +89,23 @@
     }
 }
 
-void toggleLED(){
+void mesRot(){
     led3 = !led3;
     float speedTime;
     speedTime = t.read();
     revPerMin = 1.0/(speedTime - revTime);
     revTime = speedTime;
-    pc.printf("speed: %f \n\r", revPerMin); 
 }   
 
+void controlLoop(void){
+    while(true){
+        float error = targetSpeed - revPerMin;
+        float k = 10.0;
+        dutyCycle = k*error;
+        wait(0.1);
+    }
+}
+
 void decrease(){
     dutyCycle -= 0.05;
     pc.printf("current value: %f", dutyCycle);
@@ -152,6 +161,7 @@
     int8_t intState = 0;
     int8_t intStateOld = 0;
     float per = 0.02f; 
+    targetSpeed = 10.0;
     
     L1L.period(per);
     L1H.period(per);
@@ -168,7 +178,8 @@
     //th2.set_priority(osPriorityRealtime);
     //th1.start(blinkLED2);
     //th2.start(blinkLED3);
-    I1intr.rise(&toggleLED);
+    I1intr.rise(&mesRot); //start photoInterrupt
+    controlLoopThread.start(controlLoop);  //start conrol unit thread
     t.start();
     //tick.attach(&decrease, 10);
     //Poll the rotor state and set the motor outputs accordingly to spin the motor