Francisco Santos / Mbed 2 deprecated SRA_VFF

pepe

Dependencies:   mbed Matrix

Robot.cpp@0:a7324f51348d, 2019-03-13 (annotated)

Committer:
PedroMartins96
Date:
Wed Mar 13 23:04:14 2019 +0000
Revision:
0:a7324f51348d
Child:
1:2716ea33958b
444

Who changed what in which revision?

UserRevisionLine numberNew contents of line
PedroMartins96 0:a7324f51348d 1 #include "Robot.h"
PedroMartins96 0:a7324f51348d 2 #include "mbed.h"
PedroMartins96 0:a7324f51348d 3
PedroMartins96 0:a7324f51348d 4 Serial PC(SERIAL_TX, SERIAL_RX);
PedroMartins96 0:a7324f51348d 5 I2C i2c(I2C_SDA, I2C_SCL );
PedroMartins96 0:a7324f51348d 6 const int addr8bit = 20 << 1; // 7bit I2C address is 20; 8bit I2C address is 40 (decimal).
PedroMartins96 0:a7324f51348d 7
PedroMartins96 0:a7324f51348d 8 int16_t countsLeft = 0;
PedroMartins96 0:a7324f51348d 9 int16_t countsRight = 0;
PedroMartins96 0:a7324f51348d 10 float De=0, Dd=0, pi=3.141529, Ldis=14.2, r=6.95/2, theta=0, teta=0, x=50, y=10, D=0, Wee=0, Wdd=0, v=0, w=0;
PedroMartins96 0:a7324f51348d 11
PedroMartins96 0:a7324f51348d 12
PedroMartins96 0:a7324f51348d 13 ////////////////////////////////////////
PedroMartins96 0:a7324f51348d 14 // Movimento para uma pose arbitrária //
PedroMartins96 0:a7324f51348d 15 ////////////////////////////////////////
PedroMartins96 0:a7324f51348d 16 void to_pos(float x2,float y2,float phi2)
PedroMartins96 0:a7324f51348d 17 {
PedroMartins96 0:a7324f51348d 18 while(1){
PedroMartins96 0:a7324f51348d 19 float alfa,beta,a=33,b=30;
PedroMartins96 0:a7324f51348d 20 float Kp=0.7,Ka=5,Kb=-2;
PedroMartins96 0:a7324f51348d 21 while(abs(x)<abs(x2)-1 || abs(y)<abs(y2)-1 || teta<=(phi2-0.05) || teta>=(phi2+0.05)) {// Condição de paragem
PedroMartins96 0:a7324f51348d 22 // Controlo linear
PedroMartins96 0:a7324f51348d 23 v=Kp*(sqrt(((x2-x)*(x2-x)+(y2-y)*(y2-y))));
PedroMartins96 0:a7324f51348d 24
PedroMartins96 0:a7324f51348d 25 alfa=-teta+atan2(y2-y,x2-x);
PedroMartins96 0:a7324f51348d 26
PedroMartins96 0:a7324f51348d 27 if(abs(v)<1){
PedroMartins96 0:a7324f51348d 28 alfa=0;
PedroMartins96 0:a7324f51348d 29 a=0;
PedroMartins96 0:a7324f51348d 30 b=0;
PedroMartins96 0:a7324f51348d 31 }
PedroMartins96 0:a7324f51348d 32 // Verifica se o alpha pertence ao limite entre [-pi/2; pi/2], se não adiciona/remove pi e inverte a velocidade
PedroMartins96 0:a7324f51348d 33 if(alfa<-pi/2){
PedroMartins96 0:a7324f51348d 34 alfa=alfa+pi;
PedroMartins96 0:a7324f51348d 35 v=-v;
PedroMartins96 0:a7324f51348d 36 }
PedroMartins96 0:a7324f51348d 37 else if(alfa>pi/2){
PedroMartins96 0:a7324f51348d 38 alfa=alfa-pi;
PedroMartins96 0:a7324f51348d 39 v=-v;
PedroMartins96 0:a7324f51348d 40 }
PedroMartins96 0:a7324f51348d 41
PedroMartins96 0:a7324f51348d 42 beta=-teta-alfa+phi2;
PedroMartins96 0:a7324f51348d 43 w=Ka*alfa+Kb*beta;
PedroMartins96 0:a7324f51348d 44
PedroMartins96 0:a7324f51348d 45 if(alfa==0){
PedroMartins96 0:a7324f51348d 46 w=w+abs(w)/w*4;
PedroMartins96 0:a7324f51348d 47 Kb=3;
PedroMartins96 0:a7324f51348d 48 }
PedroMartins96 0:a7324f51348d 49
PedroMartins96 0:a7324f51348d 50 motion();
PedroMartins96 0:a7324f51348d 51 setSpeeds(Wee+a*abs(v)/v,Wdd+b*abs(v)/v);
PedroMartins96 0:a7324f51348d 52
PedroMartins96 0:a7324f51348d 53 //PC.printf("v=%f w=%f alfa=%f beta=%f teta=%f phi2=%f x=%f y=%f\n", v,w,alfa,beta,teta,phi2,x,y);
PedroMartins96 0:a7324f51348d 54 PC.printf("%f %f\n", x,y);
PedroMartins96 0:a7324f51348d 55 }
PedroMartins96 0:a7324f51348d 56 setSpeeds(0,0); // A condição de paragem verifica-se e para-se o robo
PedroMartins96 0:a7324f51348d 57 }
PedroMartins96 0:a7324f51348d 58 }
PedroMartins96 0:a7324f51348d 59
PedroMartins96 0:a7324f51348d 60
PedroMartins96 0:a7324f51348d 61 /////////////////////////////////
PedroMartins96 0:a7324f51348d 62 // Movimento segundo uma linha //
PedroMartins96 0:a7324f51348d 63 /////////////////////////////////
PedroMartins96 0:a7324f51348d 64 void to_line(float a,float b,float c)
PedroMartins96 0:a7324f51348d 65 {
PedroMartins96 0:a7324f51348d 66 float d=0,alfad=0,phi1=0,alfah=0;
PedroMartins96 0:a7324f51348d 67 float Kd=0.45,Kh=15;
PedroMartins96 0:a7324f51348d 68 while(1){
PedroMartins96 0:a7324f51348d 69 while((abs(x) < 200)&&(abs(y)<200)) {
PedroMartins96 0:a7324f51348d 70 v=100;
PedroMartins96 0:a7324f51348d 71 d=(a*x+b*y+c)/sqrt(a*a+b*b);
PedroMartins96 0:a7324f51348d 72
PedroMartins96 0:a7324f51348d 73 //Controlo proporcional que roda a plataforma na direção da linha
PedroMartins96 0:a7324f51348d 74 alfad=(-Kd)*d;
PedroMartins96 0:a7324f51348d 75 if (abs(alfad)<0.3) alfad=alfad*3;
PedroMartins96 0:a7324f51348d 76 phi1=atan2(-a,b);
PedroMartins96 0:a7324f51348d 77
PedroMartins96 0:a7324f51348d 78 //Controlo proporcional que ajusta o ângulo de direcionamento
PedroMartins96 0:a7324f51348d 79 alfah=Kh*(phi1-teta);
PedroMartins96 0:a7324f51348d 80
PedroMartins96 0:a7324f51348d 81 w=alfad+alfah;
PedroMartins96 0:a7324f51348d 82
PedroMartins96 0:a7324f51348d 83 if(w > 12) w = 12;
PedroMartins96 0:a7324f51348d 84 else if(w<-12) w = -12;
PedroMartins96 0:a7324f51348d 85
PedroMartins96 0:a7324f51348d 86 motion();
PedroMartins96 0:a7324f51348d 87 setSpeeds(Wee,Wdd);
PedroMartins96 0:a7324f51348d 88 //PC.printf("v=%f w=%f alfad=%f alfah=%f teta=%f x=%f y=%f\n", v,w,alfad,alfah,teta,x,y);
PedroMartins96 0:a7324f51348d 89 PC.printf("%f %f\n", x,y);
PedroMartins96 0:a7324f51348d 90 }
PedroMartins96 0:a7324f51348d 91 setSpeeds(0,0);
PedroMartins96 0:a7324f51348d 92 }
PedroMartins96 0:a7324f51348d 93 }
PedroMartins96 0:a7324f51348d 94
PedroMartins96 0:a7324f51348d 95
PedroMartins96 0:a7324f51348d 96 /////////////////////////////
PedroMartins96 0:a7324f51348d 97 // Movimento para um ponto //
PedroMartins96 0:a7324f51348d 98 /////////////////////////////
PedroMartins96 0:a7324f51348d 99 void to_point(float x1,float y1)
PedroMartins96 0:a7324f51348d 100 {
PedroMartins96 0:a7324f51348d 101 while(1) {
PedroMartins96 0:a7324f51348d 102 float phi1=0,aux=0;
PedroMartins96 0:a7324f51348d 103 float Kv=1.5,Ks=30;
PedroMartins96 0:a7324f51348d 104 while((abs(x)<abs(x1)-1)||(abs(y)<abs(y1)-1)||(abs(x)>abs(x1)+1)||(abs(y)>abs(y1)+1)) {
PedroMartins96 0:a7324f51348d 105 v=Kv*(sqrt(((x1-x)*(x1-x)+((y1-y)*(y1-y))))); //Controlo de velocidade -- Quanto mais próximo do ponto objetivo, menor vai ser a velocidade
PedroMartins96 0:a7324f51348d 106
PedroMartins96 0:a7324f51348d 107 phi1=atan2((y1-y),(x1-x)); // Ângulo entre a posição final e a posição atual
PedroMartins96 0:a7324f51348d 108
PedroMartins96 0:a7324f51348d 109 aux=phi1-teta;
PedroMartins96 0:a7324f51348d 110 //Verifica se a diferença dos angulos pertence ao limite entre [-pi;pi]
PedroMartins96 0:a7324f51348d 111 if(aux<-pi){
PedroMartins96 0:a7324f51348d 112 aux=aux+2*pi;
PedroMartins96 0:a7324f51348d 113 }
PedroMartins96 0:a7324f51348d 114 else if(aux>pi){
PedroMartins96 0:a7324f51348d 115 aux=aux-2*pi;
PedroMartins96 0:a7324f51348d 116 }
PedroMartins96 0:a7324f51348d 117
PedroMartins96 0:a7324f51348d 118 //Controlo proporcional
PedroMartins96 0:a7324f51348d 119 w=Ks*(aux);
PedroMartins96 0:a7324f51348d 120
PedroMartins96 0:a7324f51348d 121 if(w > 12) w = 12;
PedroMartins96 0:a7324f51348d 122 else if(w<-12) w = -12;
PedroMartins96 0:a7324f51348d 123
PedroMartins96 0:a7324f51348d 124 motion();
PedroMartins96 0:a7324f51348d 125 setSpeeds(Wee+28,Wdd+25);
PedroMartins96 0:a7324f51348d 126 //PC.printf("v=%f w=%f phi1=%f x=%f y=%f teta=%f\n", v,w,phi1,x,y,teta);
PedroMartins96 0:a7324f51348d 127 PC.printf("%f %f %f %f\n", x,y,teta, w);
PedroMartins96 0:a7324f51348d 128 }
PedroMartins96 0:a7324f51348d 129 setSpeeds(0,0);
PedroMartins96 0:a7324f51348d 130 }
PedroMartins96 0:a7324f51348d 131
PedroMartins96 0:a7324f51348d 132 }
PedroMartins96 0:a7324f51348d 133
PedroMartins96 0:a7324f51348d 134 ///////////////
PedroMartins96 0:a7324f51348d 135 // Odometria //
PedroMartins96 0:a7324f51348d 136 ///////////////
PedroMartins96 0:a7324f51348d 137 void motion()
PedroMartins96 0:a7324f51348d 138 {
PedroMartins96 0:a7324f51348d 139 getCountsAndReset();
PedroMartins96 0:a7324f51348d 140 De=((2*pi*r)*countsLeft)/1440;
PedroMartins96 0:a7324f51348d 141 Dd=((2*pi*r)*countsRight)/1440;
PedroMartins96 0:a7324f51348d 142 theta=(Dd-De)/Ldis;
PedroMartins96 0:a7324f51348d 143
PedroMartins96 0:a7324f51348d 144 D=(De+Dd)/2;
PedroMartins96 0:a7324f51348d 145
PedroMartins96 0:a7324f51348d 146 if(theta==0) {
PedroMartins96 0:a7324f51348d 147 x=x+D*cos(teta);
PedroMartins96 0:a7324f51348d 148 y=y+D*sin(teta);
PedroMartins96 0:a7324f51348d 149 teta=teta;
PedroMartins96 0:a7324f51348d 150 } else {
PedroMartins96 0:a7324f51348d 151 x = x + D*((sin(theta/2)/(theta/2))*cos(teta+theta/2));
PedroMartins96 0:a7324f51348d 152 y = y + D*((sin(theta/2)/(theta/2))*sin(teta+theta/2));
PedroMartins96 0:a7324f51348d 153 teta=teta+theta;
PedroMartins96 0:a7324f51348d 154 }
PedroMartins96 0:a7324f51348d 155
PedroMartins96 0:a7324f51348d 156 //Limite entre [-2pi; 2pi]
PedroMartins96 0:a7324f51348d 157 if (teta>=(2*pi)) {
PedroMartins96 0:a7324f51348d 158 teta=teta-2*pi;
PedroMartins96 0:a7324f51348d 159 }
PedroMartins96 0:a7324f51348d 160 if (teta<=(-2*pi)) {
PedroMartins96 0:a7324f51348d 161 teta=teta+2*pi;
PedroMartins96 0:a7324f51348d 162 }
PedroMartins96 0:a7324f51348d 163
PedroMartins96 0:a7324f51348d 164 Wee=(v-(Ldis/2)*w);
PedroMartins96 0:a7324f51348d 165 Wdd=(v+(Ldis/2)*w);
PedroMartins96 0:a7324f51348d 166 //PC.printf("phi1=%f D=%f x=%f y=%f teta=%f\n", w,theta,x,y,teta);
PedroMartins96 0:a7324f51348d 167 }
PedroMartins96 0:a7324f51348d 168
PedroMartins96 0:a7324f51348d 169 void setSpeeds(int16_t leftSpeed, int16_t rightSpeed)
PedroMartins96 0:a7324f51348d 170 {
PedroMartins96 0:a7324f51348d 171 char buffer[5];
PedroMartins96 0:a7324f51348d 172
PedroMartins96 0:a7324f51348d 173 buffer[0] = 0xA1;
PedroMartins96 0:a7324f51348d 174 memcpy(&buffer[1], &leftSpeed, sizeof(leftSpeed));
PedroMartins96 0:a7324f51348d 175 memcpy(&buffer[3], &rightSpeed, sizeof(rightSpeed));
PedroMartins96 0:a7324f51348d 176
PedroMartins96 0:a7324f51348d 177 i2c.write(addr8bit, buffer, 5); // 5 bytes
PedroMartins96 0:a7324f51348d 178 }
PedroMartins96 0:a7324f51348d 179
PedroMartins96 0:a7324f51348d 180 void setLeftSpeed(int16_t speed)
PedroMartins96 0:a7324f51348d 181 {
PedroMartins96 0:a7324f51348d 182 char buffer[3];
PedroMartins96 0:a7324f51348d 183
PedroMartins96 0:a7324f51348d 184 buffer[0] = 0xA2;
PedroMartins96 0:a7324f51348d 185 memcpy(&buffer[1], &speed, sizeof(speed));
PedroMartins96 0:a7324f51348d 186
PedroMartins96 0:a7324f51348d 187 i2c.write(addr8bit, buffer, 3); // 3 bytes
PedroMartins96 0:a7324f51348d 188 }
PedroMartins96 0:a7324f51348d 189
PedroMartins96 0:a7324f51348d 190 void setRightSpeed(int16_t speed)
PedroMartins96 0:a7324f51348d 191 {
PedroMartins96 0:a7324f51348d 192 char buffer[3];
PedroMartins96 0:a7324f51348d 193
PedroMartins96 0:a7324f51348d 194 buffer[0] = 0xA3;
PedroMartins96 0:a7324f51348d 195 memcpy(&buffer[1], &speed, sizeof(speed));
PedroMartins96 0:a7324f51348d 196
PedroMartins96 0:a7324f51348d 197 i2c.write(addr8bit, buffer, 3); // 3 bytes
PedroMartins96 0:a7324f51348d 198 }
PedroMartins96 0:a7324f51348d 199
PedroMartins96 0:a7324f51348d 200 void getCounts()
PedroMartins96 0:a7324f51348d 201 {
PedroMartins96 0:a7324f51348d 202 char write_buffer[2];
PedroMartins96 0:a7324f51348d 203 char read_buffer[4];
PedroMartins96 0:a7324f51348d 204
PedroMartins96 0:a7324f51348d 205 write_buffer[0] = 0xA0;
PedroMartins96 0:a7324f51348d 206 i2c.write(addr8bit, write_buffer, 1);
PedroMartins96 0:a7324f51348d 207 wait_us(100);
PedroMartins96 0:a7324f51348d 208 i2c.read( addr8bit, read_buffer, 4);
PedroMartins96 0:a7324f51348d 209 countsLeft = (int16_t((read_buffer[0]<<8)|read_buffer[1]));
PedroMartins96 0:a7324f51348d 210 countsRight = (int16_t((read_buffer[2]<<8)|read_buffer[3]));
PedroMartins96 0:a7324f51348d 211 }
PedroMartins96 0:a7324f51348d 212
PedroMartins96 0:a7324f51348d 213 void getCountsAndReset()
PedroMartins96 0:a7324f51348d 214 {
PedroMartins96 0:a7324f51348d 215 char write_buffer[2];
PedroMartins96 0:a7324f51348d 216 char read_buffer[4];
PedroMartins96 0:a7324f51348d 217
PedroMartins96 0:a7324f51348d 218 write_buffer[0] = 0xA4;
PedroMartins96 0:a7324f51348d 219 i2c.write(addr8bit, write_buffer, 1);
PedroMartins96 0:a7324f51348d 220 wait_us(100);
PedroMartins96 0:a7324f51348d 221 i2c.read( addr8bit, read_buffer, 4);
PedroMartins96 0:a7324f51348d 222 countsLeft = (int16_t((read_buffer[0]<<8)|read_buffer[1]));
PedroMartins96 0:a7324f51348d 223 countsRight = (int16_t((read_buffer[2]<<8)|read_buffer[3]));
PedroMartins96 0:a7324f51348d 224 }