Vivien van Veldhuizen / Mbed 2 deprecated Signa-bot

Signa-bot code for project BioRobotics, at University of Twente.

Dependencies:   mbed QEI MODSERIAL FastPWM ttmath Math

Motor_tryout.cpp@32:60a71dcfdb7a, 2019-10-31 (annotated)

Committer:
viviien
Date:
Thu Oct 31 14:30:59 2019 +0000
Revision:
32:60a71dcfdb7a
Parent:
31:3c13f1c35ee5
Child:
33:88fbf14d8aaf
Het werkt zonder trillingen

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 12:2382468d36a4 3 #include "QEI.h"
Feike 18:ab0fe311e7f3 4 #include "Math.h"
Feike 18:ab0fe311e7f3 5 #include "ttmath.h"
Feike 18:ab0fe311e7f3 6
Feike 17:6da57acb7bea 7 MODSERIAL pc(USBTX, USBRX);
Feike 18:ab0fe311e7f3 8 //Serial term (USBTX, USBRX);
Feike 17:6da57acb7bea 9 PwmOut motor1_pwm(PTC2);
Feike 17:6da57acb7bea 10 DigitalOut motor1_dir(PTC3);
Feike 18:ab0fe311e7f3 11 PwmOut motor2_pwm(PTA2);
Feike 18:ab0fe311e7f3 12 DigitalOut motor2_dir(PTB23);
Feike 18:ab0fe311e7f3 13 PwmOut motor3_pwm(PTC4);
Feike 18:ab0fe311e7f3 14 DigitalOut motor3_dir(PTC12);
Feike 4:bd21569250c7 15
viviien 30:390cab7cd6e6 16 AnalogIn potmeter1(A1);
viviien 30:390cab7cd6e6 17
viviien 30:390cab7cd6e6 18
viviien 30:390cab7cd6e6 19
viviien 23:18b0be02187f 20 QEI Encoder1(D12,D13,NC,64,QEI::X4_ENCODING);
viviien 23:18b0be02187f 21 QEI Encoder2(D10,D11,NC,64,QEI::X4_ENCODING);
viviien 23:18b0be02187f 22 QEI Encoder3(D2,D3,NC,64,QEI::X4_ENCODING);
Feike 7:e119b12e5e7f 23
viviien 25:eb3204e45d33 24 float steps1 = 0;
viviien 25:eb3204e45d33 25 float steps2 = 0;
viviien 25:eb3204e45d33 26 float steps3 = 0;
Feike 19:9c8ab7922191 27 int check;
Feike 13:18dd7a15603f 28 int quit;
Feike 19:9c8ab7922191 29 int limit_pos = 8400;
Feike 19:9c8ab7922191 30 float steps;
Feike 19:9c8ab7922191 31 int g = 0;
Feike 19:9c8ab7922191 32
Feike 19:9c8ab7922191 33 bool check1;
Feike 19:9c8ab7922191 34 bool check2;
Feike 19:9c8ab7922191 35 bool check3;
Feike 18:ab0fe311e7f3 36 int counts1 = 0;
Feike 18:ab0fe311e7f3 37 int counts2 = 0;
Feike 18:ab0fe311e7f3 38 int counts3 = 0;
viviien 27:3eb181cbe183 39
Feike 13:18dd7a15603f 40
Feike 18:ab0fe311e7f3 41 const float le = 15.0;
Feike 18:ab0fe311e7f3 42 const float f = 37.5;
Feike 18:ab0fe311e7f3 43 const float re = 174.0;
Feike 18:ab0fe311e7f3 44 const float rf = 50.0;
Feike 18:ab0fe311e7f3 45 const float pi = 3.14159265358979323846;
viviien 22:9f911405e096 46 const float cospi = -0.5;
viviien 22:9f911405e096 47 const float sinpi = 0.8660254;
Feike 18:ab0fe311e7f3 48 float y2;
Feike 18:ab0fe311e7f3 49 float y1;
Feike 18:ab0fe311e7f3 50 float z1;
Feike 18:ab0fe311e7f3 51 float z2;
Feike 18:ab0fe311e7f3 52 float rje2;
Feike 18:ab0fe311e7f3 53 float rje;
Feike 18:ab0fe311e7f3 54 float r2;
Feike 18:ab0fe311e7f3 55 float r;
Feike 19:9c8ab7922191 56
Feike 18:ab0fe311e7f3 57 float z0=-172;
Feike 18:ab0fe311e7f3 58 float y0=0;
Feike 18:ab0fe311e7f3 59 float x0=0;
Feike 19:9c8ab7922191 60
Feike 18:ab0fe311e7f3 61 float theta1;
Feike 18:ab0fe311e7f3 62 float theta2;
Feike 18:ab0fe311e7f3 63 float theta3;
Feike 18:ab0fe311e7f3 64 float oldtheta1=0;
Feike 18:ab0fe311e7f3 65 float oldtheta2=0;
Feike 18:ab0fe311e7f3 66 float oldtheta3=0;
Feike 19:9c8ab7922191 67
viviien 28:43a1d67ff8ea 68 // Constant values for PI
viviien 30:390cab7cd6e6 69 float Kp;
viviien 30:390cab7cd6e6 70 float Kd;
viviien 30:390cab7cd6e6 71
viviien 30:390cab7cd6e6 72 float Ts = 0.0025;
viviien 28:43a1d67ff8ea 73 float error1 = 0;
viviien 30:390cab7cd6e6 74 float error2 = 0;
viviien 30:390cab7cd6e6 75 float error3 = 0;
viviien 28:43a1d67ff8ea 76
viviien 30:390cab7cd6e6 77 float newmotor1 = 1;
viviien 30:390cab7cd6e6 78 float newmotor2 = 1;
viviien 30:390cab7cd6e6 79 float newmotor3 = 1;
viviien 30:390cab7cd6e6 80
viviien 30:390cab7cd6e6 81 float u_k1 = 0;
viviien 30:390cab7cd6e6 82 float u_k2 = 0;
viviien 30:390cab7cd6e6 83 float u_k3 = 0;
viviien 30:390cab7cd6e6 84
viviien 30:390cab7cd6e6 85 float u_d1 = 0;
viviien 30:390cab7cd6e6 86 float u_d2 = 0;
viviien 30:390cab7cd6e6 87 float u_d3 = 0;
viviien 30:390cab7cd6e6 88
viviien 30:390cab7cd6e6 89 float lasterror1 = 0;
viviien 30:390cab7cd6e6 90 float lasterror2 = 0;
viviien 30:390cab7cd6e6 91 float lasterror3 = 0;
viviien 30:390cab7cd6e6 92
viviien 30:390cab7cd6e6 93 float derivative1 = 0;
viviien 30:390cab7cd6e6 94 float derivative2 = 0;
viviien 30:390cab7cd6e6 95 float derivative3 = 0;
viviien 30:390cab7cd6e6 96
viviien 32:60a71dcfdb7a 97 float angle1 = 0;
viviien 32:60a71dcfdb7a 98 float angle2 = 0;
viviien 32:60a71dcfdb7a 99 float angle3 = 0;
viviien 32:60a71dcfdb7a 100
viviien 32:60a71dcfdb7a 101 bool timer = false;
viviien 32:60a71dcfdb7a 102 float time_sin = 0;
viviien 32:60a71dcfdb7a 103
viviien 32:60a71dcfdb7a 104
viviien 32:60a71dcfdb7a 105 void delta_calctheta1 () {
viviien 32:60a71dcfdb7a 106 float y2 = y0 + le;
viviien 32:60a71dcfdb7a 107 float y1 = f;
viviien 32:60a71dcfdb7a 108 float z1 = 0.0;
viviien 32:60a71dcfdb7a 109 float z2 = z0;
viviien 32:60a71dcfdb7a 110 float rje2 = re*re - x0*x0;
viviien 32:60a71dcfdb7a 111 float rje = sqrt(rje2);
viviien 32:60a71dcfdb7a 112 float r2 = (y1-y2)*(y1-y2) + (z1-z0)*(z1-z0);
viviien 32:60a71dcfdb7a 113 float r = sqrt(r2);
viviien 32:60a71dcfdb7a 114
viviien 32:60a71dcfdb7a 115 if ((r+rje<rf) || (r + rf <rje) || (rf+rje<r)) {
viviien 32:60a71dcfdb7a 116 int check = 1;
viviien 32:60a71dcfdb7a 117 pc.printf("\n\rPunt bestaat niet");
viviien 32:60a71dcfdb7a 118 }
viviien 32:60a71dcfdb7a 119 else {
viviien 32:60a71dcfdb7a 120 float alpha = acos((r2 + rf*rf -rje2)/(2*rf*r));
viviien 32:60a71dcfdb7a 121 float beta = atan((z1-z2)/(y1-y2));
viviien 32:60a71dcfdb7a 122 if(beta<0) {
viviien 32:60a71dcfdb7a 123 beta = beta + pi;
viviien 32:60a71dcfdb7a 124 }
viviien 32:60a71dcfdb7a 125 theta1 = (beta - alpha);
viviien 32:60a71dcfdb7a 126 }
viviien 32:60a71dcfdb7a 127 }
viviien 32:60a71dcfdb7a 128
viviien 32:60a71dcfdb7a 129 void delta_calctheta2 () {
viviien 32:60a71dcfdb7a 130 float xref = x0*cospi+y0*sinpi;
viviien 32:60a71dcfdb7a 131 float yref = y0*cospi-x0*sinpi;
viviien 32:60a71dcfdb7a 132 float zref = z0;
viviien 32:60a71dcfdb7a 133 float y2 = yref + le;
viviien 32:60a71dcfdb7a 134 float y1 = f;
viviien 32:60a71dcfdb7a 135 float z1 = 0.0;
viviien 32:60a71dcfdb7a 136 float z2 = zref;
viviien 32:60a71dcfdb7a 137 float rje2 = re*re - xref*xref;
viviien 32:60a71dcfdb7a 138 float rje = sqrt(rje2);
viviien 32:60a71dcfdb7a 139 float r2 = (y1-y2)*(y1-y2) + (z1-zref)*(z1-zref);
viviien 32:60a71dcfdb7a 140 float r = sqrt(r2);
viviien 32:60a71dcfdb7a 141
viviien 32:60a71dcfdb7a 142 if ((r+rje<rf) || (r + rf <rje) || (rf+rje<r)) {
viviien 32:60a71dcfdb7a 143 int check = 1;
viviien 32:60a71dcfdb7a 144 pc.printf("\n\rPunt bestaat niet");
viviien 32:60a71dcfdb7a 145 }
viviien 32:60a71dcfdb7a 146 else {
viviien 32:60a71dcfdb7a 147 float alpha = acos((r2 + rf*rf -rje2)/(2*rf*r));
viviien 32:60a71dcfdb7a 148 float beta = atan((z1-z2)/(y1-y2));
viviien 32:60a71dcfdb7a 149 if(beta<0) {
viviien 32:60a71dcfdb7a 150 beta = beta + pi;
viviien 32:60a71dcfdb7a 151 }
viviien 32:60a71dcfdb7a 152 theta2 = (beta - alpha);
viviien 32:60a71dcfdb7a 153 }
viviien 32:60a71dcfdb7a 154 }
viviien 32:60a71dcfdb7a 155
viviien 32:60a71dcfdb7a 156 void delta_calctheta3 () {
viviien 32:60a71dcfdb7a 157 float xreff = x0*cospi-y0*sinpi;
viviien 32:60a71dcfdb7a 158 float yreff = y0*cospi+y0*sinpi;
viviien 32:60a71dcfdb7a 159 float zreff = z0;
viviien 32:60a71dcfdb7a 160 float y2 = yreff + le;
viviien 32:60a71dcfdb7a 161 float y1 = f;
viviien 32:60a71dcfdb7a 162 float z1 = 0.0;
viviien 32:60a71dcfdb7a 163 float z2 = zreff;
viviien 32:60a71dcfdb7a 164 float rje2 = re*re - xreff*xreff;
viviien 32:60a71dcfdb7a 165 float rje = sqrt(rje2);
viviien 32:60a71dcfdb7a 166 float r2 = (y1-y2)*(y1-y2) + (z1-zreff)*(z1-zreff);
viviien 32:60a71dcfdb7a 167 float r = sqrt(r2);
viviien 32:60a71dcfdb7a 168
viviien 32:60a71dcfdb7a 169 if ((r+rje<rf) || (r + rf <rje) || (rf+rje<r)) {
viviien 32:60a71dcfdb7a 170 int check = 1;
viviien 32:60a71dcfdb7a 171 pc.printf("\n\rPunt bestaat niet");
viviien 32:60a71dcfdb7a 172 }
viviien 32:60a71dcfdb7a 173 else {
viviien 32:60a71dcfdb7a 174 float alpha = acos((r2 + rf*rf -rje2)/(2*rf*r));
viviien 32:60a71dcfdb7a 175 float beta = atan((z1-z2)/(y1-y2));
viviien 32:60a71dcfdb7a 176 if(beta<0) {
viviien 32:60a71dcfdb7a 177 beta = beta + pi;
viviien 32:60a71dcfdb7a 178 }
viviien 32:60a71dcfdb7a 179 theta3 = (beta - alpha);
viviien 32:60a71dcfdb7a 180 }
viviien 32:60a71dcfdb7a 181 }
viviien 32:60a71dcfdb7a 182
viviien 32:60a71dcfdb7a 183
viviien 32:60a71dcfdb7a 184
viviien 30:390cab7cd6e6 185
viviien 30:390cab7cd6e6 186 Ticker motor_timer;
viviien 32:60a71dcfdb7a 187 void motor(){
viviien 32:60a71dcfdb7a 188
viviien 30:390cab7cd6e6 189 counts1 = Encoder1.getPulses();
viviien 30:390cab7cd6e6 190 counts2 = Encoder2.getPulses();
viviien 30:390cab7cd6e6 191 counts3 = Encoder3.getPulses();
viviien 32:60a71dcfdb7a 192
viviien 32:60a71dcfdb7a 193 z0 = -172.0 + 10* sin(time_sin);
viviien 29:7eb028b359a1 194
viviien 32:60a71dcfdb7a 195 delta_calctheta1 ();
viviien 32:60a71dcfdb7a 196 delta_calctheta2 ();
viviien 32:60a71dcfdb7a 197 delta_calctheta3 ();
viviien 30:390cab7cd6e6 198
viviien 32:60a71dcfdb7a 199 angle1 = counts1/(8400.0)*2.0*pi;
viviien 32:60a71dcfdb7a 200 angle2 = counts2/(8400.0)*2.0*pi;
viviien 32:60a71dcfdb7a 201 angle3 = counts3/(8400.0)*2.0*pi;
viviien 30:390cab7cd6e6 202
viviien 32:60a71dcfdb7a 203 error1 = theta1 - angle1;
viviien 32:60a71dcfdb7a 204 error2 = theta2 - angle2;
viviien 32:60a71dcfdb7a 205 error3 = theta3 - angle3;
viviien 30:390cab7cd6e6 206
viviien 32:60a71dcfdb7a 207 Kp = 10;
viviien 31:3c13f1c35ee5 208 Kd = potmeter1 * 1000;
viviien 30:390cab7cd6e6 209
viviien 32:60a71dcfdb7a 210 u_k1 = Kp * error1;
viviien 30:390cab7cd6e6 211 u_k2 = Kp * error2;
viviien 30:390cab7cd6e6 212 u_k3 = Kp * error3;
viviien 30:390cab7cd6e6 213
viviien 32:60a71dcfdb7a 214 newmotor1 = u_k1;
viviien 32:60a71dcfdb7a 215 newmotor2 = u_k2;
viviien 32:60a71dcfdb7a 216 newmotor3 = u_k3;
viviien 30:390cab7cd6e6 217
viviien 30:390cab7cd6e6 218 if (newmotor1>1.0f){
viviien 30:390cab7cd6e6 219 newmotor1 =1.0f;
viviien 23:18b0be02187f 220 }
viviien 30:390cab7cd6e6 221 if (newmotor1<-1.0f){
viviien 30:390cab7cd6e6 222 newmotor1 = -1.0f;
viviien 30:390cab7cd6e6 223 }
viviien 30:390cab7cd6e6 224
viviien 30:390cab7cd6e6 225 if (newmotor2>1.0f){
viviien 30:390cab7cd6e6 226 newmotor2 =1.0f;
viviien 30:390cab7cd6e6 227 }
viviien 30:390cab7cd6e6 228 if (newmotor2<-1.0f){
viviien 30:390cab7cd6e6 229 newmotor2 = -1.0f;
viviien 30:390cab7cd6e6 230 }
viviien 30:390cab7cd6e6 231
viviien 30:390cab7cd6e6 232 if (newmotor3>1.0f){
viviien 30:390cab7cd6e6 233 newmotor3 =1.0f;
viviien 30:390cab7cd6e6 234 }
viviien 30:390cab7cd6e6 235 if (newmotor3<-1.0f){
viviien 30:390cab7cd6e6 236 newmotor3 = -1.0f;
viviien 30:390cab7cd6e6 237 }
viviien 30:390cab7cd6e6 238
viviien 30:390cab7cd6e6 239 motor1_pwm.write(fabs(newmotor1));
viviien 32:60a71dcfdb7a 240 motor1_dir.write(newmotor1<0);
viviien 30:390cab7cd6e6 241
viviien 30:390cab7cd6e6 242 motor2_pwm.write(fabs(newmotor2));
viviien 32:60a71dcfdb7a 243 motor2_dir.write(newmotor2>0);
viviien 30:390cab7cd6e6 244
viviien 30:390cab7cd6e6 245 motor3_pwm.write(fabs(newmotor3));
viviien 32:60a71dcfdb7a 246 motor3_dir.write(newmotor3<0);
viviien 30:390cab7cd6e6 247
viviien 32:60a71dcfdb7a 248 time_sin += 0.002;
viviien 30:390cab7cd6e6 249 }
viviien 25:eb3204e45d33 250
viviien 32:60a71dcfdb7a 251
viviien 28:43a1d67ff8ea 252
viviien 28:43a1d67ff8ea 253 ///////////////////
viviien 28:43a1d67ff8ea 254 // MAIN FUNCTION //
viviien 28:43a1d67ff8ea 255 ///////////////////
viviien 28:43a1d67ff8ea 256
viviien 22:9f911405e096 257 int main(void) {
viviien 30:390cab7cd6e6 258 motor_timer.attach(&motor, 0.002);
viviien 26:432d3519ba86 259
viviien 28:43a1d67ff8ea 260 pc.baud(115200);
Feike 17:6da57acb7bea 261 char cc = pc.getc();
viviien 30:390cab7cd6e6 262
viviien 30:390cab7cd6e6 263 int frequency_pwm = 10000; //10 kHz PWM
Feike 19:9c8ab7922191 264
viviien 28:43a1d67ff8ea 265 while(true){
viviien 22:9f911405e096 266 }
viviien 32:60a71dcfdb7a 267 //END MAIN
viviien 22:9f911405e096 268 }