Diff: main.cpp

Revision:

0:2b481a8db14f

Child:

1:71617831bc4e

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp	Thu Oct 08 11:35:49 2015 +0000
@@ -0,0 +1,68 @@
+#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;
+
+// define rotation direction
+const int cw = 1;
+const int ccw = 0;
+
+// Controller gain proportional
+const double motor1_Kp = 2.5;
+
+// calculating pulses to rotations in degree.
+const double Offset = 10341 ;//8400 counts is aangegeven op de motor ( is te weinig) 10511 schiet iets over
+const double resolution = Offset / 360;
+double Rotation = 4; // rotation in degree
+double movement = Rotation * 360 * resolution; // times 360 to make Rotations degree.
+
+
+
+// Reusable P controller
+double P(double error, 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);
+    } else {
+        directionPin.write(ccw);
+        PWM.write(output);
+    }
+
+
+
+}
+
+
+
+int main()
+{
+    aansturen.attach( &motor1_Controller, 0.01f ); // 100 Hz
+    while( 1 ) { }
+}
+
+
+