Dustin Berendsen / Mbed 2 deprecated kinematics_control_v2

Working before header-files

Dependencies:   Encoder FastPWM MODSERIAL Servo mbed

Fork of kinematics_control by Dustin Berendsen

main.cpp@2:7e5ca3715fb6, 2017-10-26 (annotated)

Committer:
DBerendsen
Date:
Thu Oct 26 08:19:53 2017 +0000
Revision:
2:7e5ca3715fb6
Parent:
1:570c0d599b9e 
Complete before header-files

Who changed what in which revision?

UserRevisionLine numberNew contents of line
DBerendsen 0:5f4bc2d63814 1 #include "mbed.h"
DBerendsen 0:5f4bc2d63814 2 #include "encoder.h"
DBerendsen 0:5f4bc2d63814 3 #include "Servo.h"
DBerendsen 0:5f4bc2d63814 4 #include "MODSERIAL.h"
DBerendsen 2:7e5ca3715fb6 5 #include "FastPWM.h"
DBerendsen 0:5f4bc2d63814 6
DBerendsen 0:5f4bc2d63814 7
DBerendsen 0:5f4bc2d63814 8
DBerendsen 0:5f4bc2d63814 9
DBerendsen 0:5f4bc2d63814 10 //------------------------------------------------------------------------------
DBerendsen 0:5f4bc2d63814 11 //------------------------------------------------------------------------------
DBerendsen 0:5f4bc2d63814 12 //------------------------------------------------------------------------------
DBerendsen 0:5f4bc2d63814 13 MODSERIAL pc(USBTX, USBRX);
DBerendsen 0:5f4bc2d63814 14 Ticker MyControllerTicker1;
DBerendsen 0:5f4bc2d63814 15 Ticker MyControllerTicker2;
DBerendsen 0:5f4bc2d63814 16 const double RAD_PER_PULSE = 0.00074799825*2;
DBerendsen 2:7e5ca3715fb6 17 const float CONTROLLER_TS = 0.02;
DBerendsen 0:5f4bc2d63814 18 const double PI = 3.14159265358979323846;
DBerendsen 0:5f4bc2d63814 19 //------------------------------------------------------------------------------
DBerendsen 0:5f4bc2d63814 20 //------------------------------------------------------------------------------
DBerendsen 0:5f4bc2d63814 21 //------------------------------------------------------------------------------
DBerendsen 0:5f4bc2d63814 22
DBerendsen 0:5f4bc2d63814 23
DBerendsen 0:5f4bc2d63814 24
DBerendsen 0:5f4bc2d63814 25
DBerendsen 0:5f4bc2d63814 26 //------------------------------------------------------------------------------
DBerendsen 0:5f4bc2d63814 27 //--------------------------------Servo-----------------------------------------
DBerendsen 0:5f4bc2d63814 28 //------------------------------------------------------------------------------
DBerendsen 0:5f4bc2d63814 29 Servo MyServo(D9);
DBerendsen 0:5f4bc2d63814 30 InterruptIn But1(D8);
DBerendsen 0:5f4bc2d63814 31 int k=0;
DBerendsen 0:5f4bc2d63814 32
DBerendsen 0:5f4bc2d63814 33 void servo_control (){
DBerendsen 0:5f4bc2d63814 34 if (k==0){
DBerendsen 0:5f4bc2d63814 35 MyServo = 0;
DBerendsen 0:5f4bc2d63814 36 k=1;
DBerendsen 0:5f4bc2d63814 37 }
DBerendsen 0:5f4bc2d63814 38 else{
DBerendsen 0:5f4bc2d63814 39 MyServo = 2;
DBerendsen 0:5f4bc2d63814 40 k=0;
DBerendsen 0:5f4bc2d63814 41 }
DBerendsen 0:5f4bc2d63814 42 }
DBerendsen 0:5f4bc2d63814 43 //------------------------------------------------------------------------------
DBerendsen 0:5f4bc2d63814 44 //------------------------------------------------------------------------------
DBerendsen 0:5f4bc2d63814 45 //------------------------------------------------------------------------------
DBerendsen 0:5f4bc2d63814 46
DBerendsen 0:5f4bc2d63814 47
DBerendsen 0:5f4bc2d63814 48
DBerendsen 0:5f4bc2d63814 49
DBerendsen 0:5f4bc2d63814 50
DBerendsen 0:5f4bc2d63814 51 //------------------------------------------------------------------------------
DBerendsen 0:5f4bc2d63814 52 //-------------------------------Kinematics-------------------------------------
DBerendsen 0:5f4bc2d63814 53 //------------------------------------------------------------------------------
DBerendsen 0:5f4bc2d63814 54 AnalogIn potmeter1(A3);
DBerendsen 0:5f4bc2d63814 55 AnalogIn potmeter2(A4);
DBerendsen 1:570c0d599b9e 56 const float max_rangex = 500.0;
DBerendsen 0:5f4bc2d63814 57 const float max_rangey = 300.0;
DBerendsen 0:5f4bc2d63814 58 const float x_offset = 156.15;
DBerendsen 0:5f4bc2d63814 59 const float y_offset = -76.97;
DBerendsen 0:5f4bc2d63814 60 const float alpha_offset = -(21.52/180)*PI;
DBerendsen 0:5f4bc2d63814 61 const float beta_offset = 0.0;
DBerendsen 0:5f4bc2d63814 62 const float L1 = 450.0;
DBerendsen 0:5f4bc2d63814 63 const float L2 = 490.0;
DBerendsen 0:5f4bc2d63814 64
DBerendsen 0:5f4bc2d63814 65 float gettargetposition(double potmeter, int max_range){
DBerendsen 0:5f4bc2d63814 66 float target = potmeter * max_range;
DBerendsen 0:5f4bc2d63814 67 return target;
DBerendsen 0:5f4bc2d63814 68 }
DBerendsen 0:5f4bc2d63814 69
DBerendsen 0:5f4bc2d63814 70 float getreferenceposition(float target, float offset) {
DBerendsen 0:5f4bc2d63814 71 float referenceposition = target + offset;
DBerendsen 0:5f4bc2d63814 72 return referenceposition;
DBerendsen 0:5f4bc2d63814 73 }
DBerendsen 0:5f4bc2d63814 74
DBerendsen 0:5f4bc2d63814 75 float getreferencealpha(float max_rangex, float maxrangey, float x_offset, float y_offset, float alpha_offset, float L1, float L2) {
DBerendsen 0:5f4bc2d63814 76 float x_target = gettargetposition(potmeter1, max_rangex);
DBerendsen 0:5f4bc2d63814 77 float y_target = gettargetposition(potmeter2, max_rangey);
DBerendsen 0:5f4bc2d63814 78 float x = getreferenceposition(x_target, x_offset);
DBerendsen 0:5f4bc2d63814 79 float y = getreferenceposition(y_target, y_offset);
DBerendsen 0:5f4bc2d63814 80 float alpha_ = (0.5*PI) - atan(y/x) - acos( ( (L1*L1) - (L2*L2) + (x*x) + (y*y) ) / (2*L1*sqrt( (x*x) +(y*y) ) ));
DBerendsen 0:5f4bc2d63814 81 float alpha = alpha_ + alpha_offset;
DBerendsen 0:5f4bc2d63814 82 return alpha;
DBerendsen 0:5f4bc2d63814 83 }
DBerendsen 0:5f4bc2d63814 84
DBerendsen 0:5f4bc2d63814 85 float getreferencebeta(float max_rangex, float maxrangey, float x_offset, float y_offset, float beta_offset, float L1, float L2) {
DBerendsen 0:5f4bc2d63814 86 float x_target = gettargetposition(potmeter1, max_rangex);
DBerendsen 0:5f4bc2d63814 87 float y_target = gettargetposition(potmeter2, max_rangey);
DBerendsen 0:5f4bc2d63814 88 float x = getreferenceposition(x_target, x_offset);
DBerendsen 0:5f4bc2d63814 89 float y = getreferenceposition(y_target, y_offset);
DBerendsen 0:5f4bc2d63814 90 float alpha_ = (0.5*PI) - atan(y/x) - acos( ( (L1*L1) - (L2*L2) + (x*x) + (y*y) ) / (2*L1*sqrt( (x*x) +(y*y) ) ));
DBerendsen 0:5f4bc2d63814 91 float beta = acos( ((L1*L1) + (L2*L2) - (x*x) - (y*y)) / (2*L1*L2)) - alpha_ + beta_offset;
DBerendsen 0:5f4bc2d63814 92 return beta;
DBerendsen 0:5f4bc2d63814 93 }
DBerendsen 0:5f4bc2d63814 94 //------------------------------------------------------------------------------
DBerendsen 0:5f4bc2d63814 95 //------------------------------------------------------------------------------
DBerendsen 0:5f4bc2d63814 96 //------------------------------------------------------------------------------
DBerendsen 0:5f4bc2d63814 97
DBerendsen 0:5f4bc2d63814 98
DBerendsen 0:5f4bc2d63814 99
DBerendsen 0:5f4bc2d63814 100
DBerendsen 0:5f4bc2d63814 101
DBerendsen 0:5f4bc2d63814 102 //------------------------------------------------------------------------------
DBerendsen 0:5f4bc2d63814 103 //------------------------------PI_Controller-----------------------------------
DBerendsen 0:5f4bc2d63814 104 //------------------------------------------------------------------------------
DBerendsen 0:5f4bc2d63814 105 float PI_controller(float error, const float Kp, const float Ki, float Ts, double &e_int) {
DBerendsen 0:5f4bc2d63814 106 e_int += Ts * error;
DBerendsen 0:5f4bc2d63814 107 return Kp * error + Ki * e_int ;
DBerendsen 0:5f4bc2d63814 108 }
DBerendsen 0:5f4bc2d63814 109 //------------------------------------------------------------------------------
DBerendsen 0:5f4bc2d63814 110 //------------------------------------------------------------------------------
DBerendsen 0:5f4bc2d63814 111 //------------------------------------------------------------------------------
DBerendsen 0:5f4bc2d63814 112
DBerendsen 0:5f4bc2d63814 113
DBerendsen 0:5f4bc2d63814 114
DBerendsen 0:5f4bc2d63814 115
DBerendsen 0:5f4bc2d63814 116
DBerendsen 0:5f4bc2d63814 117 //------------------------------------------------------------------------------
DBerendsen 2:7e5ca3715fb6 118 //------------------------------PID_Controller-----------------------------------
DBerendsen 2:7e5ca3715fb6 119 //------------------------------------------------------------------------------
DBerendsen 2:7e5ca3715fb6 120
DBerendsen 2:7e5ca3715fb6 121 double PID(float error, const float Kp, const float Ki, const float Kd, const float Ts, const float N, double &v1, double &v2) {
DBerendsen 2:7e5ca3715fb6 122 const double a1 = -4 / (N*Ts+2);
DBerendsen 2:7e5ca3715fb6 123 const double a2 = -(N*Ts-2) / ( N*Ts+2);
DBerendsen 2:7e5ca3715fb6 124 const double b0 = (4*Kp + 4*Kd*N +2*Ki*Ts + 2*Kp*N*Ts + Ki*N*pow(Ts,2)) / (2*N*Ts + 4);
DBerendsen 2:7e5ca3715fb6 125 const double b1 = (Ki*N*pow(Ts,2) - 4*Kp - 4*Kd*N) / (N*Ts + 2);
DBerendsen 2:7e5ca3715fb6 126 const double b2 = (4*Kp + 4*Kd*N - 2*Ki*Ts - 2*Kp*N*Ts + Ki*N*pow(Ts,2)) / (2*N*Ts + 4);
DBerendsen 2:7e5ca3715fb6 127
DBerendsen 2:7e5ca3715fb6 128 double v = error - a1*v1 - a2*v2;
DBerendsen 2:7e5ca3715fb6 129 double u = b0*v + b1*v1 + b2*v2;
DBerendsen 2:7e5ca3715fb6 130 v2=v1;
DBerendsen 2:7e5ca3715fb6 131 v1=v;
DBerendsen 2:7e5ca3715fb6 132 return u;
DBerendsen 2:7e5ca3715fb6 133
DBerendsen 2:7e5ca3715fb6 134 }
DBerendsen 2:7e5ca3715fb6 135
DBerendsen 2:7e5ca3715fb6 136 //------------------------------------------------------------------------------
DBerendsen 2:7e5ca3715fb6 137 //------------------------------------------------------------------------------
DBerendsen 2:7e5ca3715fb6 138 //------------------------------------------------------------------------------
DBerendsen 2:7e5ca3715fb6 139
DBerendsen 2:7e5ca3715fb6 140
DBerendsen 2:7e5ca3715fb6 141
DBerendsen 2:7e5ca3715fb6 142
DBerendsen 2:7e5ca3715fb6 143
DBerendsen 2:7e5ca3715fb6 144 //------------------------------------------------------------------------------
DBerendsen 0:5f4bc2d63814 145 //--------------------------------Motor1----------------------------------------
DBerendsen 0:5f4bc2d63814 146 //------------------------------------------------------------------------------
DBerendsen 2:7e5ca3715fb6 147 FastPWM motor1(D5);
DBerendsen 0:5f4bc2d63814 148 DigitalOut motor1DirectionPin(D4);
DBerendsen 0:5f4bc2d63814 149 DigitalIn ENC2A(D12);
DBerendsen 0:5f4bc2d63814 150 DigitalIn ENC2B(D13);
DBerendsen 0:5f4bc2d63814 151 Encoder encoder1(D13,D12);
DBerendsen 2:7e5ca3715fb6 152 const float MOTOR1_KP = 40.0;
DBerendsen 2:7e5ca3715fb6 153 const float MOTOR1_KI = 0.0;
DBerendsen 2:7e5ca3715fb6 154 const float MOTOR1_KD = 15.0;
DBerendsen 2:7e5ca3715fb6 155 double M1_v1 = 0.0;
DBerendsen 2:7e5ca3715fb6 156 double M1_v2 = 0.0;
DBerendsen 0:5f4bc2d63814 157 const double motor1_gain = 2*PI;
DBerendsen 2:7e5ca3715fb6 158 const float M1_N = 0.5;
DBerendsen 0:5f4bc2d63814 159
DBerendsen 0:5f4bc2d63814 160
DBerendsen 0:5f4bc2d63814 161 void motor1_control(){
DBerendsen 0:5f4bc2d63814 162 float reference_alpha = getreferencebeta(max_rangex, max_rangey, x_offset, y_offset, beta_offset, L1, L2);
DBerendsen 0:5f4bc2d63814 163 float position_alpha = RAD_PER_PULSE * encoder1.getPosition();
DBerendsen 0:5f4bc2d63814 164 float error_alpha = reference_alpha-position_alpha;
DBerendsen 2:7e5ca3715fb6 165 float magnitude1 = PID(error_alpha, MOTOR1_KP, MOTOR1_KI, MOTOR1_KD, CONTROLLER_TS, M1_N, M1_v1, M1_v2) / motor1_gain;
DBerendsen 0:5f4bc2d63814 166 motor1 = fabs(magnitude1);
DBerendsen 0:5f4bc2d63814 167 pc.printf("err_a = %f alpha = %f mag1 = %f\r\n", error_alpha, reference_alpha, magnitude1);
DBerendsen 0:5f4bc2d63814 168 pc.printf("\r\n");
DBerendsen 0:5f4bc2d63814 169
DBerendsen 0:5f4bc2d63814 170
DBerendsen 1:570c0d599b9e 171 if (magnitude1 < 0){
DBerendsen 0:5f4bc2d63814 172 motor1DirectionPin = 1;
DBerendsen 0:5f4bc2d63814 173 }
DBerendsen 0:5f4bc2d63814 174 else{
DBerendsen 0:5f4bc2d63814 175 motor1DirectionPin = 0;
DBerendsen 0:5f4bc2d63814 176 }
DBerendsen 0:5f4bc2d63814 177 }
DBerendsen 0:5f4bc2d63814 178 //------------------------------------------------------------------------------
DBerendsen 0:5f4bc2d63814 179 //------------------------------------------------------------------------------
DBerendsen 0:5f4bc2d63814 180 //------------------------------------------------------------------------------
DBerendsen 0:5f4bc2d63814 181
DBerendsen 0:5f4bc2d63814 182
DBerendsen 0:5f4bc2d63814 183
DBerendsen 0:5f4bc2d63814 184
DBerendsen 0:5f4bc2d63814 185
DBerendsen 0:5f4bc2d63814 186 //------------------------------------------------------------------------------
DBerendsen 0:5f4bc2d63814 187 //--------------------------------Motor2----------------------------------------
DBerendsen 0:5f4bc2d63814 188 //------------------------------------------------------------------------------
DBerendsen 2:7e5ca3715fb6 189 FastPWM motor2(D6);
DBerendsen 0:5f4bc2d63814 190 DigitalOut motor2DirectionPin(D7);
DBerendsen 0:5f4bc2d63814 191 DigitalIn ENC1A(D10);
DBerendsen 0:5f4bc2d63814 192 DigitalIn ENC1B(D11);
DBerendsen 0:5f4bc2d63814 193 Encoder encoder2(D10,D11);
DBerendsen 2:7e5ca3715fb6 194 const float MOTOR2_KP = 60.0;
DBerendsen 2:7e5ca3715fb6 195 const float MOTOR2_KI = 0.0;
DBerendsen 2:7e5ca3715fb6 196 const float MOTOR2_KD = 15.0;
DBerendsen 0:5f4bc2d63814 197 double m2_err_int = 0;
DBerendsen 0:5f4bc2d63814 198 const double motor2_gain = 2*PI;
DBerendsen 2:7e5ca3715fb6 199 const float M2_N = 0.5;
DBerendsen 2:7e5ca3715fb6 200 double M2_v1 = 0.0;
DBerendsen 2:7e5ca3715fb6 201 double M2_v2 = 0.0;
DBerendsen 0:5f4bc2d63814 202
DBerendsen 0:5f4bc2d63814 203
DBerendsen 0:5f4bc2d63814 204 void motor2_control(){
DBerendsen 0:5f4bc2d63814 205 float reference_beta = getreferencealpha(max_rangex, max_rangey, x_offset, y_offset, alpha_offset, L1, L2);
DBerendsen 0:5f4bc2d63814 206 float position_beta = RAD_PER_PULSE * -encoder2.getPosition();
DBerendsen 0:5f4bc2d63814 207 float error_beta = reference_beta-position_beta;
DBerendsen 2:7e5ca3715fb6 208 float magnitude2 = PID(error_beta, MOTOR2_KP, MOTOR2_KI, MOTOR2_KD, CONTROLLER_TS, M2_N, M1_v1, M1_v2) / motor2_gain;
DBerendsen 0:5f4bc2d63814 209 motor2 = fabs(magnitude2);
DBerendsen 0:5f4bc2d63814 210 pc.printf("err2 = %f beta = %f mag2 = %f\r\n", error_beta, reference_beta, magnitude2);
DBerendsen 0:5f4bc2d63814 211 pc.printf("\r\n");
DBerendsen 0:5f4bc2d63814 212
DBerendsen 0:5f4bc2d63814 213 if (magnitude2 > 0){
DBerendsen 0:5f4bc2d63814 214 motor2DirectionPin = 1;
DBerendsen 0:5f4bc2d63814 215 }
DBerendsen 0:5f4bc2d63814 216 else{
DBerendsen 0:5f4bc2d63814 217 motor2DirectionPin = 0;
DBerendsen 0:5f4bc2d63814 218 }
DBerendsen 0:5f4bc2d63814 219 }
DBerendsen 0:5f4bc2d63814 220 //------------------------------------------------------------------------------
DBerendsen 0:5f4bc2d63814 221 //------------------------------------------------------------------------------
DBerendsen 0:5f4bc2d63814 222 //------------------------------------------------------------------------------
DBerendsen 0:5f4bc2d63814 223
DBerendsen 0:5f4bc2d63814 224
DBerendsen 0:5f4bc2d63814 225
DBerendsen 0:5f4bc2d63814 226
DBerendsen 0:5f4bc2d63814 227
DBerendsen 0:5f4bc2d63814 228 int main(){
DBerendsen 0:5f4bc2d63814 229 pc.baud(115200);
DBerendsen 2:7e5ca3715fb6 230 motor1.period(0.0001f);
DBerendsen 2:7e5ca3715fb6 231 motor2.period(0.0001f);
DBerendsen 0:5f4bc2d63814 232 MyControllerTicker1.attach(&motor1_control, CONTROLLER_TS);
DBerendsen 0:5f4bc2d63814 233 MyControllerTicker2.attach(&motor2_control, CONTROLLER_TS);
DBerendsen 0:5f4bc2d63814 234 But1.rise(&servo_control);
DBerendsen 0:5f4bc2d63814 235
DBerendsen 0:5f4bc2d63814 236
DBerendsen 0:5f4bc2d63814 237
DBerendsen 0:5f4bc2d63814 238
DBerendsen 0:5f4bc2d63814 239
DBerendsen 0:5f4bc2d63814 240 while(1) {}
DBerendsen 0:5f4bc2d63814 241 }