Diff: main.cpp

Revision:

0:bb0fd6c1d178

Child:

1:63edfaecc73e

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp	Fri Oct 09 07:49:40 2015 +0000
@@ -0,0 +1,82 @@
+#include "mbed.h"
+#include "QEI.h"
+
+Serial pc(USBTX, USBRX);
+QEI wheel (PTC10, PTC11, NC, 624); // Pin for counting (analog in)
+
+// Define pin for motor control
+DigitalOut directionPin(D4);
+PwmOut PWM(D5);
+
+// define ticker
+Ticker aansturen;
+Ticker Printen;
+
+// define rotation direction
+const int cw = 1;
+const int ccw = 0;
+
+// Controller gain proportional and intergrator
+const double motor1_Kp = 5; // more or les random number.
+
+
+// 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.
+
+
+
+// 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;
+    return P_output;
+}
+// Next task, measure the error and apply the output to the plant
+void motor1_Controller()
+{
+    double reference = movement;
+    double position = wheel.getPulses();
+    double error = reference - position;
+
+    double output = P( error, motor1_Kp);
+
+
+
+    if(error > 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");
+    }
+
+
+
+}
+
+
+void counts_showing()
+{ 
+
+ double kijken = wheel.getPulses();
+pc.printf("ref %.0f count%.0f \n",movement,kijken);
+    
+    }
+
+int main()
+{
+    aansturen.attach( &motor1_Controller, 0.1f ); // 100 Hz
+    Printen.attach(&counts_showing,1); // 10 Hz // printstatement here because printing cost to much time. the motor void wouldn't be completed
+    while( 1 ) { }
+}
+
+