Revision 0:7098cdb29180, committed 2015-10-19

Comitter:

roosbulthuis

Date:

Mon Oct 19 13:43:58 2015 +0000

Commit message:

function for motor control we still need values for inputs and we dont know how to refer to PWM and directionPin. And how can we declare QEI wheel differently for two motors?

Changed in this revision

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/QEI.lib	Mon Oct 19 13:43:58 2015 +0000
@@ -0,0 +1,1 @@
+http://developer.mbed.org/users/aberk/code/QEI/#5c2ad81551aa

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/mbed.bld	Mon Oct 19 13:43:58 2015 +0000
@@ -0,0 +1,1 @@
+http://mbed.org/users/mbed_official/code/mbed/builds/34e6b704fe68
\ No newline at end of file

--- /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

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/movemotor.h	Mon Oct 19 13:43:58 2015 +0000
@@ -0,0 +1,17 @@
+#ifndef MOVEMOTOR_H_
+#define MOVEMOTOR_H_
+
+void Go_flag_motor();
+double PI(double error, const double Kp, const double Ki, double Ts, double &e_int);
+void motor1_Controller();
+void move_motor(string state);
+
+Ticker aansturen;
+volatile bool flag_motor;
+QEI wheel (PTC10, PTC11, NC, 624); //how to call this?
+const double pulses_per_revolution;
+const double M1_timestep;
+const int cw;
+const int ccw;
+
+#endif /* MOVEMOTOR_H_ */
\ No newline at end of file