Diff: move_motor.cpp

Revision:

0:fc6fa085d591

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/move_motor.cpp	Fri Oct 23 13:14:57 2015 +0000
@@ -0,0 +1,105 @@
+/*
+ * move_motor.cpp
+ *
+ *  Created on: Oct 20, 2015
+ *      Author: User
+ */
+
+#include "QEI.h"
+#include "mbed.h"
+
+
+
+//define the pins for the left and right
+
+// 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 timestep = 0.01; // reason ticker works with 100 Hz. ASSUMING TIME STEP IS THE SAME FOR BOTH MOTORS
+
+//we dont we Pi control so this can remain 0, for now
+double motor_error_integral = 0;
+
+// define rotation direction and begin position
+const int cw = 1;
+const int ccw = 0;
+
+// define ticker
+Ticker aansturen;
+
+// 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
+
+//How to pass Digitalout and PWM?
+void motor_Controller( DigitalOut &directionPin, PwmOut &pwm, double position, double setpoint, double Kp, double Ki, double error_rotation, double error_pulses) 
+{
+
+    double output = abs(PI( error_rotation, Kp, Ki, timestep, motor_error_integral ));
+
+// MOTORS ROTATE IN DIFFERENT DIRECTIONS WITH THE SAME DIRECTION INPUT SO WE NEED TO CHANGE THE ENCODER WIRES. CHANGING IN CODE IS TRICKIER. WONT BE UNIVERSAL
+    if(error_pulses > 0) 
+    {
+        directionPin.write(cw);
+    }
+    
+    else if(error_pulses < 0) 
+    {
+        directionPin.write(ccw);
+    }
+    //else output = 0
+
+    // out of the if loop due to abs output
+    pwm.write(output);
+}
+
+
+
+void move_motor(DigitalOut &directionPin, PwmOut &pwm, double position, double setpoint, double Kp, double Ki) //state can be left, right, or keypress
+{
+	//attach ticker to run the motor at 100hz
+	aansturen.attach( &Go_flag_motor, 0.01f ); // 100 Hz // timer 0.00001f motor keeps spinning // attaching the flag to the ticker
+	
+	//error rotation initialised to high value to enter the loop, control works until error is a small enough value to ignore
+	double error_rotation = 1;
+	
+	//run the control loop until the error is acceptable (3mm)
+	//nEED TO CALCULATE THE VALUE
+	while (error_rotation > 0.1) //error is not between -0.05 and 0.05 or something error should be a maximum of 3 mm
+	{
+		if(flag_motor)
+		{
+			flag_motor = false;
+			
+			//calculate the errors here so there is a way for the motor to stop
+			//could return it from the control function?
+			double error_pulses = (setpoint - position); // calculate the error in pulses
+
+    		error_rotation = error_pulses / pulses_per_revolution; //calculate how much the rotations the motor has to turn
+    
+			motor_Controller(directionPin, pwm, position, setpoint, Kp, Ki, error_rotation, error_pulses );
+		}
+	}
+}
+
+
+