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@3:1514725c8cc8, 2017-10-16 (annotated)

Committer:
peterknoben
Date:
Mon Oct 16 12:15:09 2017 +0000
Revision:
3:1514725c8cc8
Parent:
2:47ec66bbe8f9
Child:
4:d385669b2f50
Wordt al mooier

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"
peterknoben 3:1514725c8cc8 4 #include "FastPWM.h"
DBerendsen 0:bbd4e22aca8a 5
peterknoben 2:47ec66bbe8f9 6 Ticker MyControllerTicker1;
peterknoben 2:47ec66bbe8f9 7 Ticker MyControllerTicker2;
DBerendsen 0:bbd4e22aca8a 8 const double PI = 3.141592653589793;
peterknoben 3:1514725c8cc8 9 const double RAD_PER_PULSE = 0.000749425;
peterknoben 2:47ec66bbe8f9 10 const double CONTROLLER_TS = 0.01;
DBerendsen 0:bbd4e22aca8a 11
DBerendsen 0:bbd4e22aca8a 12
DBerendsen 0:bbd4e22aca8a 13 //Motor1
DBerendsen 0:bbd4e22aca8a 14 PwmOut motor1(D5);
DBerendsen 0:bbd4e22aca8a 15 DigitalOut motor1DirectionPin(D4);
DBerendsen 0:bbd4e22aca8a 16 DigitalIn ENC2A(D12);
DBerendsen 0:bbd4e22aca8a 17 DigitalIn ENC2B(D13);
DBerendsen 0:bbd4e22aca8a 18 Encoder encoder1(D13,D12);
DBerendsen 0:bbd4e22aca8a 19 AnalogIn potmeter1(A3);
peterknoben 3:1514725c8cc8 20 const double MOTOR1_KP = 15;
peterknoben 3:1514725c8cc8 21 const double MOTOR1_KI = 10;
DBerendsen 0:bbd4e22aca8a 22 double m1_err_int = 0;
DBerendsen 0:bbd4e22aca8a 23 const double motor1_gain = 2*PI;
DBerendsen 0:bbd4e22aca8a 24
DBerendsen 0:bbd4e22aca8a 25
DBerendsen 0:bbd4e22aca8a 26 //Motor2
DBerendsen 0:bbd4e22aca8a 27 PwmOut motor2(D6);
DBerendsen 0:bbd4e22aca8a 28 DigitalOut motor2DirectionPin(D7);
DBerendsen 0:bbd4e22aca8a 29 DigitalIn ENC1A(D10);
DBerendsen 0:bbd4e22aca8a 30 DigitalIn ENC1B(D11);
DBerendsen 0:bbd4e22aca8a 31 Encoder encoder2(D10,D11);
DBerendsen 0:bbd4e22aca8a 32 AnalogIn potmeter2(A4);
peterknoben 3:1514725c8cc8 33 const double MOTOR2_KP = 15;
peterknoben 3:1514725c8cc8 34 const double MOTOR2_KI = 10;
DBerendsen 0:bbd4e22aca8a 35 double m2_err_int = 0;
DBerendsen 0:bbd4e22aca8a 36 const double motor2_gain = 2*PI;
DBerendsen 0:bbd4e22aca8a 37
peterknoben 3:1514725c8cc8 38 //________________________________________________________________
peterknoben 3:1514725c8cc8 39 //Test kinematica
peterknoben 3:1514725c8cc8 40
peterknoben 3:1514725c8cc8 41 //Motor offsets (kinematica implementation)
peterknoben 3:1514725c8cc8 42 float alphaoffset = 10;
peterknoben 3:1514725c8cc8 43 float betaoffset = 35;
peterknoben 3:1514725c8cc8 44 float x_target, y_target;
peterknoben 3:1514725c8cc8 45 float x, y;
peterknoben 3:1514725c8cc8 46 int max_rangex = 800;
peterknoben 3:1514725c8cc8 47 int max_rangey = 500;
peterknoben 3:1514725c8cc8 48
peterknoben 3:1514725c8cc8 49 float L1 = 450;
peterknoben 3:1514725c8cc8 50 float L2 = 490;
peterknoben 3:1514725c8cc8 51 float x_offset = 0.0;
peterknoben 3:1514725c8cc8 52 float y_offset = 0.0;
peterknoben 3:1514725c8cc8 53
peterknoben 3:1514725c8cc8 54
peterknoben 3:1514725c8cc8 55 float getreferencepositionx(double potmeter){
peterknoben 3:1514725c8cc8 56 x_target = potmeter * max_rangex;
peterknoben 3:1514725c8cc8 57 return x_target;
peterknoben 3:1514725c8cc8 58 }
peterknoben 3:1514725c8cc8 59 float getreferencepositiony(double potmeter){
peterknoben 3:1514725c8cc8 60 y_target = potmeter * max_rangey;
peterknoben 3:1514725c8cc8 61 return y_target;
peterknoben 3:1514725c8cc8 62 }
peterknoben 3:1514725c8cc8 63
peterknoben 3:1514725c8cc8 64 float getreferenceangle(){
peterknoben 3:1514725c8cc8 65 x_target = getreferencepositionx(potmeter1);
peterknoben 3:1514725c8cc8 66 y_target = getreferencepositiony(potmeter2);
peterknoben 3:1514725c8cc8 67 float x = x_target - x_offset;
peterknoben 3:1514725c8cc8 68 float y = y_target - y_offset;
peterknoben 3:1514725c8cc8 69 float L3_ = sqrt(x*x + y*y);
peterknoben 3:1514725c8cc8 70 float L3 = L3_;
peterknoben 3:1514725c8cc8 71
peterknoben 3:1514725c8cc8 72 float beta = 3.1415 - acos(((L1*L1 + L2*L2 - L3*L3)/(2 * L1 * L2)));
peterknoben 3:1514725c8cc8 73 beta = beta * 180 / 3.1415;
peterknoben 3:1514725c8cc8 74
peterknoben 3:1514725c8cc8 75 float alpha = (acos((L1*L1+L3*L3-L2*L2)/(2*L1*L3))) *180 / 3.1415;
peterknoben 3:1514725c8cc8 76 float beta_ = beta - alpha;
peterknoben 3:1514725c8cc8 77 }
peterknoben 3:1514725c8cc8 78
peterknoben 3:1514725c8cc8 79 //________________________________________________________________
peterknoben 3:1514725c8cc8 80
peterknoben 2:47ec66bbe8f9 81 //Servo
peterknoben 2:47ec66bbe8f9 82 Servo MyServo(D9);
peterknoben 2:47ec66bbe8f9 83 InterruptIn But1(D8);
peterknoben 2:47ec66bbe8f9 84 int k=0;
DBerendsen 0:bbd4e22aca8a 85
peterknoben 2:47ec66bbe8f9 86
peterknoben 3:1514725c8cc8 87 /*float getreferenceangle(const double PI, double potmeter){
DBerendsen 0:bbd4e22aca8a 88 float max_angle = 2*PI;
DBerendsen 0:bbd4e22aca8a 89 float reference_angle = max_angle * potmeter;
DBerendsen 0:bbd4e22aca8a 90 return reference_angle;
peterknoben 3:1514725c8cc8 91 }*/
DBerendsen 0:bbd4e22aca8a 92
peterknoben 2:47ec66bbe8f9 93 double PI_controller(double error, const double Kp, const double Ki, double Ts, double &e_int) {
DBerendsen 0:bbd4e22aca8a 94 e_int =+ Ts * error;
DBerendsen 0:bbd4e22aca8a 95 return Kp * error + Ki * e_int ;
DBerendsen 0:bbd4e22aca8a 96 }
DBerendsen 0:bbd4e22aca8a 97
peterknoben 2:47ec66bbe8f9 98 void motor1_control(){
DBerendsen 0:bbd4e22aca8a 99 double referenceangle1 = getreferenceangle(PI, potmeter1);
DBerendsen 0:bbd4e22aca8a 100 double position1 = RAD_PER_PULSE * encoder1.getPosition();
DBerendsen 0:bbd4e22aca8a 101 double magnitude1 = PI_controller(referenceangle1-position1, MOTOR1_KP, MOTOR1_KI, CONTROLLER_TS, m1_err_int) / motor1_gain;
DBerendsen 0:bbd4e22aca8a 102 motor1 = fabs(magnitude1);
DBerendsen 0:bbd4e22aca8a 103
peterknoben 2:47ec66bbe8f9 104 if (magnitude1 < 0){
DBerendsen 0:bbd4e22aca8a 105 motor1DirectionPin = 1;
DBerendsen 0:bbd4e22aca8a 106 }
peterknoben 2:47ec66bbe8f9 107 else{
DBerendsen 0:bbd4e22aca8a 108 motor1DirectionPin = 0;
DBerendsen 0:bbd4e22aca8a 109 }
DBerendsen 0:bbd4e22aca8a 110 }
DBerendsen 0:bbd4e22aca8a 111
peterknoben 2:47ec66bbe8f9 112
peterknoben 2:47ec66bbe8f9 113 void motor2_control(){
DBerendsen 0:bbd4e22aca8a 114 double referenceangle2 = getreferenceangle(PI, potmeter2);
DBerendsen 0:bbd4e22aca8a 115 double position2 = RAD_PER_PULSE * encoder2.getPosition();
DBerendsen 0:bbd4e22aca8a 116 double magnitude2 = PI_controller(referenceangle2-position2, MOTOR2_KP, MOTOR2_KI, CONTROLLER_TS, m2_err_int) / motor2_gain;
DBerendsen 1:5681fcdc22fe 117 motor2 = fabs(magnitude2);
DBerendsen 0:bbd4e22aca8a 118
peterknoben 2:47ec66bbe8f9 119 if (magnitude2 < 0){
DBerendsen 1:5681fcdc22fe 120 motor2DirectionPin = 1;
DBerendsen 0:bbd4e22aca8a 121 }
peterknoben 2:47ec66bbe8f9 122 else{
DBerendsen 1:5681fcdc22fe 123 motor2DirectionPin = 0;
DBerendsen 0:bbd4e22aca8a 124 }
DBerendsen 0:bbd4e22aca8a 125 }
DBerendsen 0:bbd4e22aca8a 126
peterknoben 3:1514725c8cc8 127 /*void servo_control (){
peterknoben 3:1514725c8cc8 128 motor1.period(0.02f);
peterknoben 3:1514725c8cc8 129 motor2.period(0.02f);
peterknoben 2:47ec66bbe8f9 130 if (k==0){
peterknoben 2:47ec66bbe8f9 131 MyServo = 0;
peterknoben 2:47ec66bbe8f9 132 k=1;
peterknoben 3:1514725c8cc8 133 motor1.period_us(200);
peterknoben 2:47ec66bbe8f9 134 }
peterknoben 2:47ec66bbe8f9 135 else{
peterknoben 2:47ec66bbe8f9 136 MyServo = 2;
peterknoben 2:47ec66bbe8f9 137 k=0;
peterknoben 3:1514725c8cc8 138 motor1.period_us(200);
peterknoben 3:1514725c8cc8 139 }
peterknoben 3:1514725c8cc8 140 }*/
DBerendsen 0:bbd4e22aca8a 141
peterknoben 2:47ec66bbe8f9 142
peterknoben 2:47ec66bbe8f9 143 int main(){
peterknoben 3:1514725c8cc8 144 But1.rise(&servo_control);
peterknoben 3:1514725c8cc8 145 motor1.period(0.0002f);
peterknoben 3:1514725c8cc8 146 motor2.period(0.0002f);
peterknoben 2:47ec66bbe8f9 147 MyControllerTicker1.attach(&motor1_control, CONTROLLER_TS);
peterknoben 2:47ec66bbe8f9 148 MyControllerTicker2.attach(&motor2_control, CONTROLLER_TS);
peterknoben 3:1514725c8cc8 149
peterknoben 2:47ec66bbe8f9 150
peterknoben 2:47ec66bbe8f9 151 while(1) {}
peterknoben 3:1514725c8cc8 152 }