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@5:b4ec742aa7d4, 2017-10-17 (annotated)

Committer:
peterknoben
Date:
Tue Oct 17 10:09:51 2017 +0000
Revision:
5:b4ec742aa7d4
Parent:
4:d385669b2f50 
Inclusief kinematica (nog niet getest)

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 5:b4ec742aa7d4 38 //________________________________________________________________
peterknoben 5:b4ec742aa7d4 39 //Kinematica
peterknoben 5:b4ec742aa7d4 40
peterknoben 5:b4ec742aa7d4 41 //Motor offsets (kinematica implementation)
peterknoben 5:b4ec742aa7d4 42 int max_rangex = 800;
peterknoben 5:b4ec742aa7d4 43 int max_rangey = 500;
peterknoben 5:b4ec742aa7d4 44 int L1 = 450;
peterknoben 5:b4ec742aa7d4 45 int L2 = 490;
peterknoben 5:b4ec742aa7d4 46 float alphaoffset = 10;
peterknoben 5:b4ec742aa7d4 47 float betaoffset = 35;
peterknoben 5:b4ec742aa7d4 48 float x_offset = 0.0;
peterknoben 5:b4ec742aa7d4 49 float y_offset = 0.0;
peterknoben 5:b4ec742aa7d4 50
peterknoben 5:b4ec742aa7d4 51
peterknoben 5:b4ec742aa7d4 52 float getreferencepositionx(double potmeter){
peterknoben 5:b4ec742aa7d4 53 float x_target = potmeter * max_rangex;
peterknoben 5:b4ec742aa7d4 54 return x_target;
peterknoben 5:b4ec742aa7d4 55 }
peterknoben 5:b4ec742aa7d4 56 float getreferencepositiony(double potmeter){
peterknoben 5:b4ec742aa7d4 57 float y_target = potmeter * max_rangey;
peterknoben 5:b4ec742aa7d4 58 return y_target;
peterknoben 5:b4ec742aa7d4 59 }
peterknoben 5:b4ec742aa7d4 60
peterknoben 5:b4ec742aa7d4 61 float getreferenceanglealpha(const double PI){
peterknoben 5:b4ec742aa7d4 62 float x = getreferencepositionx(potmeter1);
peterknoben 5:b4ec742aa7d4 63 float y = getreferencepositiony(potmeter2);
peterknoben 5:b4ec742aa7d4 64 float theta2 = acos((x*x + y*y - L1*L1 - L2*L2)/(2*L1*L2));
peterknoben 5:b4ec742aa7d4 65 float theta1 = asin(L2*sin(theta2)/sqrt(x*x +y*y) + atan(y/x));
peterknoben 5:b4ec742aa7d4 66 float alpha = (05.*PI) - theta1;
peterknoben 5:b4ec742aa7d4 67 return alpha;
peterknoben 5:b4ec742aa7d4 68 }
peterknoben 5:b4ec742aa7d4 69
peterknoben 5:b4ec742aa7d4 70 float getreferenceanglebeta(const double PI){
peterknoben 5:b4ec742aa7d4 71 float x = getreferencepositionx(potmeter1);
peterknoben 5:b4ec742aa7d4 72 float y = getreferencepositiony(potmeter2);
peterknoben 5:b4ec742aa7d4 73 float theta2 = acos((x*x + y*y - L1*L1 - L2*L2)/(2*L1*L2));
peterknoben 5:b4ec742aa7d4 74 float theta1 = asin(L2*sin(theta2)/sqrt(x*x +y*y) + atan(y/x));
peterknoben 5:b4ec742aa7d4 75 float alpha = (05.*PI) - theta1;
peterknoben 5:b4ec742aa7d4 76 float beta = PI - alpha - theta2;
peterknoben 5:b4ec742aa7d4 77 return beta;
peterknoben 5:b4ec742aa7d4 78 }
peterknoben 5:b4ec742aa7d4 79
peterknoben 5:b4ec742aa7d4 80 //________________________________________________________________
peterknoben 5:b4ec742aa7d4 81
peterknoben 3:1514725c8cc8 82
peterknoben 2:47ec66bbe8f9 83 //Servo
peterknoben 2:47ec66bbe8f9 84 Servo MyServo(D9);
peterknoben 2:47ec66bbe8f9 85 InterruptIn But1(D8);
peterknoben 2:47ec66bbe8f9 86 int k=0;
DBerendsen 0:bbd4e22aca8a 87
peterknoben 2:47ec66bbe8f9 88 double PI_controller(double error, const double Kp, const double Ki, double Ts, double &e_int) {
DBerendsen 0:bbd4e22aca8a 89 e_int =+ Ts * error;
DBerendsen 0:bbd4e22aca8a 90 return Kp * error + Ki * e_int ;
DBerendsen 0:bbd4e22aca8a 91 }
DBerendsen 0:bbd4e22aca8a 92
peterknoben 2:47ec66bbe8f9 93 void motor1_control(){
peterknoben 5:b4ec742aa7d4 94 double referenceangle1 = getreferenceanglealpha(PI);
DBerendsen 0:bbd4e22aca8a 95 double position1 = RAD_PER_PULSE * encoder1.getPosition();
DBerendsen 0:bbd4e22aca8a 96 double magnitude1 = PI_controller(referenceangle1-position1, MOTOR1_KP, MOTOR1_KI, CONTROLLER_TS, m1_err_int) / motor1_gain;
DBerendsen 0:bbd4e22aca8a 97 motor1 = fabs(magnitude1);
DBerendsen 0:bbd4e22aca8a 98
peterknoben 2:47ec66bbe8f9 99 if (magnitude1 < 0){
DBerendsen 0:bbd4e22aca8a 100 motor1DirectionPin = 1;
DBerendsen 0:bbd4e22aca8a 101 }
peterknoben 2:47ec66bbe8f9 102 else{
DBerendsen 0:bbd4e22aca8a 103 motor1DirectionPin = 0;
DBerendsen 0:bbd4e22aca8a 104 }
DBerendsen 0:bbd4e22aca8a 105 }
DBerendsen 0:bbd4e22aca8a 106
peterknoben 2:47ec66bbe8f9 107
peterknoben 2:47ec66bbe8f9 108 void motor2_control(){
peterknoben 5:b4ec742aa7d4 109 double referenceangle2 = getreferenceanglebeta(PI);
DBerendsen 0:bbd4e22aca8a 110 double position2 = RAD_PER_PULSE * encoder2.getPosition();
DBerendsen 0:bbd4e22aca8a 111 double magnitude2 = PI_controller(referenceangle2-position2, MOTOR2_KP, MOTOR2_KI, CONTROLLER_TS, m2_err_int) / motor2_gain;
DBerendsen 1:5681fcdc22fe 112 motor2 = fabs(magnitude2);
DBerendsen 0:bbd4e22aca8a 113
peterknoben 2:47ec66bbe8f9 114 if (magnitude2 < 0){
DBerendsen 1:5681fcdc22fe 115 motor2DirectionPin = 1;
DBerendsen 0:bbd4e22aca8a 116 }
peterknoben 2:47ec66bbe8f9 117 else{
DBerendsen 1:5681fcdc22fe 118 motor2DirectionPin = 0;
DBerendsen 0:bbd4e22aca8a 119 }
DBerendsen 0:bbd4e22aca8a 120 }
DBerendsen 0:bbd4e22aca8a 121
peterknoben 5:b4ec742aa7d4 122 /*void servo_control (){
peterknoben 3:1514725c8cc8 123 motor1.period(0.02f);
peterknoben 3:1514725c8cc8 124 motor2.period(0.02f);
peterknoben 2:47ec66bbe8f9 125 if (k==0){
peterknoben 2:47ec66bbe8f9 126 MyServo = 0;
peterknoben 2:47ec66bbe8f9 127 k=1;
peterknoben 3:1514725c8cc8 128 motor1.period_us(200);
peterknoben 2:47ec66bbe8f9 129 }
peterknoben 2:47ec66bbe8f9 130 else{
peterknoben 2:47ec66bbe8f9 131 MyServo = 2;
peterknoben 2:47ec66bbe8f9 132 k=0;
peterknoben 3:1514725c8cc8 133 motor1.period_us(200);
peterknoben 3:1514725c8cc8 134 }
peterknoben 5:b4ec742aa7d4 135 }*/
DBerendsen 0:bbd4e22aca8a 136
peterknoben 2:47ec66bbe8f9 137
peterknoben 2:47ec66bbe8f9 138 int main(){
peterknoben 5:b4ec742aa7d4 139 //But1.rise(&servo_control);
peterknoben 3:1514725c8cc8 140 motor1.period(0.0002f);
peterknoben 3:1514725c8cc8 141 motor2.period(0.0002f);
peterknoben 2:47ec66bbe8f9 142 MyControllerTicker1.attach(&motor1_control, CONTROLLER_TS);
peterknoben 2:47ec66bbe8f9 143 MyControllerTicker2.attach(&motor2_control, CONTROLLER_TS);
peterknoben 3:1514725c8cc8 144
peterknoben 2:47ec66bbe8f9 145
peterknoben 2:47ec66bbe8f9 146 while(1) {}
peterknoben 3:1514725c8cc8 147 }