Angle control and Servo control
Dependencies: Encoder Servo mbed
Fork of Angle_control by
main.cpp@2:47ec66bbe8f9, 2017-10-11 (annotated)
- Committer:
- peterknoben
- Date:
- Wed Oct 11 12:14:37 2017 +0000
- Revision:
- 2:47ec66bbe8f9
- Parent:
- 1:5681fcdc22fe
Angle control with servo
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" |
peterknoben | 2:47ec66bbe8f9 | 3 | #include "Servo.h" |
DBerendsen | 0:bbd4e22aca8a | 4 | |
peterknoben | 2:47ec66bbe8f9 | 5 | Ticker MyControllerTicker1; |
peterknoben | 2:47ec66bbe8f9 | 6 | Ticker MyControllerTicker2; |
DBerendsen | 0:bbd4e22aca8a | 7 | const double PI = 3.141592653589793; |
DBerendsen | 0:bbd4e22aca8a | 8 | const double RAD_PER_PULSE = 0.000476190; |
peterknoben | 2:47ec66bbe8f9 | 9 | const double CONTROLLER_TS = 0.01; |
DBerendsen | 0:bbd4e22aca8a | 10 | |
DBerendsen | 0:bbd4e22aca8a | 11 | |
DBerendsen | 0:bbd4e22aca8a | 12 | //Motor1 |
DBerendsen | 0:bbd4e22aca8a | 13 | PwmOut motor1(D5); |
DBerendsen | 0:bbd4e22aca8a | 14 | DigitalOut motor1DirectionPin(D4); |
DBerendsen | 0:bbd4e22aca8a | 15 | DigitalIn ENC2A(D12); |
DBerendsen | 0:bbd4e22aca8a | 16 | DigitalIn ENC2B(D13); |
DBerendsen | 0:bbd4e22aca8a | 17 | Encoder encoder1(D13,D12); |
DBerendsen | 0:bbd4e22aca8a | 18 | AnalogIn potmeter1(A3); |
DBerendsen | 0:bbd4e22aca8a | 19 | const double MOTOR1_KP = 0.5; |
DBerendsen | 0:bbd4e22aca8a | 20 | const double MOTOR1_KI = 1.5; |
DBerendsen | 0:bbd4e22aca8a | 21 | double m1_err_int = 0; |
DBerendsen | 0:bbd4e22aca8a | 22 | const double motor1_gain = 2*PI; |
DBerendsen | 0:bbd4e22aca8a | 23 | |
DBerendsen | 0:bbd4e22aca8a | 24 | |
DBerendsen | 0:bbd4e22aca8a | 25 | //Motor2 |
DBerendsen | 0:bbd4e22aca8a | 26 | PwmOut motor2(D6); |
DBerendsen | 0:bbd4e22aca8a | 27 | DigitalOut motor2DirectionPin(D7); |
DBerendsen | 0:bbd4e22aca8a | 28 | DigitalIn ENC1A(D10); |
DBerendsen | 0:bbd4e22aca8a | 29 | DigitalIn ENC1B(D11); |
DBerendsen | 0:bbd4e22aca8a | 30 | Encoder encoder2(D10,D11); |
DBerendsen | 0:bbd4e22aca8a | 31 | AnalogIn potmeter2(A4); |
DBerendsen | 0:bbd4e22aca8a | 32 | const double MOTOR2_KP = 0.5; |
DBerendsen | 0:bbd4e22aca8a | 33 | const double MOTOR2_KI = 1.5; |
DBerendsen | 0:bbd4e22aca8a | 34 | double m2_err_int = 0; |
DBerendsen | 0:bbd4e22aca8a | 35 | const double motor2_gain = 2*PI; |
DBerendsen | 0:bbd4e22aca8a | 36 | |
peterknoben | 2:47ec66bbe8f9 | 37 | //Servo |
peterknoben | 2:47ec66bbe8f9 | 38 | Servo MyServo(D9); |
peterknoben | 2:47ec66bbe8f9 | 39 | InterruptIn But1(D8); |
peterknoben | 2:47ec66bbe8f9 | 40 | int k=0; |
DBerendsen | 0:bbd4e22aca8a | 41 | |
peterknoben | 2:47ec66bbe8f9 | 42 | |
peterknoben | 2:47ec66bbe8f9 | 43 | float getreferenceangle(const double PI, double potmeter){ |
DBerendsen | 0:bbd4e22aca8a | 44 | float max_angle = 2*PI; |
DBerendsen | 0:bbd4e22aca8a | 45 | float reference_angle = max_angle * potmeter; |
DBerendsen | 0:bbd4e22aca8a | 46 | return reference_angle; |
DBerendsen | 0:bbd4e22aca8a | 47 | } |
DBerendsen | 0:bbd4e22aca8a | 48 | |
peterknoben | 2:47ec66bbe8f9 | 49 | double PI_controller(double error, const double Kp, const double Ki, double Ts, double &e_int) { |
DBerendsen | 0:bbd4e22aca8a | 50 | e_int =+ Ts * error; |
DBerendsen | 0:bbd4e22aca8a | 51 | return Kp * error + Ki * e_int ; |
DBerendsen | 0:bbd4e22aca8a | 52 | } |
DBerendsen | 0:bbd4e22aca8a | 53 | |
peterknoben | 2:47ec66bbe8f9 | 54 | void motor1_control(){ |
DBerendsen | 0:bbd4e22aca8a | 55 | double referenceangle1 = getreferenceangle(PI, potmeter1); |
DBerendsen | 0:bbd4e22aca8a | 56 | double position1 = RAD_PER_PULSE * encoder1.getPosition(); |
DBerendsen | 0:bbd4e22aca8a | 57 | double magnitude1 = PI_controller(referenceangle1-position1, MOTOR1_KP, MOTOR1_KI, CONTROLLER_TS, m1_err_int) / motor1_gain; |
DBerendsen | 0:bbd4e22aca8a | 58 | motor1 = fabs(magnitude1); |
DBerendsen | 0:bbd4e22aca8a | 59 | |
peterknoben | 2:47ec66bbe8f9 | 60 | if (magnitude1 < 0){ |
DBerendsen | 0:bbd4e22aca8a | 61 | motor1DirectionPin = 1; |
DBerendsen | 0:bbd4e22aca8a | 62 | } |
peterknoben | 2:47ec66bbe8f9 | 63 | else{ |
DBerendsen | 0:bbd4e22aca8a | 64 | motor1DirectionPin = 0; |
DBerendsen | 0:bbd4e22aca8a | 65 | } |
DBerendsen | 0:bbd4e22aca8a | 66 | } |
DBerendsen | 0:bbd4e22aca8a | 67 | |
peterknoben | 2:47ec66bbe8f9 | 68 | |
peterknoben | 2:47ec66bbe8f9 | 69 | void motor2_control(){ |
DBerendsen | 0:bbd4e22aca8a | 70 | double referenceangle2 = getreferenceangle(PI, potmeter2); |
DBerendsen | 0:bbd4e22aca8a | 71 | double position2 = RAD_PER_PULSE * encoder2.getPosition(); |
DBerendsen | 0:bbd4e22aca8a | 72 | double magnitude2 = PI_controller(referenceangle2-position2, MOTOR2_KP, MOTOR2_KI, CONTROLLER_TS, m2_err_int) / motor2_gain; |
DBerendsen | 1:5681fcdc22fe | 73 | motor2 = fabs(magnitude2); |
DBerendsen | 0:bbd4e22aca8a | 74 | |
peterknoben | 2:47ec66bbe8f9 | 75 | if (magnitude2 < 0){ |
DBerendsen | 1:5681fcdc22fe | 76 | motor2DirectionPin = 1; |
DBerendsen | 0:bbd4e22aca8a | 77 | } |
peterknoben | 2:47ec66bbe8f9 | 78 | else{ |
DBerendsen | 1:5681fcdc22fe | 79 | motor2DirectionPin = 0; |
DBerendsen | 0:bbd4e22aca8a | 80 | } |
DBerendsen | 0:bbd4e22aca8a | 81 | } |
DBerendsen | 0:bbd4e22aca8a | 82 | |
peterknoben | 2:47ec66bbe8f9 | 83 | void servo_control (){ |
peterknoben | 2:47ec66bbe8f9 | 84 | if (k==0){ |
peterknoben | 2:47ec66bbe8f9 | 85 | MyServo = 0; |
peterknoben | 2:47ec66bbe8f9 | 86 | k=1; |
peterknoben | 2:47ec66bbe8f9 | 87 | } |
peterknoben | 2:47ec66bbe8f9 | 88 | else{ |
peterknoben | 2:47ec66bbe8f9 | 89 | MyServo = 2; |
peterknoben | 2:47ec66bbe8f9 | 90 | k=0; |
peterknoben | 2:47ec66bbe8f9 | 91 | } |
peterknoben | 2:47ec66bbe8f9 | 92 | } |
DBerendsen | 0:bbd4e22aca8a | 93 | |
peterknoben | 2:47ec66bbe8f9 | 94 | |
peterknoben | 2:47ec66bbe8f9 | 95 | int main(){ |
peterknoben | 2:47ec66bbe8f9 | 96 | //motor1.period(0.01); |
peterknoben | 2:47ec66bbe8f9 | 97 | MyControllerTicker1.attach(&motor1_control, CONTROLLER_TS); |
peterknoben | 2:47ec66bbe8f9 | 98 | MyControllerTicker2.attach(&motor2_control, CONTROLLER_TS); |
peterknoben | 2:47ec66bbe8f9 | 99 | But1.rise(&servo_control); |
peterknoben | 2:47ec66bbe8f9 | 100 | |
peterknoben | 2:47ec66bbe8f9 | 101 | while(1) {} |
peterknoben | 2:47ec66bbe8f9 | 102 | } |
peterknoben | 2:47ec66bbe8f9 | 103 | |
peterknoben | 2:47ec66bbe8f9 | 104 | //{while(1) |
peterknoben | 2:47ec66bbe8f9 | 105 | // motor1_control(motor1_gain); |
peterknoben | 2:47ec66bbe8f9 | 106 | // wait(0.005f); |
peterknoben | 2:47ec66bbe8f9 | 107 | // motor2_control(motor2_gain); |
peterknoben | 2:47ec66bbe8f9 | 108 | // wait(0.005f); |
peterknoben | 2:47ec66bbe8f9 | 109 | // } |
peterknoben | 2:47ec66bbe8f9 | 110 |