Diff: movemotor.cpp

Revision:

0:7098cdb29180

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/movemotor.cpp	Mon Oct 19 13:43:58 2015 +0000
@@ -0,0 +1,109 @@
+#include "QEI.h"
+#include "mbed.h"
+
+// Defining pulses per revolution (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 M1_timestep = 0.01; // reason ticker works with 100 Hz. ASSUMING TIME STEP IS THE SAME FOR BOTH MOTORS
+
+// define rotation direction and begin possition
+const int cw = 1;
+const int ccw = 0;
+
+// define ticker
+Ticker aansturen;
+
+QEI wheel (PTC10, PTC11, NC, 624); // Pin for counting (analog in) // ask victor whether it can be in if-loop for the two different motors
+
+// defining flags
+volatile bool flag_motor = false;
+
+
+// making function flags.
+void Go_flag_motor()
+{
+    flag_motor = true;
+}
+
+
+// defining reusable P controller
+double PI(double error, const double Kp, const double Ki, double Ts, double &e_int)
+{ 
+
+  e_int = e_int + Ts * error;
+
+    double PI_output = (Kp * error) + (Ki * e_int);
+    return PI_output;
+}
+
+// Measure the error and apply the output to the plant
+void motor1_Controller(DigitalOut directionPin(), PwmOut PWM(), double position, double Setpoint, double Kp, double Ki) //How to pass Digitalout and PWM, by reference?
+{
+    double reference = setpoint; // setpoint is in pulses
+    double position = wheel.getPulses();
+    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 ));
+
+    if(error_pulses > 0) {
+        directionPin.write(cw);
+    }
+    else if(error_pulses < 0) {
+        directionPin.write(ccw); 
+    }
+    else{
+        output = 0;
+        }
+PWM.write(output); // out of the if loop due to abs output
+}
+
+
+
+void move_motor(std::string state) //state can be left, right, or keypress
+{
+    
+if (state == "left")
+{
+DigitalOut directionPin(D4); //direction
+PwmOut PWM(D5); //speed
+double position = wheel.getPulses(); //actual position
+double Setpoint; //desired position
+Kp; //controller gain
+Ki; //set to zero
+}
+
+else if (state == "right")
+{
+DigitalOut directionPin(D4);
+PwmOut PWM(D5);
+double position = wheel.getPulses(); //actual position
+double Setpoint; //desired position
+double Kp; //controller gain, same as for left
+double Ki; //set to zero
+}
+
+else if (state == "keypress") //different encoder than left and right
+{
+DigitalOut directionPin(D6); //direction
+PwmOut PWM(D7); //speed
+double position = wheel.getPulses(); //actual position, but different motor!
+double Setpoint; //desired position, always the same actually
+Kp; //controller gain, different from left and right
+Ki; //set to zero
+}
+
+
+
+//call the flags and p-controller
+
+
+aansturen.attach( &Go_flag_motor, 0.01f ); // 100 Hz // timer 0.00001f motor keeps spinning // attaching the flag to the ticker
+while (1) //error is not between -0.05 and 0.05 or something error should be a maximum of 3 mm
+{
+// to make the motor move
+        if(flag_motor) {
+            flag_motor = false;
+            motor1_Controller(directionPin, PWM, position, Setpoint, Kp, Ki); //How to pass Digitalout and PWM, by reference?
+        }
+    }
+}
\ No newline at end of file