Angle control and Servo control
Dependencies: Encoder Servo mbed
Fork of Angle_control by
main.cpp
- Committer:
- DBerendsen
- Date:
- 2017-10-11
- Revision:
- 1:5681fcdc22fe
- Parent:
- 0:bbd4e22aca8a
- Child:
- 2:47ec66bbe8f9
File content as of revision 1:5681fcdc22fe:
#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; motor2 = fabs(magnitude2); if (magnitude2 < 0) { motor2DirectionPin = 1; } else { motor2DirectionPin = 0; } } int main() { while(1) { motor1_control(motor1_gain); wait(0.005f); motor2_control(motor2_gain); wait(0.005f); } }