Diff: main.cpp

Revision:

15:80af7a7671d4

Parent:

13:a0113cf2fc5f

Child:

16:8975d6c900f0

--- a/main.cpp	Mon Sep 21 14:32:58 2015 +0000
+++ b/main.cpp	Mon Sep 21 15:02:31 2015 +0000
@@ -24,11 +24,7 @@
     return Kp*error;
 }
 
-void motor2_Controller(){
-    double reference=400;
-    double position=motor2.getPosition();
-    double Error_position=P(reference-position,motor2_Kp);
-}
+
 
 int main()
 {
@@ -42,10 +38,18 @@
         } else {                  // If button is not pressed
             motor2direction = 0;
             motor2speed = 0;
-            while (motor2_Controller /// WHILE MOTOR > ... <... HIER BEZIG
-            pc.printf("positie = %d en Error_positie \r\n", motor2.getPosition(), Error_position);
+            //while (motor2_Controller /// WHILE MOTOR > ... <... HIER BEZIG
+            double reference=400;
+            double position=motor2.getPosition();
+            double Error_position=(reference-position)*motor2_Kp;
+            pc.printf("positie = %d en Error_positie =%d \r\n", motor2.getPosition(), Error_position);
             //myControllerTicker.attach(&motor2_Controller,0.01f);
-            //while(1){}
+            if (Error_position > 0) {
+                motor2direction = 0;
+                motor2speed=0.3f;}
+            else {
+                motor2direction = 1;
+                motor2speed=0.3f;}
         }
         
         while ((motor2.getPosition()>counts_per_revolution) || (motor2.getPosition()<-counts_per_revolution)) // If value is outside -4200 and 4200 (number of counts equal to one revolution) reset to zero