M9 Biorobotics Group 8 / Mbed 2 deprecated motorcalibration_StateMasjien

Moet dit er bij

Dependencies:   mbed QEI HIDScope biquadFilter MODSERIAL FastPWM

main.cpp@0:335646ab45c0, 2019-10-28 (annotated)

Committer:
BasB
Date:
Mon Oct 28 15:50:03 2019 +0000
Revision:
0:335646ab45c0
Child:
1:a76fd17e18b3
Dit is het beginpunt. Voor we gaan rommelen

Who changed what in which revision?

UserRevisionLine numberNew contents of line
BasB 0:335646ab45c0 1 #include "mbed.h"
BasB 0:335646ab45c0 2 #include "HIDScope.h"
BasB 0:335646ab45c0 3 #include "QEI.h"
BasB 0:335646ab45c0 4 #include "MODSERIAL.h"
BasB 0:335646ab45c0 5 #include "BiQuad.h"
BasB 0:335646ab45c0 6 #include "FastPWM.h"
BasB 0:335646ab45c0 7
BasB 0:335646ab45c0 8 // Button and potmeter1 control
BasB 0:335646ab45c0 9 InterruptIn button1(D11);
BasB 0:335646ab45c0 10 InterruptIn button2(D10);
BasB 0:335646ab45c0 11 InterruptIn buttonsw2(SW2);
BasB 0:335646ab45c0 12 InterruptIn buttonsw3(SW3);
BasB 0:335646ab45c0 13 AnalogIn potmeter1(A0);
BasB 0:335646ab45c0 14 AnalogIn potmeter2(A1);
BasB 0:335646ab45c0 15 AnalogIn potmeter3(A2);
BasB 0:335646ab45c0 16 AnalogIn potmeter4(A3);
BasB 0:335646ab45c0 17 // Encoder
BasB 0:335646ab45c0 18 DigitalIn encA1(D9);
BasB 0:335646ab45c0 19 DigitalIn encB1(D8);
BasB 0:335646ab45c0 20 DigitalIn encA2(D13);
BasB 0:335646ab45c0 21 DigitalIn encB2(D13);
BasB 0:335646ab45c0 22 QEI encoder1(D9,D8,NC,64,QEI::X4_ENCODING); //Encoding motor 1
BasB 0:335646ab45c0 23 QEI encoder2(D13,D12,NC,64,QEI::X4_ENCODING); //Encoding motor 2
BasB 0:335646ab45c0 24 float Ts = 0.01; //Sample time
BasB 0:335646ab45c0 25 float motor1angle; //Measured angle motor 1
BasB 0:335646ab45c0 26 float motor2angle; //Measured angle motor 2
BasB 0:335646ab45c0 27 float omega1; //velocity rad/s motor 1
BasB 0:335646ab45c0 28 float omega2; //Velocity rad/s motor2
BasB 0:335646ab45c0 29 float deg2rad=0.0174532; //Conversion factor degree to rad
BasB 0:335646ab45c0 30 float rad2deg=57.29578; //Conversion factor rad to degree
BasB 0:335646ab45c0 31
BasB 0:335646ab45c0 32
BasB 0:335646ab45c0 33 // Motor
BasB 0:335646ab45c0 34 DigitalOut motor2Direction(D4);
BasB 0:335646ab45c0 35 FastPWM motor2Power(D5);
BasB 0:335646ab45c0 36 DigitalOut motor1Direction(D7);
BasB 0:335646ab45c0 37 FastPWM motor1Power(D6);
BasB 0:335646ab45c0 38
BasB 0:335646ab45c0 39 volatile int motortoggle = 1; //Toggle to stop motors
BasB 0:335646ab45c0 40
BasB 0:335646ab45c0 41 //Motorcontrol
BasB 0:335646ab45c0 42 bool motordir1;
BasB 0:335646ab45c0 43 bool motordir2;
BasB 0:335646ab45c0 44 float motor1ref=0.1745;
BasB 0:335646ab45c0 45 float motor2ref=0.0873;
BasB 0:335646ab45c0 46 double controlsignal1;
BasB 0:335646ab45c0 47 double controlsignal2;
BasB 0:335646ab45c0 48 double pi2= 6.283185;
BasB 0:335646ab45c0 49 float motor1error; //motor 1 error
BasB 0:335646ab45c0 50 float motor2error;
BasB 0:335646ab45c0 51 float Kp=0.27;
BasB 0:335646ab45c0 52 float Ki=0.35;
BasB 0:335646ab45c0 53 float Kd=0.1;
BasB 0:335646ab45c0 54 float u_p1;
BasB 0:335646ab45c0 55 float u_p2;
BasB 0:335646ab45c0 56 float u_i1;
BasB 0:335646ab45c0 57 float u_i2;
BasB 0:335646ab45c0 58
BasB 0:335646ab45c0 59 //Windup control
BasB 0:335646ab45c0 60 float ux1;
BasB 0:335646ab45c0 61 float ux2;
BasB 0:335646ab45c0 62 float up1; //Proportional contribution motor 1
BasB 0:335646ab45c0 63 float up2; //Proportional contribution motor 2
BasB 0:335646ab45c0 64 float ek1;
BasB 0:335646ab45c0 65 float ek2;
BasB 0:335646ab45c0 66 float ei1= 0.0; //Error integral motor 1
BasB 0:335646ab45c0 67 float ei2=0.0; //Error integral motor 2
BasB 0:335646ab45c0 68 float Ka= 1.0; //Integral windup gain
BasB 0:335646ab45c0 69
BasB 0:335646ab45c0 70 //RKI
BasB 0:335646ab45c0 71 float Vx=0.0; //Desired linear velocity x direction
BasB 0:335646ab45c0 72 float Vy=0.0; //Desired linear velocity y direction
BasB 0:335646ab45c0 73 float q1=0.0f*deg2rad; //Angle of first joint [rad]
BasB 0:335646ab45c0 74 float q2=-135.0f*deg2rad; //Angle of second joint [rad]
BasB 0:335646ab45c0 75 float q1dot; //Velocity of first joint [rad/s]
BasB 0:335646ab45c0 76 float q2dot; //Velocity of second joint [rad/s]
BasB 0:335646ab45c0 77 float l1=26.0; //Distance base-link [cm]
BasB 0:335646ab45c0 78 float l2=62.0; //Distance link-endpoint [cm]
BasB 0:335646ab45c0 79 float xe; //Endpoint x position [cm]
BasB 0:335646ab45c0 80 float ye; //Endpoint y position [cm]
BasB 0:335646ab45c0 81
BasB 0:335646ab45c0 82 //Hidscope
BasB 0:335646ab45c0 83 HIDScope scope(6); //Going to send x channels of data. To access data go to 'http:/localhost:18082/' after starting HIDScope application.
BasB 0:335646ab45c0 84
BasB 0:335646ab45c0 85 // PC connection
BasB 0:335646ab45c0 86 MODSERIAL pc(USBTX, USBRX);
BasB 0:335646ab45c0 87
BasB 0:335646ab45c0 88 // Intializing tickers
BasB 0:335646ab45c0 89 Ticker motorTicker;
BasB 0:335646ab45c0 90 Ticker controlTicker;
BasB 0:335646ab45c0 91 Ticker directionTicker;
BasB 0:335646ab45c0 92 Ticker encoderTicker;
BasB 0:335646ab45c0 93 Ticker scopeTicker;
BasB 0:335646ab45c0 94
BasB 0:335646ab45c0 95 const float PWM_period = 1e-6;
BasB 0:335646ab45c0 96
BasB 0:335646ab45c0 97 volatile int counts1; // Encoder counts
BasB 0:335646ab45c0 98 volatile int counts2;
BasB 0:335646ab45c0 99 volatile int countsPrev1 = 0;
BasB 0:335646ab45c0 100 volatile int countsPrev2 = 0;
BasB 0:335646ab45c0 101 volatile int deltaCounts1;
BasB 0:335646ab45c0 102 volatile int deltaCounts2;
BasB 0:335646ab45c0 103
BasB 0:335646ab45c0 104 float factorin = 6.23185/64; // Convert encoder counts to angle in rad
BasB 0:335646ab45c0 105 float gearratio = 131.25; // Gear ratio of gearbox
BasB 0:335646ab45c0 106
BasB 0:335646ab45c0 107
BasB 0:335646ab45c0 108 float PID_controller1(float motor1error){
BasB 0:335646ab45c0 109 static float error_integral1=0;
BasB 0:335646ab45c0 110 static float e_prev1=motor1error;
BasB 0:335646ab45c0 111
BasB 0:335646ab45c0 112 //Proportional part:
BasB 0:335646ab45c0 113 u_p1=Kp*motor1error;
BasB 0:335646ab45c0 114
BasB 0:335646ab45c0 115 //Integral part
BasB 0:335646ab45c0 116 error_integral1=error_integral1+ei1*Ts;
BasB 0:335646ab45c0 117 u_i1=Ki*error_integral1;
BasB 0:335646ab45c0 118
BasB 0:335646ab45c0 119 //Derivative part
BasB 0:335646ab45c0 120 float error_derivative1=(motor1error-e_prev1)/Ts;
BasB 0:335646ab45c0 121 float u_d1=Kd*error_derivative1;
BasB 0:335646ab45c0 122 e_prev1=motor1error;
BasB 0:335646ab45c0 123
BasB 0:335646ab45c0 124 // Sum and limit
BasB 0:335646ab45c0 125 up1= u_p1+u_i1+u_d1;
BasB 0:335646ab45c0 126 if (up1>1){
BasB 0:335646ab45c0 127 controlsignal1=1;}
BasB 0:335646ab45c0 128 else if (up1<-1){
BasB 0:335646ab45c0 129 controlsignal1=-1;}
BasB 0:335646ab45c0 130 else {
BasB 0:335646ab45c0 131 controlsignal1=up1;}
BasB 0:335646ab45c0 132
BasB 0:335646ab45c0 133 // To prevent windup
BasB 0:335646ab45c0 134 ux1= up1-controlsignal1;
BasB 0:335646ab45c0 135 ek1= Ka*ux1;
BasB 0:335646ab45c0 136 ei1= motor1error-ek1;
BasB 0:335646ab45c0 137 //Return
BasB 0:335646ab45c0 138 return controlsignal1;
BasB 0:335646ab45c0 139 }
BasB 0:335646ab45c0 140
BasB 0:335646ab45c0 141 float PID_controller2(float motor2error){
BasB 0:335646ab45c0 142 static float error_integral2=0;
BasB 0:335646ab45c0 143 static float e_prev2=motor2error;
BasB 0:335646ab45c0 144
BasB 0:335646ab45c0 145 //Proportional part:
BasB 0:335646ab45c0 146 u_p2=Kp*motor2error;
BasB 0:335646ab45c0 147
BasB 0:335646ab45c0 148 //Integral part
BasB 0:335646ab45c0 149 error_integral2=error_integral2+ei2*Ts;
BasB 0:335646ab45c0 150 u_i2=Ki*error_integral2;
BasB 0:335646ab45c0 151
BasB 0:335646ab45c0 152 //Derivative part
BasB 0:335646ab45c0 153 float error_derivative2=(motor2error-e_prev2)/Ts;
BasB 0:335646ab45c0 154 float u_d2=Kd*error_derivative2;
BasB 0:335646ab45c0 155 e_prev2=motor2error;
BasB 0:335646ab45c0 156
BasB 0:335646ab45c0 157 // Sum and limit
BasB 0:335646ab45c0 158 up2= u_p2+u_i2+u_d2;
BasB 0:335646ab45c0 159 if (up2>1){
BasB 0:335646ab45c0 160 controlsignal2=1;}
BasB 0:335646ab45c0 161 else if (up2<-1){
BasB 0:335646ab45c0 162 controlsignal2=-1;}
BasB 0:335646ab45c0 163 else {
BasB 0:335646ab45c0 164 controlsignal2=up2;}
BasB 0:335646ab45c0 165
BasB 0:335646ab45c0 166 // To prevent windup
BasB 0:335646ab45c0 167 ux2= up2-controlsignal2;
BasB 0:335646ab45c0 168 ek2= Ka*ux2;
BasB 0:335646ab45c0 169 ei2= motor2error-ek2;
BasB 0:335646ab45c0 170 //Return
BasB 0:335646ab45c0 171 return controlsignal2;
BasB 0:335646ab45c0 172 }
BasB 0:335646ab45c0 173
BasB 0:335646ab45c0 174
BasB 0:335646ab45c0 175 void readEncoder()
BasB 0:335646ab45c0 176 {
BasB 0:335646ab45c0 177 counts1 = encoder1.getPulses();
BasB 0:335646ab45c0 178 deltaCounts1 = counts1 - countsPrev1;
BasB 0:335646ab45c0 179 countsPrev1 = counts1;
BasB 0:335646ab45c0 180
BasB 0:335646ab45c0 181 counts2 = encoder2.getPulses();
BasB 0:335646ab45c0 182 deltaCounts2 = counts2 - countsPrev2;
BasB 0:335646ab45c0 183 countsPrev2 = counts2;
BasB 0:335646ab45c0 184 }
BasB 0:335646ab45c0 185
BasB 0:335646ab45c0 186 void togglehoek(){
BasB 0:335646ab45c0 187 // static float t=0;
BasB 0:335646ab45c0 188 // Vy=6.0f*sin(50.0f*t);
BasB 0:335646ab45c0 189 // t+=0.01f;
BasB 0:335646ab45c0 190 }
BasB 0:335646ab45c0 191
BasB 0:335646ab45c0 192 void RKI(){
BasB 0:335646ab45c0 193
BasB 0:335646ab45c0 194 //Vy=potmeter1.read()*10.0*motortoggle;
BasB 0:335646ab45c0 195 //Vy=potmeter2.read()*10*motortoggle;
BasB 0:335646ab45c0 196 static float t=0;
BasB 0:335646ab45c0 197 Vx=6.0f*sin(1.0f*t);
BasB 0:335646ab45c0 198 t+=0.01f;
BasB 0:335646ab45c0 199 //Vx=-1.0*;
BasB 0:335646ab45c0 200 Vy=0.0f;
BasB 0:335646ab45c0 201 q1dot=(l2*cos(q1+q2)*Vx+l2*sin(q1+q2)*Vy)/((-l1*sin(q1)-l2*sin(q1+q2))*l2*cos(q1+q2)+l2*sin(q1+q2)*(l1*cos(q1)+l2*cos(q1+q2)));
BasB 0:335646ab45c0 202 q2dot=((-l1*cos(q1)-l2*cos(q1+q2))*Vx+(-l1*sin(q1)-l2*sin(q1+q2))*Vy)/((-l1*sin(q1)-l2*sin(q1+q2))*l2*cos(q1+q2)+l2*sin(q1+q2)*(l1*cos(q1)+l2*cos(q1+q2)));
BasB 0:335646ab45c0 203 q1=q1+q1dot*Ts;
BasB 0:335646ab45c0 204 q2=q2+q2dot*Ts;
BasB 0:335646ab45c0 205
BasB 0:335646ab45c0 206 xe=l1*cos(q1)+l2*cos(q1+q2);
BasB 0:335646ab45c0 207 ye=l1*sin(q1)+l2*sin(q1+q2);
BasB 0:335646ab45c0 208
BasB 0:335646ab45c0 209
BasB 0:335646ab45c0 210
BasB 0:335646ab45c0 211 if (q1<0.0f){
BasB 0:335646ab45c0 212 q1=0.0;}
BasB 0:335646ab45c0 213 else if (q1>90.0f*deg2rad){
BasB 0:335646ab45c0 214 q1=90.0f*deg2rad;}
BasB 0:335646ab45c0 215 else{
BasB 0:335646ab45c0 216 q1=q1;}
BasB 0:335646ab45c0 217
BasB 0:335646ab45c0 218 if (q2>-45.0*deg2rad){
BasB 0:335646ab45c0 219 q2=-45.0*deg2rad;}
BasB 0:335646ab45c0 220 else if (q2<-135.0*deg2rad){
BasB 0:335646ab45c0 221 q2=-135.0*deg2rad;}
BasB 0:335646ab45c0 222 else{
BasB 0:335646ab45c0 223 q2=q2;}
BasB 0:335646ab45c0 224
BasB 0:335646ab45c0 225 motor1ref=5.5f*q1+5.5f*q2;
BasB 0:335646ab45c0 226 motor2ref=2.75f*q1;
BasB 0:335646ab45c0 227 }
BasB 0:335646ab45c0 228
BasB 0:335646ab45c0 229 void motorControl()
BasB 0:335646ab45c0 230 {
BasB 0:335646ab45c0 231 button1.fall(&togglehoek);
BasB 0:335646ab45c0 232 RKI();
BasB 0:335646ab45c0 233 motor1angle = (counts1 * factorin / gearratio)-(135.0*5.5*deg2rad); // Angle of motor shaft in rad
BasB 0:335646ab45c0 234 omega1 = deltaCounts1 / Ts * factorin / gearratio; // Angular velocity of motor shaft in rad/s
BasB 0:335646ab45c0 235 motor1error=motor1ref-motor1angle;
BasB 0:335646ab45c0 236 controlsignal1=PID_controller1(motor1error);
BasB 0:335646ab45c0 237 if (controlsignal1<0){
BasB 0:335646ab45c0 238 motordir1= 0;}
BasB 0:335646ab45c0 239 else {
BasB 0:335646ab45c0 240 motordir1= 1;}
BasB 0:335646ab45c0 241 motor1Power.write(abs(controlsignal1*motortoggle));
BasB 0:335646ab45c0 242 motor1Direction= motordir1;
BasB 0:335646ab45c0 243
BasB 0:335646ab45c0 244 motor2angle = (counts2 * factorin / gearratio); // Angle of motor shaft in rad
BasB 0:335646ab45c0 245 omega2 = deltaCounts2 / Ts * factorin / gearratio; // Angular velocity of motor shaft in rad/s
BasB 0:335646ab45c0 246 motor2error=motor2ref-motor2angle;
BasB 0:335646ab45c0 247 controlsignal2=PID_controller2(motor2error);
BasB 0:335646ab45c0 248 if (controlsignal2<0){
BasB 0:335646ab45c0 249 motordir2= 0;}
BasB 0:335646ab45c0 250 else {
BasB 0:335646ab45c0 251 motordir2= 1;}
BasB 0:335646ab45c0 252 motor2Power.write(abs(controlsignal2*motortoggle));
BasB 0:335646ab45c0 253 motor2Direction= motordir2;
BasB 0:335646ab45c0 254 }
BasB 0:335646ab45c0 255
BasB 0:335646ab45c0 256 void Plotje(){
BasB 0:335646ab45c0 257 scope.set(0,q1*rad2deg); //gewenste hoek
BasB 0:335646ab45c0 258 scope.set(1,motor1angle/5.5f*rad2deg); //Gemeten hoek
BasB 0:335646ab45c0 259 scope.set(2,motor1error/5.5f*rad2deg); //verschil in gewenste en gemeten hoek
BasB 0:335646ab45c0 260 scope.set(3,q2*rad2deg); //gewenste hoek
BasB 0:335646ab45c0 261 scope.set(4,motor2angle/5.5f*rad2deg); //Gemeten hoek
BasB 0:335646ab45c0 262 scope.set(5,motor2error/5.5f*rad2deg); //verschil in gewenste en gemeten hoek
BasB 0:335646ab45c0 263
BasB 0:335646ab45c0 264 scope.send(); //send what's in scope memory to PC
BasB 0:335646ab45c0 265 }
BasB 0:335646ab45c0 266
BasB 0:335646ab45c0 267 void toggleMotor()
BasB 0:335646ab45c0 268 {
BasB 0:335646ab45c0 269 motortoggle = !motortoggle;
BasB 0:335646ab45c0 270 }
BasB 0:335646ab45c0 271
BasB 0:335646ab45c0 272 int main()
BasB 0:335646ab45c0 273 {
BasB 0:335646ab45c0 274 pc.baud(115200);
BasB 0:335646ab45c0 275 pc.printf("\r\nStarting...\r\n\r\n");
BasB 0:335646ab45c0 276
BasB 0:335646ab45c0 277 motor1Power.period(PWM_period);
BasB 0:335646ab45c0 278 motor2Power.period(PWM_period);
BasB 0:335646ab45c0 279 motorTicker.attach(motorControl, 0.01);
BasB 0:335646ab45c0 280 scopeTicker.attach(Plotje, 0.01);
BasB 0:335646ab45c0 281 encoderTicker.attach(readEncoder, Ts);
BasB 0:335646ab45c0 282
BasB 0:335646ab45c0 283 button2.fall(&toggleMotor);
BasB 0:335646ab45c0 284
BasB 0:335646ab45c0 285 while (true) {
BasB 0:335646ab45c0 286 // pc.printf("Angle1: %f Omega1: %f\r\n", motor1angle, omega1);
BasB 0:335646ab45c0 287 // pc.printf("refangle1: %f Error1: %f \r\n",motor1ref, motor1error);
BasB 0:335646ab45c0 288 // pc.printf("Angle2: %f Omega2: %f\r\n", motor2angle, omega2);
BasB 0:335646ab45c0 289 // pc.printf("refangle2: %f Error2: %f \r\n",motor2ref, motor2error);
BasB 0:335646ab45c0 290 // pc.printf("controlsignal2: %d \r\n", controlsignal2);
BasB 0:335646ab45c0 291 // pc.printf("Vx: %f Vy: %f \r\n",Vx,Vy);
BasB 0:335646ab45c0 292 // pc.printf("q1dot: %f q2dot: %f \r\n",q1dot,q2dot);
BasB 0:335646ab45c0 293 pc.printf("q1: %f q1dot: %f motor1ref: %f motor1angle: %f\r\n", q1*rad2deg, q1dot*rad2deg, motor1ref*rad2deg, motor1angle*rad2deg);
BasB 0:335646ab45c0 294 pc.printf("q2: %f q2dot: %f motor2ref: %f motor2angle: %f\r\n", q2*rad2deg, q2dot*rad2deg, motor2ref*rad2deg, motor2angle*rad2deg);
BasB 0:335646ab45c0 295 pc.printf("X: %f Y: %f\r\n",xe,ye);
BasB 0:335646ab45c0 296 wait(0.5);
BasB 0:335646ab45c0 297 }
BasB 0:335646ab45c0 298 }