Francisco Santos
/
SRA_VFF
pepe
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?
User | Revision | Line number | New 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 | } |