Peter Knoben / Mbed 2 deprecated Angle_control_v3

Angle control and Servo control with kinematics

Dependencies:   Encoder FastPWM Servo mbed

Fork of Angle_control_v2 by Peter Knoben

main.cpp@0:bbd4e22aca8a, 2017-10-11 (annotated)

Committer:
DBerendsen
Date:
Wed Oct 11 08:50:26 2017 +0000
Revision:
0:bbd4e22aca8a
Child:
1:5681fcdc22fe
controlling 2 motors

Who changed what in which revision?

UserRevisionLine numberNew contents of line
DBerendsen 0:bbd4e22aca8a 1 #include "mbed.h"
DBerendsen 0:bbd4e22aca8a 2 #include "encoder.h"
DBerendsen 0:bbd4e22aca8a 3
DBerendsen 0:bbd4e22aca8a 4 Ticker timer;
DBerendsen 0:bbd4e22aca8a 5 const double PI = 3.141592653589793;
DBerendsen 0:bbd4e22aca8a 6 const double RAD_PER_PULSE = 0.000476190;
DBerendsen 0:bbd4e22aca8a 7 float CONTROLLER_TS = 0.01;
DBerendsen 0:bbd4e22aca8a 8
DBerendsen 0:bbd4e22aca8a 9
DBerendsen 0:bbd4e22aca8a 10 //Motor1
DBerendsen 0:bbd4e22aca8a 11 PwmOut motor1(D5);
DBerendsen 0:bbd4e22aca8a 12 DigitalOut motor1DirectionPin(D4);
DBerendsen 0:bbd4e22aca8a 13 DigitalIn ENC2A(D12);
DBerendsen 0:bbd4e22aca8a 14 DigitalIn ENC2B(D13);
DBerendsen 0:bbd4e22aca8a 15 Encoder encoder1(D13,D12);
DBerendsen 0:bbd4e22aca8a 16 AnalogIn potmeter1(A3);
DBerendsen 0:bbd4e22aca8a 17 const double MOTOR1_KP = 0.5;
DBerendsen 0:bbd4e22aca8a 18 const double MOTOR1_KI = 1.5;
DBerendsen 0:bbd4e22aca8a 19 double m1_err_int = 0;
DBerendsen 0:bbd4e22aca8a 20 const double motor1_gain = 2*PI;
DBerendsen 0:bbd4e22aca8a 21
DBerendsen 0:bbd4e22aca8a 22
DBerendsen 0:bbd4e22aca8a 23 //Motor2
DBerendsen 0:bbd4e22aca8a 24 PwmOut motor2(D6);
DBerendsen 0:bbd4e22aca8a 25 DigitalOut motor2DirectionPin(D7);
DBerendsen 0:bbd4e22aca8a 26 DigitalIn ENC1A(D10);
DBerendsen 0:bbd4e22aca8a 27 DigitalIn ENC1B(D11);
DBerendsen 0:bbd4e22aca8a 28 Encoder encoder2(D10,D11);
DBerendsen 0:bbd4e22aca8a 29 AnalogIn potmeter2(A4);
DBerendsen 0:bbd4e22aca8a 30 const double MOTOR2_KP = 0.5;
DBerendsen 0:bbd4e22aca8a 31 const double MOTOR2_KI = 1.5;
DBerendsen 0:bbd4e22aca8a 32 double m2_err_int = 0;
DBerendsen 0:bbd4e22aca8a 33 const double motor2_gain = 2*PI;
DBerendsen 0:bbd4e22aca8a 34
DBerendsen 0:bbd4e22aca8a 35
DBerendsen 0:bbd4e22aca8a 36 float getreferenceangle(const double PI, double potmeter)
DBerendsen 0:bbd4e22aca8a 37 {
DBerendsen 0:bbd4e22aca8a 38 float max_angle = 2*PI;
DBerendsen 0:bbd4e22aca8a 39 float reference_angle = max_angle * potmeter;
DBerendsen 0:bbd4e22aca8a 40 return reference_angle;
DBerendsen 0:bbd4e22aca8a 41 }
DBerendsen 0:bbd4e22aca8a 42
DBerendsen 0:bbd4e22aca8a 43 double PI_controller(double error, const double Kp, const double Ki, double Ts, double &e_int)
DBerendsen 0:bbd4e22aca8a 44 {
DBerendsen 0:bbd4e22aca8a 45 e_int =+ Ts * error;
DBerendsen 0:bbd4e22aca8a 46 return Kp * error + Ki * e_int ;
DBerendsen 0:bbd4e22aca8a 47 }
DBerendsen 0:bbd4e22aca8a 48
DBerendsen 0:bbd4e22aca8a 49 void motor1_control(const double motor1_gain)
DBerendsen 0:bbd4e22aca8a 50 {
DBerendsen 0:bbd4e22aca8a 51 double referenceangle1 = getreferenceangle(PI, potmeter1);
DBerendsen 0:bbd4e22aca8a 52 double position1 = RAD_PER_PULSE * encoder1.getPosition();
DBerendsen 0:bbd4e22aca8a 53 double magnitude1 = PI_controller(referenceangle1-position1, MOTOR1_KP, MOTOR1_KI, CONTROLLER_TS, m1_err_int) / motor1_gain;
DBerendsen 0:bbd4e22aca8a 54 motor1 = fabs(magnitude1);
DBerendsen 0:bbd4e22aca8a 55
DBerendsen 0:bbd4e22aca8a 56 if (magnitude1 < 0)
DBerendsen 0:bbd4e22aca8a 57 {
DBerendsen 0:bbd4e22aca8a 58 motor1DirectionPin = 1;
DBerendsen 0:bbd4e22aca8a 59 }
DBerendsen 0:bbd4e22aca8a 60 else
DBerendsen 0:bbd4e22aca8a 61 {
DBerendsen 0:bbd4e22aca8a 62 motor1DirectionPin = 0;
DBerendsen 0:bbd4e22aca8a 63 }
DBerendsen 0:bbd4e22aca8a 64 }
DBerendsen 0:bbd4e22aca8a 65
DBerendsen 0:bbd4e22aca8a 66 void motor2_control(const double motor2_gain)
DBerendsen 0:bbd4e22aca8a 67 {
DBerendsen 0:bbd4e22aca8a 68 double referenceangle2 = getreferenceangle(PI, potmeter2);
DBerendsen 0:bbd4e22aca8a 69 double position2 = RAD_PER_PULSE * encoder2.getPosition();
DBerendsen 0:bbd4e22aca8a 70 double magnitude2 = PI_controller(referenceangle2-position2, MOTOR2_KP, MOTOR2_KI, CONTROLLER_TS, m2_err_int) / motor2_gain;
DBerendsen 0:bbd4e22aca8a 71 motor1 = fabs(magnitude2);
DBerendsen 0:bbd4e22aca8a 72
DBerendsen 0:bbd4e22aca8a 73 if (magnitude2 < 0)
DBerendsen 0:bbd4e22aca8a 74 {
DBerendsen 0:bbd4e22aca8a 75 motor1DirectionPin = 1;
DBerendsen 0:bbd4e22aca8a 76 }
DBerendsen 0:bbd4e22aca8a 77 else
DBerendsen 0:bbd4e22aca8a 78 {
DBerendsen 0:bbd4e22aca8a 79 motor1DirectionPin = 0;
DBerendsen 0:bbd4e22aca8a 80 }
DBerendsen 0:bbd4e22aca8a 81 }
DBerendsen 0:bbd4e22aca8a 82
DBerendsen 0:bbd4e22aca8a 83
DBerendsen 0:bbd4e22aca8a 84 int main()
DBerendsen 0:bbd4e22aca8a 85 {
DBerendsen 0:bbd4e22aca8a 86
DBerendsen 0:bbd4e22aca8a 87 while(1)
DBerendsen 0:bbd4e22aca8a 88 {
DBerendsen 0:bbd4e22aca8a 89 motor1_control(motor1_gain);
DBerendsen 0:bbd4e22aca8a 90 motor2_control(motor1_gain);
DBerendsen 0:bbd4e22aca8a 91 }
DBerendsen 0:bbd4e22aca8a 92 }