Diff: main.cpp

Revision:

4:1b4885298ade

Parent:

3:b9e2c7d52953

Child:

5:d8195d9d098f

--- a/main.cpp	Wed Oct 14 12:47:44 2015 +0000
+++ b/main.cpp	Wed Oct 14 12:58:25 2015 +0000
@@ -22,9 +22,8 @@
 
 // calculating pulses to rotations in degree.
 const double pulses_per_revolution = 4200 ;//8400 counts is aangegeven op de motor for x4.  10 - 30 counts oveshoot. for moter 1(tape)! Motor 2 almost the same(nice)
-const double resolution = pulses_per_revolution / 360;
 double Rotation = -2; // rotation in degree
-double movement = Rotation * 360 * resolution; // times 360 to make Rotations degree.
+double movement = Rotation * pulses_per_revolution; // times 360 to make Rotations degree.
 
 // defining flags
 volatile bool flag_motor = false;
@@ -43,11 +42,9 @@
 
 // Reusable P controller
 double P(double error, const double Kp)
-{
-    double error_normalised_degree = error / resolution; // calculate how much degree the motor has to turn.
-    double error_normalised_rotation = error_normalised_degree / 360; //calculate how much the rotaions the motor has to turn
+{ 
 
-    double P_output = Kp * error_normalised_rotation;
+    double P_output = Kp * error;
     return P_output;
 }
 // Next task, measure the error and apply the output to the plant
@@ -55,24 +52,19 @@
 {
     double reference = movement;
     double position = wheel.getPulses();
-    double error = reference - position;
-
-    double output = P( error, motor1_Kp);
+    double error_pulses = (reference - position); // calculate the error in pulses
+    double error_rotation = error_pulses / pulses_per_revolution; //calculate how much the rotaions the motor has to turn
+    
+    double output = abs(P( error_rotation, motor1_Kp));
 
-
-
-    if(error > 0) {
+    if(error_pulses > 0) {
         directionPin.write(cw);
-        PWM.write(output);
-        //pc.printf("ref %.0f count%.0f cw\n",movement,position);
-
+    
     }
-    if(error < 0) {
-        directionPin.write(ccw);
-        PWM.write(-output); //min output to get output positive
-        //pc.printf("a");
+    if(error_pulses < 0) {
+        directionPin.write(ccw); 
     }
-
+PWM.write(output); // out of the if loop due to abs output
 
 
 }