Angle control and Servo control
Dependencies: Encoder Servo mbed
Fork of Angle_control by
Diff: main.cpp
- Revision:
- 0:bbd4e22aca8a
- Child:
- 1:5681fcdc22fe
diff -r 000000000000 -r bbd4e22aca8a main.cpp --- /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