Feike van Veen / Mbed 2 deprecated Minor_test_serial

Robot tryout

Dependencies:   mbed QEI biquadFilter MODSERIAL FastPWM ttmath Math

Motor_tryout.cpp@18:ab0fe311e7f3, 2019-10-21 (annotated)

Committer:
Feike
Date:
Mon Oct 21 11:01:18 2019 +0000
Revision:
18:ab0fe311e7f3
Parent:
17:6da57acb7bea
Child:
19:9c8ab7922191
nieuwe versie met het aansturen van de motor met keyboard

Who changed what in which revision?

UserRevisionLine numberNew contents of line
Feike 4:bd21569250c7 1 #include "mbed.h"
Feike 8:017b813c72bb 2 #include "MODSERIAL.h"
Feike 18:ab0fe311e7f3 3 #include "QEI1.h"
Feike 18:ab0fe311e7f3 4 #include "QEI2.h"
Feike 12:2382468d36a4 5 #include "QEI.h"
Feike 18:ab0fe311e7f3 6 #include "Math.h"
Feike 18:ab0fe311e7f3 7 #include "ttmath.h"
Feike 18:ab0fe311e7f3 8
Feike 17:6da57acb7bea 9 MODSERIAL pc(USBTX, USBRX);
Feike 18:ab0fe311e7f3 10 //Serial term (USBTX, USBRX);
Feike 17:6da57acb7bea 11 PwmOut motor1_pwm(PTC2);
Feike 17:6da57acb7bea 12 DigitalOut motor1_dir(PTC3);
Feike 18:ab0fe311e7f3 13 PwmOut motor2_pwm(PTA2);
Feike 18:ab0fe311e7f3 14 DigitalOut motor2_dir(PTB23);
Feike 18:ab0fe311e7f3 15 PwmOut motor3_pwm(PTC4);
Feike 18:ab0fe311e7f3 16 DigitalOut motor3_dir(PTC12);
Feike 4:bd21569250c7 17
Feike 18:ab0fe311e7f3 18 QEI1 Encoder1(D12,D13,NC,64,QEI1::X4_ENCODING);
Feike 18:ab0fe311e7f3 19 QEI2 Encoder2(D10,D11,NC,64,QEI2::X4_ENCODING);
Feike 18:ab0fe311e7f3 20 QEI Encoder(D2,D3,NC,64,QEI::X4_ENCODING);
Feike 7:e119b12e5e7f 21
Feike 13:18dd7a15603f 22 int quit;
Feike 18:ab0fe311e7f3 23 int counts1 = 0;
Feike 18:ab0fe311e7f3 24 int counts2 = 0;
Feike 18:ab0fe311e7f3 25 int counts3 = 0;
Feike 13:18dd7a15603f 26 int limit_pos = 8400;
Feike 18:ab0fe311e7f3 27 int x;
Feike 18:ab0fe311e7f3 28 int y;
Feike 18:ab0fe311e7f3 29 int z;
Feike 18:ab0fe311e7f3 30 float steps;
Feike 18:ab0fe311e7f3 31 int g = 0;
Feike 13:18dd7a15603f 32
Feike 18:ab0fe311e7f3 33 const float le = 15.0;
Feike 18:ab0fe311e7f3 34 const float f = 37.5;
Feike 18:ab0fe311e7f3 35 const float re = 174.0;
Feike 18:ab0fe311e7f3 36 const float rf = 50.0;
Feike 18:ab0fe311e7f3 37 const float pi = 3.14159265358979323846;
Feike 18:ab0fe311e7f3 38 float y2;
Feike 18:ab0fe311e7f3 39 float y1;
Feike 18:ab0fe311e7f3 40 float z1;
Feike 18:ab0fe311e7f3 41 float z2;
Feike 18:ab0fe311e7f3 42 float rje2;
Feike 18:ab0fe311e7f3 43 float rje;
Feike 18:ab0fe311e7f3 44 float r2;
Feike 18:ab0fe311e7f3 45 float r;
Feike 18:ab0fe311e7f3 46 float z0=-172;
Feike 18:ab0fe311e7f3 47 float y0=0;
Feike 18:ab0fe311e7f3 48 float x0=0;
Feike 18:ab0fe311e7f3 49 int check;
Feike 18:ab0fe311e7f3 50 float theta1;
Feike 18:ab0fe311e7f3 51 float theta2;
Feike 18:ab0fe311e7f3 52 float theta3;
Feike 18:ab0fe311e7f3 53 float oldtheta1=0;
Feike 18:ab0fe311e7f3 54 float oldtheta2=0;
Feike 18:ab0fe311e7f3 55 float oldtheta3=0;
Feike 18:ab0fe311e7f3 56 int direction;
Feike 18:ab0fe311e7f3 57 float delta_calcangleyz(float x0, float y0, float z0) {
Feike 18:ab0fe311e7f3 58 float y2 = y0 + le;
Feike 18:ab0fe311e7f3 59 float y1 = f;
Feike 18:ab0fe311e7f3 60 float z1 = 0.0;
Feike 18:ab0fe311e7f3 61 float z2 = z0;
Feike 18:ab0fe311e7f3 62 float rje2 = re*re - x0*x0;
Feike 18:ab0fe311e7f3 63 float rje = sqrt(rje2);
Feike 18:ab0fe311e7f3 64 float r2 = (y1-y2)*(y1-y2) + (z1-z0)*(z1-z0);
Feike 18:ab0fe311e7f3 65 float r = sqrt(r2);
Feike 18:ab0fe311e7f3 66
Feike 18:ab0fe311e7f3 67
Feike 18:ab0fe311e7f3 68 if ((r+rje<rf) || (r + rf <rje) || (rf+rje<r))
Feike 18:ab0fe311e7f3 69 {
Feike 18:ab0fe311e7f3 70 int check = 1;
Feike 18:ab0fe311e7f3 71 pc.printf("\n\rPunt bestaat niet");
Feike 18:ab0fe311e7f3 72 }
Feike 18:ab0fe311e7f3 73 else
Feike 18:ab0fe311e7f3 74 {
Feike 18:ab0fe311e7f3 75 float alpha = acos((r2 + rf*rf -rje2)/(2*rf*r));
Feike 18:ab0fe311e7f3 76 float beta = atan((z1-z2)/(y1-y2));
Feike 18:ab0fe311e7f3 77
Feike 18:ab0fe311e7f3 78 if(beta<0) {
Feike 18:ab0fe311e7f3 79 beta = beta + pi;
Feike 18:ab0fe311e7f3 80 }
Feike 18:ab0fe311e7f3 81 float theta1 = (beta - alpha)*180.0/pi;
Feike 18:ab0fe311e7f3 82 int check = 0;
Feike 18:ab0fe311e7f3 83 return theta1;
Feike 18:ab0fe311e7f3 84 }
Feike 18:ab0fe311e7f3 85 }
Feike 18:ab0fe311e7f3 86
Feike 18:ab0fe311e7f3 87 float delta_calcinverse(float x0, float y0, float z0) {
Feike 18:ab0fe311e7f3 88 theta1 = theta2 = theta3 = 0;
Feike 18:ab0fe311e7f3 89 theta1 = delta_calcangleyz(x0, y0, z0);
Feike 18:ab0fe311e7f3 90 if (check == 0)
Feike 18:ab0fe311e7f3 91 {
Feike 18:ab0fe311e7f3 92 theta2 = delta_calcangleyz(x0*cos(2/3*pi) + y0*sin(2/3*pi), y0*cos(2/3*pi)-x0*sin(2/3*pi), z0);
Feike 18:ab0fe311e7f3 93 theta3 = delta_calcangleyz(x0*cos(2/3*pi) - y0*sin(2/3*pi), y0*cos(2/3*pi)+x0*sin(2/3*pi), z0);
Feike 18:ab0fe311e7f3 94 //return theta2;
Feike 18:ab0fe311e7f3 95 //return theta3;
Feike 18:ab0fe311e7f3 96 }
Feike 18:ab0fe311e7f3 97 return theta1, theta2, theta3;
Feike 18:ab0fe311e7f3 98
Feike 18:ab0fe311e7f3 99 }
Feike 18:ab0fe311e7f3 100
Feike 18:ab0fe311e7f3 101
Feike 12:2382468d36a4 102 Ticker pulse;
Feike 12:2382468d36a4 103 void pulseget()
Feike 13:18dd7a15603f 104 {
Feike 18:ab0fe311e7f3 105 counts1 = Encoder1.getPulses();
Feike 18:ab0fe311e7f3 106 counts2 = Encoder2.getPulses();
Feike 18:ab0fe311e7f3 107 counts3 = Encoder.getPulses();
Feike 12:2382468d36a4 108 }
Feike 12:2382468d36a4 109
Feike 18:ab0fe311e7f3 110 int anglestep(float angle)
Feike 17:6da57acb7bea 111 {
Feike 18:ab0fe311e7f3 112 float steps;
Feike 18:ab0fe311e7f3 113 steps = angle / 360 * 8400;
Feike 18:ab0fe311e7f3 114 return steps;
Feike 17:6da57acb7bea 115 }
Feike 17:6da57acb7bea 116
Feike 12:2382468d36a4 117 // DE MAIN FUNCTIE
Feike 4:bd21569250c7 118 int main(void)
Feike 18:ab0fe311e7f3 119 {
Feike 18:ab0fe311e7f3 120 pc.baud(115200);
Feike 18:ab0fe311e7f3 121
Feike 17:6da57acb7bea 122 char cc = pc.getc();
Feike 18:ab0fe311e7f3 123 pc.printf("\n\r cc check");
Feike 18:ab0fe311e7f3 124 //char key = term.getc();
Feike 18:ab0fe311e7f3 125 pc.printf("\n\r term check");
Feike 18:ab0fe311e7f3 126
Feike 18:ab0fe311e7f3 127 float angle = 360;
Feike 13:18dd7a15603f 128 while(true)
Feike 13:18dd7a15603f 129 {
Feike 18:ab0fe311e7f3 130 counts1 = 0;
Feike 18:ab0fe311e7f3 131 counts2 = 0;
Feike 18:ab0fe311e7f3 132 counts3 = 0;
Feike 18:ab0fe311e7f3 133
Feike 18:ab0fe311e7f3 134 float steps1;
Feike 18:ab0fe311e7f3 135 float steps2;
Feike 18:ab0fe311e7f3 136 float steps3;
Feike 18:ab0fe311e7f3 137
Feike 18:ab0fe311e7f3 138 counts1 = Encoder1.getPulses();
Feike 18:ab0fe311e7f3 139 counts2 = Encoder2.getPulses();
Feike 18:ab0fe311e7f3 140 counts3 = Encoder.getPulses();
Feike 18:ab0fe311e7f3 141
Feike 18:ab0fe311e7f3 142 delta_calcinverse(x0,y0,z0);
Feike 18:ab0fe311e7f3 143
Feike 18:ab0fe311e7f3 144 oldtheta1 = theta1;
Feike 18:ab0fe311e7f3 145 oldtheta2 = theta2;
Feike 18:ab0fe311e7f3 146 oldtheta3 = theta3;
Feike 18:ab0fe311e7f3 147
Feike 18:ab0fe311e7f3 148 char cc = pc.getc();
Feike 18:ab0fe311e7f3 149
Feike 18:ab0fe311e7f3 150
Feike 18:ab0fe311e7f3 151 if (cc=='a') {
Feike 18:ab0fe311e7f3 152 pc.printf("\n\rleft");
Feike 18:ab0fe311e7f3 153 z0=z0-1.0f;
Feike 18:ab0fe311e7f3 154
Feike 18:ab0fe311e7f3 155 theta1 = delta_calcinverse(x0,y0,z0);
Feike 18:ab0fe311e7f3 156 theta1 = delta_calcangleyz(x0,y0,z0);
Feike 18:ab0fe311e7f3 157
Feike 18:ab0fe311e7f3 158 theta1 = theta1 - oldtheta1;
Feike 18:ab0fe311e7f3 159 theta2 = theta2 - oldtheta2;
Feike 18:ab0fe311e7f3 160 theta3 = theta3 - oldtheta3;
Feike 18:ab0fe311e7f3 161
Feike 18:ab0fe311e7f3 162 steps1 = anglestep(theta1);
Feike 18:ab0fe311e7f3 163 steps2 = anglestep(theta2);
Feike 18:ab0fe311e7f3 164 steps3 = anglestep(theta3);
Feike 18:ab0fe311e7f3 165
Feike 13:18dd7a15603f 166 int frequency_pwm = 10000; //10 kHz PWM
Feike 17:6da57acb7bea 167 motor1_pwm.period(1.0/(double)frequency_pwm); // T=1/f
Feike 18:ab0fe311e7f3 168 motor1_dir.write(1); // positief
Feike 18:ab0fe311e7f3 169 motor1_pwm.write(0.7); // write Duty Cycle
Feike 18:ab0fe311e7f3 170
Feike 18:ab0fe311e7f3 171 motor2_pwm.period(1.0/(double)frequency_pwm); // T=1/f
Feike 18:ab0fe311e7f3 172 motor2_dir.write(0); // positief
Feike 18:ab0fe311e7f3 173 motor2_pwm.write(0.7); // write Duty Cycle
Feike 17:6da57acb7bea 174
Feike 17:6da57acb7bea 175 motor3_pwm.period(1.0/(double)frequency_pwm); // T=1/f
Feike 18:ab0fe311e7f3 176 motor3_dir.write(1); // positief
Feike 18:ab0fe311e7f3 177 motor3_pwm.write(0.7); // write Duty Cycle
Feike 18:ab0fe311e7f3 178
Feike 18:ab0fe311e7f3 179
Feike 18:ab0fe311e7f3 180 }
Feike 18:ab0fe311e7f3 181 if (cc=='d') {
Feike 18:ab0fe311e7f3 182 pc.printf("\n\rright");
Feike 18:ab0fe311e7f3 183
Feike 18:ab0fe311e7f3 184 z0=z0-10.0f;
Feike 18:ab0fe311e7f3 185
Feike 18:ab0fe311e7f3 186 theta1 = delta_calcinverse(x0,y0,z0);
Feike 18:ab0fe311e7f3 187 theta1 = delta_calcangleyz(x0,y0,z0);
Feike 17:6da57acb7bea 188
Feike 18:ab0fe311e7f3 189 theta1 = theta1 - oldtheta1;
Feike 18:ab0fe311e7f3 190 theta2 = theta2 - oldtheta2;
Feike 18:ab0fe311e7f3 191 theta3 = theta3 - oldtheta3;
Feike 18:ab0fe311e7f3 192
Feike 18:ab0fe311e7f3 193 float steps1 = anglestep(theta1);
Feike 18:ab0fe311e7f3 194 float steps2 = anglestep(theta2);
Feike 18:ab0fe311e7f3 195 float steps3 = anglestep(theta3);
Feike 17:6da57acb7bea 196
Feike 18:ab0fe311e7f3 197 pc.printf("\n\r the difference in angles are (%f, %f, %f)", theta1, theta2, theta3);
Feike 18:ab0fe311e7f3 198 pc.printf("\n\r the steps are (%f, %f, %f)", steps1, steps2, steps3);
Feike 18:ab0fe311e7f3 199
Feike 18:ab0fe311e7f3 200 motor1_dir.write(0); // positief
Feike 18:ab0fe311e7f3 201 motor2_dir.write(1); // positief
Feike 18:ab0fe311e7f3 202 motor3_dir.write(0); // positief
Feike 18:ab0fe311e7f3 203
Feike 18:ab0fe311e7f3 204 if (steps1<0) {
Feike 18:ab0fe311e7f3 205 motor1_dir.write(1); // positief
Feike 18:ab0fe311e7f3 206 }
Feike 17:6da57acb7bea 207
Feike 18:ab0fe311e7f3 208 if (steps1<0) {
Feike 18:ab0fe311e7f3 209 motor2_dir.write(0); // positief
Feike 18:ab0fe311e7f3 210
Feike 18:ab0fe311e7f3 211 }
Feike 18:ab0fe311e7f3 212
Feike 18:ab0fe311e7f3 213 if (steps1<0) {
Feike 18:ab0fe311e7f3 214 motor3_dir.write(1); // positief
Feike 8:017b813c72bb 215 }
Feike 18:ab0fe311e7f3 216
Feike 18:ab0fe311e7f3 217 int frequency_pwm = 10000; //10 kHz PWM
Feike 18:ab0fe311e7f3 218 motor1_pwm.period(1.0/(double)frequency_pwm); // T=1/f
Feike 18:ab0fe311e7f3 219 motor1_pwm.write(0.7); // write Duty Cycle
Feike 18:ab0fe311e7f3 220
Feike 18:ab0fe311e7f3 221 motor2_pwm.period(1.0/(double)frequency_pwm); // T=1/f
Feike 18:ab0fe311e7f3 222 motor2_pwm.write(0.7); // write Duty Cycle
Feike 18:ab0fe311e7f3 223
Feike 18:ab0fe311e7f3 224 motor3_pwm.period(1.0/(double)frequency_pwm); // T=1/f
Feike 18:ab0fe311e7f3 225 motor3_pwm.write(0.7); // write Duty Cycle
Feike 18:ab0fe311e7f3 226
Feike 18:ab0fe311e7f3 227 while (abs(counts1)<=abs(steps1) || abs(counts2)<=abs(steps2) || abs(counts3)<=abs(steps3)) {
Feike 18:ab0fe311e7f3 228 pc.printf("while");
Feike 18:ab0fe311e7f3 229 if(abs(counts1)>=abs(steps1)) {
Feike 18:ab0fe311e7f3 230 motor1_pwm.write(0);
Feike 18:ab0fe311e7f3 231 }
Feike 18:ab0fe311e7f3 232 if (abs(counts2)>=abs(steps2)) {
Feike 18:ab0fe311e7f3 233 motor2_pwm.write(0);
Feike 18:ab0fe311e7f3 234 }
Feike 18:ab0fe311e7f3 235 if (abs(counts3)>=abs(steps3)) {
Feike 18:ab0fe311e7f3 236 motor3_pwm.write(0);
Feike 18:ab0fe311e7f3 237 }
Feike 8:017b813c72bb 238 }
Feike 18:ab0fe311e7f3 239
Feike 18:ab0fe311e7f3 240 }
Feike 18:ab0fe311e7f3 241
Feike 18:ab0fe311e7f3 242
Feike 18:ab0fe311e7f3 243 wait(1.0/100);
Feike 18:ab0fe311e7f3 244 }
Feike 18:ab0fe311e7f3 245 }