Diff: main.cpp

Revision:

6:8ab07cce3098

Parent:

5:d1ab07fd3355

Child:

7:6006a473ea0b

--- a/main.cpp	Wed Oct 14 10:33:34 2015 +0000
+++ b/main.cpp	Wed Oct 14 13:26:48 2015 +0000
@@ -18,21 +18,21 @@
 
 // Controller gain proportional and intergrator
 const double motor1_Kp = 5; // more or les random number.
-const double motor1_Ki = 0;
-const double M1_timestep = 0.001; // reason ticker works with 100 Hz.
+const double motor1_Ki = 0.5;
+const double M1_timestep = 0.01; // reason ticker works with 100 Hz.
 double motor1_error_integraal = 0; // initial value of integral error
 
+
 // calculating pulses to rotations in degree.
-const double Offset = 3450 ;//8400 counts is aangegeven op de motor. 3500 is almost 1 cirkel with a bit of overshoot 3450 looks good. about 10 - 20 counts oveshoot. for moter 1(tape)! Motor 2 almost the same(nice)
-const double resolution = Offset / 360;
-double Rotation = 2; // rotation in degree
-double movement = Rotation * 360 * resolution; // times 360 to make Rotations 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)
+double Rotation = -2; // rotation in degree
+double movement = Rotation * pulses_per_revolution; // times 360 to make Rotations degree.
 
 // defining flags
 volatile bool flag_motor = false;
 volatile bool flag_pcprint = false;
 
-// making flags.
+// making function flags.
 void Go_flag_motor()
 {
     flag_motor = true;
@@ -45,53 +45,50 @@
 
 // Reusable P controller
 double PI(double error, const double Kp, const double Ki, double Ts, double &e_int)
-{
-    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
-    
-    e_int = e_int + Ts * error_normalised_rotation;
-    
-    double PI_output = Kp * error_normalised_rotation + Ki * e_int; // 
+{ 
+
+  e_int = e_int + Ts * error;
+
+    double PI_output = (Kp * error) + (Ki * e_int);
     return PI_output;
 }
 // Next task, measure the error and apply the output to the plant
 void motor1_Controller()
 {
-    double reference = movement;
+    double reference = movement; // movement is in pulses
     double position = wheel.getPulses();
-    double error = reference - position;
+    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(PI( error_rotation, motor1_Kp, motor1_Ki, M1_timestep, motor1_error_integraal ));
 
-    double output = PI( error, motor1_Kp, motor1_Ki, M1_timestep, motor1_error_integraal );
-
-
-
-    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 possitif
-        //pc.printf("a");
+    
+    }
+    else if(error_pulses < 0) {
+        directionPin.write(ccw); 
     }
-
+    else{
+        output = 0;
+        }
+PWM.write(output); // out of the if loop due to abs output
 
 
 }
 
 
 void counts_showing()
-{ 
+{
 
- double kijken = wheel.getPulses();
-pc.printf("ref %.0f count%.0f int err %.2f \n",movement,kijken, motor1_error_integraal);
-    
-    }
+    double kijken = wheel.getPulses() / pulses_per_revolution;
+    pc.printf("ref %.0f rounds%.2f \n",Rotation,kijken);
+
+}
 
 int main()
 {
-    aansturen.attach( &Go_flag_motor, 0.001f ); // 100 Hz // timer 0.00001f motor keeps spinning
+    aansturen.attach( &Go_flag_motor, 0.01f ); // 100 Hz // timer 0.00001f motor keeps spinning
     Printen.attach(&Go_flag_pcprint,0.1f); // 10 Hz // printstatement here because printing cost to much time. the motor void wouldn't be completed
     while( 1 ) {