Diff: main.cpp

Revision:

0:bbd4e22aca8a

Child:

1:5681fcdc22fe

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp	Wed Oct 11 08:50:26 2017 +0000
@@ -0,0 +1,92 @@
+#include "mbed.h"
+#include "encoder.h"
+
+Ticker timer;
+const double PI = 3.141592653589793;
+const double RAD_PER_PULSE = 0.000476190;
+float CONTROLLER_TS = 0.01;
+
+
+//Motor1
+PwmOut motor1(D5);
+DigitalOut motor1DirectionPin(D4);
+DigitalIn ENC2A(D12);
+DigitalIn ENC2B(D13);
+Encoder encoder1(D13,D12);
+AnalogIn potmeter1(A3);
+const double MOTOR1_KP = 0.5;
+const double MOTOR1_KI = 1.5;
+double m1_err_int = 0;
+const double motor1_gain = 2*PI;
+
+
+//Motor2
+PwmOut motor2(D6);
+DigitalOut motor2DirectionPin(D7);
+DigitalIn ENC1A(D10);
+DigitalIn ENC1B(D11);
+Encoder encoder2(D10,D11);
+AnalogIn potmeter2(A4);
+const double MOTOR2_KP = 0.5;
+const double MOTOR2_KI = 1.5;
+double m2_err_int = 0;
+const double motor2_gain = 2*PI;
+
+
+float getreferenceangle(const double PI, double potmeter)
+{
+    float max_angle = 2*PI;
+    float  reference_angle = max_angle * potmeter;
+    return reference_angle;    
+}
+
+double PI_controller(double error, const double Kp, const double Ki, double Ts, double &e_int) 
+{
+    e_int =+ Ts * error;
+    return Kp * error + Ki * e_int ;
+}
+
+void motor1_control(const double motor1_gain)
+{
+    double referenceangle1 = getreferenceangle(PI, potmeter1);
+    double position1 = RAD_PER_PULSE * encoder1.getPosition();
+    double magnitude1 = PI_controller(referenceangle1-position1, MOTOR1_KP, MOTOR1_KI, CONTROLLER_TS, m1_err_int) / motor1_gain;
+    motor1 = fabs(magnitude1);
+    
+    if (magnitude1 < 0)
+    {
+        motor1DirectionPin = 1;
+    }
+    else
+    {
+        motor1DirectionPin = 0;
+    }
+}
+
+void motor2_control(const double motor2_gain)
+{
+    double referenceangle2 = getreferenceangle(PI, potmeter2);
+    double position2 = RAD_PER_PULSE * encoder2.getPosition();
+    double magnitude2 = PI_controller(referenceangle2-position2, MOTOR2_KP, MOTOR2_KI, CONTROLLER_TS, m2_err_int) / motor2_gain;
+    motor1 = fabs(magnitude2);
+    
+    if (magnitude2 < 0)
+    {
+        motor1DirectionPin = 1;
+    }
+    else
+    {
+        motor1DirectionPin = 0;
+    }
+}
+
+
+int main()
+{
+    
+    while(1) 
+    {
+        motor1_control(motor1_gain);
+        motor2_control(motor1_gain);
+    }
+}
\ No newline at end of file