Dustin Berendsen / Mbed 2 deprecated kinematics_control

1st Revision of clean control code for project biorobotics

Dependencies:   Encoder MODSERIAL Servo mbed

main.cpp@0:5f4bc2d63814, 2017-10-19 (annotated)

Committer:
DBerendsen
Date:
Thu Oct 19 19:42:08 2017 +0000
Revision:
0:5f4bc2d63814
1st Revision of clean control code for project biorobotics

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 0:5f4bc2d63814 5
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 0:5f4bc2d63814 17 const float CONTROLLER_TS = 0.5;
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 0:5f4bc2d63814 56 const float max_rangex = 300.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 0:5f4bc2d63814 118 //--------------------------------Motor1----------------------------------------
DBerendsen 0:5f4bc2d63814 119 //------------------------------------------------------------------------------
DBerendsen 0:5f4bc2d63814 120 PwmOut motor1(D5);
DBerendsen 0:5f4bc2d63814 121 DigitalOut motor1DirectionPin(D4);
DBerendsen 0:5f4bc2d63814 122 DigitalIn ENC2A(D12);
DBerendsen 0:5f4bc2d63814 123 DigitalIn ENC2B(D13);
DBerendsen 0:5f4bc2d63814 124 Encoder encoder1(D13,D12);
DBerendsen 0:5f4bc2d63814 125 const float MOTOR1_KP = 20.0;
DBerendsen 0:5f4bc2d63814 126 const float MOTOR1_KI = 10.0;
DBerendsen 0:5f4bc2d63814 127 double m1_err_int = 0;
DBerendsen 0:5f4bc2d63814 128 const double motor1_gain = 2*PI;
DBerendsen 0:5f4bc2d63814 129
DBerendsen 0:5f4bc2d63814 130
DBerendsen 0:5f4bc2d63814 131 void motor1_control(){
DBerendsen 0:5f4bc2d63814 132 float reference_alpha = getreferencebeta(max_rangex, max_rangey, x_offset, y_offset, beta_offset, L1, L2);
DBerendsen 0:5f4bc2d63814 133 float position_alpha = RAD_PER_PULSE * encoder1.getPosition();
DBerendsen 0:5f4bc2d63814 134 float error_alpha = reference_alpha-position_alpha;
DBerendsen 0:5f4bc2d63814 135 float magnitude1 = PI_controller(error_alpha, MOTOR1_KP, MOTOR1_KI, CONTROLLER_TS, m1_err_int)/ motor1_gain;
DBerendsen 0:5f4bc2d63814 136 motor1 = fabs(magnitude1);
DBerendsen 0:5f4bc2d63814 137 pc.printf("err_a = %f alpha = %f mag1 = %f\r\n", error_alpha, reference_alpha, magnitude1);
DBerendsen 0:5f4bc2d63814 138 pc.printf("\r\n");
DBerendsen 0:5f4bc2d63814 139
DBerendsen 0:5f4bc2d63814 140
DBerendsen 0:5f4bc2d63814 141 if (magnitude1 < 0){
DBerendsen 0:5f4bc2d63814 142 motor1DirectionPin = 1;
DBerendsen 0:5f4bc2d63814 143 }
DBerendsen 0:5f4bc2d63814 144 else{
DBerendsen 0:5f4bc2d63814 145 motor1DirectionPin = 0;
DBerendsen 0:5f4bc2d63814 146 }
DBerendsen 0:5f4bc2d63814 147 }
DBerendsen 0:5f4bc2d63814 148 //------------------------------------------------------------------------------
DBerendsen 0:5f4bc2d63814 149 //------------------------------------------------------------------------------
DBerendsen 0:5f4bc2d63814 150 //------------------------------------------------------------------------------
DBerendsen 0:5f4bc2d63814 151
DBerendsen 0:5f4bc2d63814 152
DBerendsen 0:5f4bc2d63814 153
DBerendsen 0:5f4bc2d63814 154
DBerendsen 0:5f4bc2d63814 155
DBerendsen 0:5f4bc2d63814 156 //------------------------------------------------------------------------------
DBerendsen 0:5f4bc2d63814 157 //--------------------------------Motor2----------------------------------------
DBerendsen 0:5f4bc2d63814 158 //------------------------------------------------------------------------------
DBerendsen 0:5f4bc2d63814 159 PwmOut motor2(D6);
DBerendsen 0:5f4bc2d63814 160 DigitalOut motor2DirectionPin(D7);
DBerendsen 0:5f4bc2d63814 161 DigitalIn ENC1A(D10);
DBerendsen 0:5f4bc2d63814 162 DigitalIn ENC1B(D11);
DBerendsen 0:5f4bc2d63814 163 Encoder encoder2(D10,D11);
DBerendsen 0:5f4bc2d63814 164 const float MOTOR2_KP = 20.0;
DBerendsen 0:5f4bc2d63814 165 const float MOTOR2_KI = 10.0;
DBerendsen 0:5f4bc2d63814 166 double m2_err_int = 0;
DBerendsen 0:5f4bc2d63814 167 const double motor2_gain = 2*PI;
DBerendsen 0:5f4bc2d63814 168
DBerendsen 0:5f4bc2d63814 169
DBerendsen 0:5f4bc2d63814 170 void motor2_control(){
DBerendsen 0:5f4bc2d63814 171 float reference_beta = getreferencealpha(max_rangex, max_rangey, x_offset, y_offset, alpha_offset, L1, L2);
DBerendsen 0:5f4bc2d63814 172 float position_beta = RAD_PER_PULSE * -encoder2.getPosition();
DBerendsen 0:5f4bc2d63814 173 float error_beta = reference_beta-position_beta;
DBerendsen 0:5f4bc2d63814 174 float magnitude2 = PI_controller(error_beta, MOTOR2_KP, MOTOR2_KI, CONTROLLER_TS, m2_err_int)/ motor2_gain;
DBerendsen 0:5f4bc2d63814 175 motor2 = fabs(magnitude2);
DBerendsen 0:5f4bc2d63814 176 pc.printf("err2 = %f beta = %f mag2 = %f\r\n", error_beta, reference_beta, magnitude2);
DBerendsen 0:5f4bc2d63814 177 pc.printf("\r\n");
DBerendsen 0:5f4bc2d63814 178
DBerendsen 0:5f4bc2d63814 179 if (magnitude2 > 0){
DBerendsen 0:5f4bc2d63814 180 motor2DirectionPin = 1;
DBerendsen 0:5f4bc2d63814 181 }
DBerendsen 0:5f4bc2d63814 182 else{
DBerendsen 0:5f4bc2d63814 183 motor2DirectionPin = 0;
DBerendsen 0:5f4bc2d63814 184 }
DBerendsen 0:5f4bc2d63814 185 }
DBerendsen 0:5f4bc2d63814 186 //------------------------------------------------------------------------------
DBerendsen 0:5f4bc2d63814 187 //------------------------------------------------------------------------------
DBerendsen 0:5f4bc2d63814 188 //------------------------------------------------------------------------------
DBerendsen 0:5f4bc2d63814 189
DBerendsen 0:5f4bc2d63814 190
DBerendsen 0:5f4bc2d63814 191
DBerendsen 0:5f4bc2d63814 192
DBerendsen 0:5f4bc2d63814 193
DBerendsen 0:5f4bc2d63814 194 int main(){
DBerendsen 0:5f4bc2d63814 195 pc.baud(115200);
DBerendsen 0:5f4bc2d63814 196 motor1.period(0.0002f);
DBerendsen 0:5f4bc2d63814 197 motor2.period(0.0002f);
DBerendsen 0:5f4bc2d63814 198 MyControllerTicker1.attach(&motor1_control, CONTROLLER_TS);
DBerendsen 0:5f4bc2d63814 199 MyControllerTicker2.attach(&motor2_control, CONTROLLER_TS);
DBerendsen 0:5f4bc2d63814 200 But1.rise(&servo_control);
DBerendsen 0:5f4bc2d63814 201
DBerendsen 0:5f4bc2d63814 202
DBerendsen 0:5f4bc2d63814 203
DBerendsen 0:5f4bc2d63814 204
DBerendsen 0:5f4bc2d63814 205
DBerendsen 0:5f4bc2d63814 206 while(1) {}
DBerendsen 0:5f4bc2d63814 207 }