![](/media/cache/img/default_profile.jpg.50x50_q85.jpg)
Angle control and Servo control
Dependencies: Encoder Servo mbed
Fork of Angle_control by
main.cpp@1:5681fcdc22fe, 2017-10-11 (annotated)
- Committer:
- DBerendsen
- Date:
- Wed Oct 11 09:33:32 2017 +0000
- Revision:
- 1:5681fcdc22fe
- Parent:
- 0:bbd4e22aca8a
- Child:
- 2:47ec66bbe8f9
beter;
Who changed what in which revision?
User | Revision | Line number | New 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 | 1:5681fcdc22fe | 71 | motor2 = fabs(magnitude2); |
DBerendsen | 0:bbd4e22aca8a | 72 | |
DBerendsen | 0:bbd4e22aca8a | 73 | if (magnitude2 < 0) |
DBerendsen | 0:bbd4e22aca8a | 74 | { |
DBerendsen | 1:5681fcdc22fe | 75 | motor2DirectionPin = 1; |
DBerendsen | 0:bbd4e22aca8a | 76 | } |
DBerendsen | 0:bbd4e22aca8a | 77 | else |
DBerendsen | 0:bbd4e22aca8a | 78 | { |
DBerendsen | 1:5681fcdc22fe | 79 | motor2DirectionPin = 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 | 1:5681fcdc22fe | 86 | while(1) |
DBerendsen | 1:5681fcdc22fe | 87 | { |
DBerendsen | 1:5681fcdc22fe | 88 | motor1_control(motor1_gain); |
DBerendsen | 1:5681fcdc22fe | 89 | wait(0.005f); |
DBerendsen | 1:5681fcdc22fe | 90 | motor2_control(motor2_gain); |
DBerendsen | 1:5681fcdc22fe | 91 | wait(0.005f); |
DBerendsen | 0:bbd4e22aca8a | 92 | } |
DBerendsen | 1:5681fcdc22fe | 93 | |
DBerendsen | 1:5681fcdc22fe | 94 | |
DBerendsen | 1:5681fcdc22fe | 95 | |
DBerendsen | 0:bbd4e22aca8a | 96 | } |