Diff: main.cpp

Revision:

13:a0113cf2fc5f

Parent:

12:af428d56b4eb

Child:

15:80af7a7671d4

--- a/main.cpp	Mon Sep 21 13:59:59 2015 +0000
+++ b/main.cpp	Mon Sep 21 14:32:58 2015 +0000
@@ -9,26 +9,49 @@
 DigitalIn button(PTA4);
 Encoder motor2(D13,D12);
 MODSERIAL pc(USBTX,USBRX);
+Ticker myControllerTicker;
 //AnalogIn potmeter2(A0); /NIEUW
+
+// constantes definieren
+double counts_per_revolution=4200;
+double degrees_per_turn=360;
+double counts_per_degree=counts_per_revolution/degrees_per_turn; //11.67 counts/degree
+const double motor2_Kp = 2.5; //constante motor
+
+//functies
+double P(double error, const double Kp)
+{
+    return Kp*error;
+}
+
+void motor2_Controller(){
+    double reference=400;
+    double position=motor2.getPosition();
+    double Error_position=P(reference-position,motor2_Kp);
+}
+
 int main()
 {
     pc.baud(9600);
     motor2.setPosition(0);
     while(true) {
         if (button.read() < 0.5) {   //if button pressed
-            motor2direction = 0;
+            motor2direction = 0; // zero is clockwise (front view)
             motor2speed = 0.5f;
             pc.printf("positie = %d \r\n", motor2.getPosition());
         } else {                  // If button is not pressed
             motor2direction = 0;
             motor2speed = 0;
-            pc.printf("positie = %d \r\n", motor2.getPosition());
+            while (motor2_Controller /// WHILE MOTOR > ... <... HIER BEZIG
+            pc.printf("positie = %d en Error_positie \r\n", motor2.getPosition(), Error_position);
+            //myControllerTicker.attach(&motor2_Controller,0.01f);
+            //while(1){}
         }
         
-        while ((motor2.getPosition()>4200) || (motor2.getPosition()<-4200)) // If value is outside -4200 and 4200 (number of counts equal to one revolution) reset to zero
+        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
         {
             motor2.setPosition(0);
-            pc.printf("HE \r\n LLO \r\n WO \r\n RLD \r\n !!! \r\n FOO! \r\n");
+            pc.printf(" HE \r\n LLO \r\n WO \r\n RLD \r\n !!! \r\n FOO! \r\n");
             break;
         } 
     }