Diff: main.cpp

Revision:

21:fb17e0b8924e

Parent:

20:4d128c3f1228

Child:

22:d22381fe3b16

--- a/main.cpp	Tue Sep 22 12:56:18 2015 +0000
+++ b/main.cpp	Tue Sep 22 13:47:11 2015 +0000
@@ -7,7 +7,7 @@
 DigitalOut motor2direction(D4); //D4 en D5 zijn motor 2 (op het motorshield)
 PwmOut motor2speed(D5); // D5 is an input on the motor that controls the speed of motor number 2
 DigitalIn button(PTA4); // PTA4 is used as a button controller (0 when pressed and 1 when not pressed)
-Encoder motor2(D13,D12); // The encoder is able to read and set position/speed values
+Encoder motor2(D13,D12,true); // The encoder is able to read and set position/speed values
 MODSERIAL pc(USBTX,USBRX); // This input is used to send data to the pc
 Ticker TickerController; // This adds a Ticker to the function +TickerController
 //AnalogIn potmeter2(A0); /NIEUW
@@ -17,6 +17,7 @@
 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; //controller gain which will be multiplied with the error (*how fast will the error be corrected)
+const double reference = 400;
 
 // Function P-controller
 double P(double error, const double Kp) //returns error * controller gain
@@ -24,31 +25,37 @@
     return Kp*error; 
 }
 // Function that calls the P-controller
-void motor2_controller() // Void function that compares the current position with a reference value and outputs 
+double motor2_controller() // Void function that compares the current position with a reference value and outputs 
     {
-        double reference=400; 
         double position=motor2.getPosition(); //current motor position
-        double motor2_p_output=P((reference-position),motor2_Kp); // reference-position=> current -> this is multiplied with the controller gain motor2_Kp
-        pc.printf("ik doe het positie = %d en Error_positie =%d \r\n", position, (reference-position));
+        double error=reference-position;
+        pc.printf("ik doe het positie = %d en error =%d en reference=%d \r\n", position, error, reference);
+        return  P(error,motor2_Kp);
     }
 
 // MAIN
 int main()
 {
     pc.baud(9600); // baud rate at which the information is processed to the computer
-    motor2.setPosition(0); // calls the current position of the motor zero
+    //motor2.setPosition(0); // calls the current position of the motor zero
     while(true) {
         if (button.read() < 0.5) {   //if button pressed
             motor2direction = 0; // zero is clockwise (front view)
             motor2speed = 0.5f; // motorspeed
             pc.printf("positie = %d \r\n", motor2.getPosition());
         } else {                  // If button is not pressed
-            motor2direction = 0;
-            motor2speed = 0;
-            TickerController.attach(&motor2_controller,0.01f);
-            while(1){} //100Hz
+            double output_motor = motor2_controller();
+            while(output_motor>0){
+        motor2speed=output_motor;
+        motor2direction=1;
+        break;}
+        while(output_motor<0){
+        motor2speed=fabs(output_motor);
+        motor2direction=0;
+        break;}
+            pc.printf("else loop controller");
         }
-            
+    
 //  while (Error_position > 0) {
 //                motor2direction = 0;
 //                motor2speed=0.3f;}