Diff: main.cpp
- Revision:
- 3:892592b2c2ea
- Parent:
- 2:faef6636d456
- Child:
- 4:0fac5e5d08f7
--- a/main.cpp Wed May 05 14:14:28 2021 +0000
+++ b/main.cpp Mon May 10 13:20:10 2021 +0000
@@ -1,17 +1,150 @@
-// Coded by Luís Afonso 11-04-2019
#include "mbed.h"
#include "BufferedSerial.h"
#include "rplidar.h"
#include "Robot.h"
#include "Communication.h"
+#include <math.h>
+#include <time.h>
+
+#define M_PI 3.14159265358979323846
Serial pc(SERIAL_TX, SERIAL_RX);
RPLidar lidar;
BufferedSerial se_lidar(PA_9, PA_10);
PwmOut rplidar_motor(D3);
-int main()
-{
+clock_t start, end;
+
+int nW=2;
+float wTheta[2] = {0, 0};
+float wRad[2] = {3.5, 3.5};
+float wWid[2] = {1.5, 1.5};
+float wheelsRadius = 3.5;
+float wheelsDistance = 14.7;
+
+//Platform Properties
+float platRad=18/2;
+
+//Constante de Tempo
+double T = 0;
+
+//Definição do Mapa
+float Mapa200[200][200] = {0.5};
+float Mapa40[40][40]={0.5};
+int log_mapa[40][40]={0};
+
+
+//Size Map40
+int X = ceil(200/5);
+int Y = ceil(200/5);
+
+/*
+%Parede da esquerda
+for(int i = 0; i <= 200; i++){
+ Mapa200[i][0] = {1};
+ Mapa200[i][1] = {1};
+ Mapa200[i][2] = {1};
+}
+
+%Parede da direita
+for(int i = 0; i <= 200; i++){
+ Mapa200[i][199] = {1};
+ Mapa200[i][198] = {1};
+ Mapa200[i][197] = {1};
+}
+
+%Parede de cima
+for(int i = 0; i <= 200; i++){
+ Mapa200[0][i] = {1};
+ Mapa200[1][i] = {1};
+ Mapa200[2][i] = {1};
+}
+
+%Parede de baixo
+for(int i = 0; i <= 200; i++){
+ Mapa200[199][i] = {1};
+ Mapa200[198][i] = {1};
+ Mapa200[197][i] = {1};
+}*/
+
+float pose[3][1];
+float pose_f[3][1];
+float PointDetected[2][1];
+
+//posicao inicial
+float xa = 0;
+float ya = 150;
+float rPos_a={{xa}, {ya}};
+float rTheta_a = 0;
+
+//posicao final
+float xf = 50;
+float yf = 150;
+float rTheta_f=0;
+float rPos_a={{xf}, {yf}};
+
+//Waypoints
+int waypoints = 1;
+float Wx[1] = {25};
+float Wy[1] = {125};
+
+//Inicialização do robô
+Robot Robot;
+robot(Robot, rPos, rTheta, nW, wPos, wTheta, wRad, wWid, platRad);
+
+//Calculo pose inicial
+getPose(Robot, pose);
+
+//------------VFF------------
+int fcr = 460; //Intensidade da força repulsiva
+int fca = 20; //Intensidade da força atrativa
+int KS = 25;
+int KV = 3;
+int KI = 0.05;
+float erro_int = 0;
+float erro = 0;
+
+float vRobot=0;
+float wRobot=0;
+
+float w = 0;
+
+float phi = 0;
+
+float dt = 0;
+float delta_fx = 0;
+float delta_fy = 0;
+
+//Força Repulsiva
+float fax = 0;
+float fay = 0;
+
+//Força Resultante
+float frx = 0;
+float fry = 0;
+float Fx = 0;
+float Fy = 0;
+
+float i_aux = 0;
+float j_aux = 0;
+float dij = 0;
+float F = 0;
+float angulo = 0;
+
+float x_prox = 0;
+float y_prox = 0;
+
+float delta_theta = 0;
+
+/*// poll for measurements. Returns -1 if no new measurements are available. returns 0 if found one.
+ if(lidar.pollSensorData(&data) == 0) {
+ pc.printf("%f\t%f\t%d\t%c\n", data.distance, data.angle, data.quality, data.startBit); // Prints one lidar measurement.
+ }
+
+ wait(0.01);
+*/
+
+int main(){
struct RPLidarMeasurement data;
pc.baud(115200);
@@ -19,19 +152,122 @@
// Lidar initialization
rplidar_motor.period(0.0005f);
+ rplidar_motor.write(0.5f);
lidar.begin(se_lidar);
lidar.setAngle(0,360);
+ setSpeeds(50,50);
pc.printf("Program started.\n");
-
+
lidar.startThreadScan();
-
+
+ //Scan for 3s the environment
+ while (T < 3){
+ start = clock();
+ if(lidar.pollSensorData(&data) == 0) {
+ //pc.printf("%f\t%f\t%d\t%c\n", data.distance, data.angle, data.quality, data.startBit); // Prints one lidar measurement.
+ ComputeLidarPoint(pose, PointDetected, data.distance, data.angle, float theta)
+ occupancy_grid_mapping(log_mapa, Mapa40, d, pose[2], xa, ya, theta, X_grelha, Y_grelha);
+ }
+ end = clock();
+ T = ((double) (end - start)) / CLOCKS_PER_SEC;
+ }
+
while(1) {
- // poll for measurements. Returns -1 if no new measurements are available. returns 0 if found one.
- if(lidar.pollSensorData(&data) == 0)
- {
- pc.printf("%f\t%f\t%d\t%c\n", data.distance, data.angle, data.quality, data.startBit); // Prints one lidar measurement.
+ for(int way = 0; way <= waypoints+1; way++){
+ pc.printf("Waypoint %d\n", way);
+
+ if (way < waypoints+1){
+ //Vai adicionando os waypoints como pontos finais
+ Robot_f = robot([{Wx[way]}, {Wy[way]}],rTheta_f,nW,wPos,wTheta,wRad,wWid,platRad);
+ getPose(Robot,pose_f);
+ df = sqrt((pose_f[1]-pose[1])^2 + (pose_f[2]-pose[2])^2);
+ }
+ elseif (way == waypoints+1){
+ //Vai adicionar o ponto final proposto pelo o utilizador
+ Robot_f = robot([{rPos_f[1]}, {rPos_f[2]}],rTheta_f,nW,wPos,wTheta,wRad,wWid,platRad);
+ getPose(Robot,pose_f);
+ df = sqrt((pose_f[1]-pose[1])^2 + (pose_f[2]-pose[2])^2);
+ }
+
+ while (df > 5){
+ start = clock();
+ velRobot2velWheels(vRobot, wRobot, wheelsRadius, wheelsDistance, w);
+ nextPose(T, w, wRobot, pose, wheelsRadius, wheelsDistance, pose);
+
+ //FAZER CALCULO DO ON DEMAND
+
+
+ //Força Atrativa
+ delta_fx=pose_f[0]-pose[0];
+ delta_fy=pose_f[1]-pose[1];
+
+ df=sqrt(delta_fx^2 + delta_fy^2);
+
+ fax=fca*(delta_fx)/df;
+ fay=fca*(delta_fy)/df;
+
+ //Força Repulsiva
+ frx=0;
+ fry=0;
+
+ for(int j = xa-40; j <= xa+40; j++){
+ for(int i = ya-40; i <= ya+40; i++){
+ if((i > 0) && (i <= Y) && (j > 0) && (j <= X)){
+ if(Mapa40[i][j] ~= 0){
+ //Passar para coordenas cartesianas
+ i_aux=(i-1)*5+2.5;
+ j_aux=(j-1)*5+2.5;
+
+ dij=sqrt(((i_aux-pose[0])^2+(j_aux-pose[1])^2));
+
+ F=(fcr*Mapa40(i,j)/(dij^2));
+ angulo=atan2(j_aux-pose[1],i_aux-pose[0]);
+
+ frx=frx+F*cos(angulo);
+ fry=fry+F*sin(angulo);
+
+ }
+ }
+ }
+ }
+
+ //Força Resultante
+ Fx=fax-frx;
+ Fy=fay-fry;
+
+ //Posição seguinte
+ x_prox=pose[0]+Fx;
+ y_prox=pose[1]+Fy;
+
+ //Calculo do Erro
+ erro=sqrt((x_prox-pose[0])^2 + (y_prox-pose[1])^2) - 2;
+ erro_int=erro_int + erro;
+
+ //Calculo Velocidade
+ vRobot=KV*erro+KI*erro_int;
+
+ if (vRobot > 50){
+ vRobot = 50;
+ }
+
+ //Calculo Phi
+ phi=atan2(Fy,Fx);
+ phi=atan2(sin(phi),cos(phi));
+
+ delta_theta=atan2(sin(phi-pose[2]),cos(phi-pose[2]));
+
+ //Velocidade Angular
+ wRobot=KS*delta_theta;
+
+ //Colocar as velocidades no Robot
+ setPose(Robot, pose);
+
+ //Calcula o tempo que recorreu
+ end = clock();
+ T = ((double) (end - start)) / CLOCKS_PER_SEC;
+ }
}
- wait(0.01);
+ pc.printf("Chegou ao ponto final!\n");
}
-}
+}
\ No newline at end of file