Dustin Berendsen / Mbed 2 deprecated Angle_control_v3

message

Dependencies:   Encoder FastPWM MODSERIAL Servo mbed

Fork of Angle_control_v3 by Peter Knoben

main.cpp@6:6ae6256cf234, 2017-10-19 (annotated)

Committer:
DBerendsen
Date:
Thu Oct 19 09:10:20 2017 +0000
Revision:
6:6ae6256cf234
Parent:
5:b4ec742aa7d4 
message

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