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@2:47ec66bbe8f9, 2017-10-11 (annotated)

Committer:
peterknoben
Date:
Wed Oct 11 12:14:37 2017 +0000
Revision:
2:47ec66bbe8f9
Parent:
1:5681fcdc22fe
Child:
3:1514725c8cc8
Angle control with servo

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"
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