Francisco Santos
pepe
Diff: Robot.cpp
- Revision:
- 0:a7324f51348d
- Child:
- 1:2716ea33958b
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/Robot.cpp Wed Mar 13 23:04:14 2019 +0000
@@ -0,0 +1,224 @@
+#include "Robot.h"
+#include "mbed.h"
+
+Serial PC(SERIAL_TX, SERIAL_RX);
+I2C i2c(I2C_SDA, I2C_SCL );
+const int addr8bit = 20 << 1; // 7bit I2C address is 20; 8bit I2C address is 40 (decimal).
+
+int16_t countsLeft = 0;
+int16_t countsRight = 0;
+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;
+
+
+////////////////////////////////////////
+// Movimento para uma pose arbitrária //
+////////////////////////////////////////
+void to_pos(float x2,float y2,float phi2)
+{
+ while(1){
+ float alfa,beta,a=33,b=30;
+ float Kp=0.7,Ka=5,Kb=-2;
+ while(abs(x)<abs(x2)-1 || abs(y)<abs(y2)-1 || teta<=(phi2-0.05) || teta>=(phi2+0.05)) {// Condição de paragem
+ // Controlo linear
+ v=Kp*(sqrt(((x2-x)*(x2-x)+(y2-y)*(y2-y))));
+
+ alfa=-teta+atan2(y2-y,x2-x);
+
+ if(abs(v)<1){
+ alfa=0;
+ a=0;
+ b=0;
+ }
+ // Verifica se o alpha pertence ao limite entre [-pi/2; pi/2], se não adiciona/remove pi e inverte a velocidade
+ if(alfa<-pi/2){
+ alfa=alfa+pi;
+ v=-v;
+ }
+ else if(alfa>pi/2){
+ alfa=alfa-pi;
+ v=-v;
+ }
+
+ beta=-teta-alfa+phi2;
+ w=Ka*alfa+Kb*beta;
+
+ if(alfa==0){
+ w=w+abs(w)/w*4;
+ Kb=3;
+ }
+
+ motion();
+ setSpeeds(Wee+a*abs(v)/v,Wdd+b*abs(v)/v);
+
+ //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);
+ PC.printf("%f %f\n", x,y);
+ }
+ setSpeeds(0,0); // A condição de paragem verifica-se e para-se o robo
+ }
+}
+
+
+/////////////////////////////////
+// Movimento segundo uma linha //
+/////////////////////////////////
+void to_line(float a,float b,float c)
+{
+ float d=0,alfad=0,phi1=0,alfah=0;
+ float Kd=0.45,Kh=15;
+ while(1){
+ while((abs(x) < 200)&&(abs(y)<200)) {
+ v=100;
+ d=(a*x+b*y+c)/sqrt(a*a+b*b);
+
+ //Controlo proporcional que roda a plataforma na direção da linha
+ alfad=(-Kd)*d;
+ if (abs(alfad)<0.3) alfad=alfad*3;
+ phi1=atan2(-a,b);
+
+ //Controlo proporcional que ajusta o ângulo de direcionamento
+ alfah=Kh*(phi1-teta);
+
+ w=alfad+alfah;
+
+ if(w > 12) w = 12;
+ else if(w<-12) w = -12;
+
+ motion();
+ setSpeeds(Wee,Wdd);
+ //PC.printf("v=%f w=%f alfad=%f alfah=%f teta=%f x=%f y=%f\n", v,w,alfad,alfah,teta,x,y);
+ PC.printf("%f %f\n", x,y);
+ }
+ setSpeeds(0,0);
+ }
+}
+
+
+/////////////////////////////
+// Movimento para um ponto //
+/////////////////////////////
+void to_point(float x1,float y1)
+{
+ while(1) {
+ float phi1=0,aux=0;
+ float Kv=1.5,Ks=30;
+ while((abs(x)<abs(x1)-1)||(abs(y)<abs(y1)-1)||(abs(x)>abs(x1)+1)||(abs(y)>abs(y1)+1)) {
+ 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
+
+ phi1=atan2((y1-y),(x1-x)); // Ângulo entre a posição final e a posição atual
+
+ aux=phi1-teta;
+ //Verifica se a diferença dos angulos pertence ao limite entre [-pi;pi]
+ if(aux<-pi){
+ aux=aux+2*pi;
+ }
+ else if(aux>pi){
+ aux=aux-2*pi;
+ }
+
+ //Controlo proporcional
+ w=Ks*(aux);
+
+ if(w > 12) w = 12;
+ else if(w<-12) w = -12;
+
+ motion();
+ setSpeeds(Wee+28,Wdd+25);
+ //PC.printf("v=%f w=%f phi1=%f x=%f y=%f teta=%f\n", v,w,phi1,x,y,teta);
+ PC.printf("%f %f %f %f\n", x,y,teta, w);
+ }
+ setSpeeds(0,0);
+ }
+
+}
+
+///////////////
+// Odometria //
+///////////////
+void motion()
+{
+ getCountsAndReset();
+ De=((2*pi*r)*countsLeft)/1440;
+ Dd=((2*pi*r)*countsRight)/1440;
+ theta=(Dd-De)/Ldis;
+
+ D=(De+Dd)/2;
+
+ if(theta==0) {
+ x=x+D*cos(teta);
+ y=y+D*sin(teta);
+ teta=teta;
+ } else {
+ x = x + D*((sin(theta/2)/(theta/2))*cos(teta+theta/2));
+ y = y + D*((sin(theta/2)/(theta/2))*sin(teta+theta/2));
+ teta=teta+theta;
+ }
+
+ //Limite entre [-2pi; 2pi]
+ if (teta>=(2*pi)) {
+ teta=teta-2*pi;
+ }
+ if (teta<=(-2*pi)) {
+ teta=teta+2*pi;
+ }
+
+ Wee=(v-(Ldis/2)*w);
+ Wdd=(v+(Ldis/2)*w);
+ //PC.printf("phi1=%f D=%f x=%f y=%f teta=%f\n", w,theta,x,y,teta);
+}
+
+void setSpeeds(int16_t leftSpeed, int16_t rightSpeed)
+{
+ char buffer[5];
+
+ buffer[0] = 0xA1;
+ memcpy(&buffer[1], &leftSpeed, sizeof(leftSpeed));
+ memcpy(&buffer[3], &rightSpeed, sizeof(rightSpeed));
+
+ i2c.write(addr8bit, buffer, 5); // 5 bytes
+}
+
+void setLeftSpeed(int16_t speed)
+{
+ char buffer[3];
+
+ buffer[0] = 0xA2;
+ memcpy(&buffer[1], &speed, sizeof(speed));
+
+ i2c.write(addr8bit, buffer, 3); // 3 bytes
+}
+
+void setRightSpeed(int16_t speed)
+{
+ char buffer[3];
+
+ buffer[0] = 0xA3;
+ memcpy(&buffer[1], &speed, sizeof(speed));
+
+ i2c.write(addr8bit, buffer, 3); // 3 bytes
+}
+
+void getCounts()
+{
+ char write_buffer[2];
+ char read_buffer[4];
+
+ write_buffer[0] = 0xA0;
+ i2c.write(addr8bit, write_buffer, 1);
+ wait_us(100);
+ i2c.read( addr8bit, read_buffer, 4);
+ countsLeft = (int16_t((read_buffer[0]<<8)|read_buffer[1]));
+ countsRight = (int16_t((read_buffer[2]<<8)|read_buffer[3]));
+}
+
+void getCountsAndReset()
+{
+ char write_buffer[2];
+ char read_buffer[4];
+
+ write_buffer[0] = 0xA4;
+ i2c.write(addr8bit, write_buffer, 1);
+ wait_us(100);
+ i2c.read( addr8bit, read_buffer, 4);
+ countsLeft = (int16_t((read_buffer[0]<<8)|read_buffer[1]));
+ countsRight = (int16_t((read_buffer[2]<<8)|read_buffer[3]));
+}
\ No newline at end of file