Diff: main.cpp
- Revision:
- 0:0d3a25d4697e
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp Thu May 27 08:53:19 2021 +0000
@@ -0,0 +1,512 @@
+#include "mbed.h"
+#include "BufferedSerial.h"
+#include "rplidar.h"
+#include "Robot.h"
+#include "Communication.h"
+
+#define D_EIXO 14.05 // Distancia entre rodas
+#define D_RODA 7.0 // Raio da roda ( cm )
+#define KV 10.0 //uperior a zero
+#define KS 70.0 //superior a KV
+#define KI 0.05 // superior a KV
+#define D_ERRO 0.5 // cm
+#define MAX_SPEED 70.0
+#define MIN_MAX_SPEED 70.0
+#define MIN_SPEED 50.0
+#define DIST_STOP 8.0
+
+// Mapa
+#define D_QUAD 5.0 // tamanho do quadrado ... cm
+#define D_JANELA 70.0 // tamanho da janela ... cm (tem de ser multiplo impar de D_QUAD)
+#define D_MAPAX 200.0 // comprimento mapa em x ... cm
+#define D_MAPAY 200.0 // comprimento mapa em y ... cm
+#define MAPBUFFER 5.0 // buffer de 5cm para cada direcao
+
+double PI_val = 3.14159265358979323846f;
+
+// Configuracao GPIO
+DigitalIn mybutton(USER_BUTTON);
+
+// Configuracao comunicacao
+Serial bt(PA_0, PA_1, 38400); // Bluetooth
+Serial pc(SERIAL_TX, SERIAL_RX, 115200); // USB
+
+// Lidar
+RPLidar lidar;
+BufferedSerial se_lidar(PA_9, PA_10);
+PwmOut rplidar_motor(D3);
+struct RPLidarMeasurement data;
+double lidar_angle, lidar_dist;
+double lidar_distMin = 7.5f;
+double lidar_distMax = 200.0f;
+double offSetLidarX = -2.8, offSetLidarY = -1.5, lidar_angRet = 1.1*PI_val/180.0;
+double lidar_angleCs, lidar_angleSn, tMaxX, tMaxY, tDeltaX, tDeltaY;
+int stepX, stepY;
+double angDif, posDifX, posDifY;
+
+
+// Dados a modificar
+double pos_x[] = {30.0f,130.0f,30.0f,30.0f};
+double pos_y[] = {30.0f,130.0f,130.0f,30.0f};
+double THETA_Start = 0.0f;
+
+// Periodos
+int T_printOdo = 250; int flag_printOdo = 1; // Periodo para print da odometria
+int T_upOdo = 10; // Periodo para update dos Encoders
+
+// Variaveis Globais
+float X = 0.0f;
+float Y = 0.0f;
+float THETA = 0.0f;
+double delta_D = 0.0f;
+double delta_TH = 0.0f;
+double goal_x, goal_y;
+double v, w, phi, racio, df, delta_theta;
+double DR_cm, DL_cm;
+int lastEncR = -1, lastEncL = -1, cont, pos_cont = 1;
+int w_e, w_d, dir_e, dir_d, sw_e, sw_d;
+int size_pos;
+bool running = true;
+double thetaCs, thetaSn;
+
+// Mapa
+double fca = 30; double fcr = 2000;
+float mapa[42][42];
+int t_mapax = floor((D_MAPAX + MAPBUFFER*2.0)/D_QUAD);
+int t_mapay = floor((D_MAPAY + MAPBUFFER*2.0)/D_QUAD);
+int xa, ya, x_janela, y_janela;
+double frx, fry, da, delta_dax, delta_day, T_JANELA = D_JANELA/D_QUAD; // tamanho da janela em quadrados (janela) ;
+double delta_fx, delta_fy, fax, fay, Fx, Fy, x_prox, y_prox;
+double erro, erro_int = 0;
+int X_ind, Y_ind;
+double exp_mapa;
+
+// Obstaculos
+double obstX, obstY;
+int obstX_ind, obstY_ind;
+
+// Tempos
+uint32_t last_printOdo = 0, now_time = 0; // Referencia do ultimo print
+uint32_t last_upOdo = 0, t_control = 0; // Referencia do ultimo print
+uint32_t last_upMap = 0, t_map = 0; // Referencia do ultimo updateMap
+uint32_t start_upLid = 0; // Referencia do ultimo print
+uint32_t last_printMap =0;
+Timer t; // Variavel print
+
+// Declaracao de funcoes
+void start_system();
+void lidar_setUp();
+int check_enc();
+void wait_user();
+void print_odo();
+void update_goal();
+void update_odo();
+void update_map(int, int);
+void update_control();
+void update_vel(double v, double w);
+void update_pos();
+void check_error();
+void shutdown_system();
+void print_mapa();
+
+int main(){
+ size_pos = sizeof(pos_x)/sizeof(pos_x[0]);
+ const double v = 175;
+ const double w = 4.5;
+
+ start_system();
+ update_vel(v,w);
+
+ /*while(running) {
+ now_time = t.read_ms();
+ t_control = now_time - last_upOdo;
+ if( t_control >= T_upOdo){
+ if(check_enc()){
+ update_odo();
+ update_pos();
+ update_map(2, 2);
+ update_control();
+ update_vel();
+ check_error();
+ last_upOdo = now_time;
+ }
+ }
+
+ now_time = t.read_ms();
+ if((now_time - last_printOdo >= T_printOdo) && (flag_printOdo)){
+ print_odo();
+ last_printOdo = now_time;
+ }
+ }*/
+ //shutdown_system();
+}
+
+void update_goal(){
+ goal_x = pos_x[pos_cont] + MAPBUFFER;
+ goal_y = pos_y[pos_cont] + MAPBUFFER;
+ pos_cont++;
+}
+
+void start_system(){
+ init_communication(&pc);
+
+ X = pos_x[0] + MAPBUFFER;
+ Y = pos_y[0] + MAPBUFFER;
+ THETA = THETA_Start;
+
+ update_goal();
+
+ pc.printf("Robo Speed to Zero...\n");
+ setSpeeds(0, 0);
+ pc.printf(" Done!\n");
+
+ pc.printf("Initiating Lidar...\n");
+ lidar_setUp();
+ pc.printf(" Done!\n");
+
+ wait_user();
+
+ pc.printf("START\n");
+ pc.printf("LAB3\n");
+ pc.printf("VFF\n");
+
+ char str[100], str2[10];
+ for(int posInt = 1; posInt < size_pos; posInt++){
+ sprintf(str2,"/%.2f/%2.f", pos_x[posInt], pos_y[posInt]);
+ strcat(str, str2);
+ }
+ pc.printf("%.2f/%.2f/%.2f%s/%.2f/%.2f/%.2f/%.2f/%.2f/%.2f \n", pos_x[0], pos_y[0], THETA_Start, str, KV, KS, KI, DIST_STOP, fca, fcr);
+
+ pc.printf("BEGIN\n");
+
+ pc.printf("Lidar Speed to 100%%...\n");
+ //rplidar_motor.write(0.35f);
+ //pc.printf(" Done!\n");
+
+ pc.printf("Stabilizing for 0.7 Second...\n");
+ wait(0.7);
+ pc.printf(" Done!\n");
+
+ pc.printf("Reseting Encoders...\n");
+ getCountsAndReset();
+ pc.printf(" Done!\n");
+
+ print_odo();
+
+ t.start();
+
+ pc.printf("Updating Map with 250 Measures...\n");
+ last_upMap = t.read_ms();
+ //update_map(2, 700);
+ t_map = t.read_ms() - last_upMap;
+ pc.printf(" Done! %f\n", t_map/250.0);
+}
+
+void lidar_setUp(){
+ // Lidar initialization
+
+ rplidar_motor.period(0.01f);
+ lidar.begin(se_lidar);
+ lidar.setAngle(0,360);
+ lidar.startThreadScan();
+ rplidar_motor.write(0.0f);
+}
+
+int check_enc(){
+
+ getCountsAndReset(); // Obtem encoder e reset encoder
+ //pc.printf("%d %d ", countsRight, countsLeft);
+ if(!(abs(countsRight) == 1 && countsLeft == 255)){
+ lastEncR = countsRight;
+ lastEncL = countsLeft;
+ //pc.printf("|1| %d %d\n\r", countsRight, countsLeft);
+ return 1;
+ }else{
+ if(lastEncR == -1 && lastEncL == -1){
+ //pc.printf("|2| %d %d\n\r", countsRight, countsLeft);
+ return 0;
+ }else{
+ countsRight = lastEncR;
+ countsLeft = lastEncL;
+ //pc.printf("|3| %d %d\n\r", countsRight, countsLeft);
+ return 1;
+ }
+ }
+ }
+
+void update_odo(){
+
+ DR_cm = (double)countsRight * ((D_RODA*PI_val)/1440.0);
+ DL_cm = (double)countsLeft * ((D_RODA*PI_val)/1440.0);
+
+ delta_D = (DR_cm + DL_cm)/2.0;
+ delta_TH = (DR_cm - DL_cm)/D_EIXO;
+ delta_TH = atan2(sin(delta_TH), cos(delta_TH));
+}
+
+void update_pos(){
+ if(delta_TH == 0){
+ X = X + delta_D * cos(THETA);
+ Y = Y + delta_D * sin(THETA);
+ }else{
+ X = X + delta_D*sin(delta_TH/2.0)*cos(THETA+delta_TH/2.0)/(delta_TH/2.0);
+ Y = Y + delta_D*sin(delta_TH/2.0)*sin(THETA+delta_TH/2.0)/(delta_TH/2.0);
+ THETA += delta_TH;
+ THETA = atan2(sin(THETA), cos(THETA));
+ }
+}
+
+void update_map(int update_mode, int qnt){
+ // update_mode = 1 -> Timer
+ // update_mode = 2 -> Contador
+
+ X_ind = (int)floor(X/D_QUAD); // Indice célula posicao X
+ Y_ind = (int)floor(Y/D_QUAD); // Indice célula posicao Y
+
+ thetaCs = cos(THETA);
+ thetaSn = sin(THETA);
+ mapa[X_ind][Y_ind] -= 0.65; // Posição inicial está livre
+
+ cont = 0;
+ start_upLid = t.read_ms();
+ while((cont < qnt)){
+ if(lidar.pollSensorData(&data)==0){
+ X_ind = (int)floor(X/D_QUAD); // Indice célula posicao X
+ Y_ind = (int)floor(Y/D_QUAD); // Indice célula posicao Y
+
+ lidar_dist = (double) data.distance/10.0;
+ lidar_angle = (double) data.angle;
+ if((lidar_dist > lidar_distMin) && (lidar_dist < lidar_distMax)){
+
+ lidar_angle = lidar_angle*PI_val/180.0 ;
+
+ obstX = X + offSetLidarX*thetaCs - offSetLidarY*thetaSn - lidar_dist*sin(lidar_angRet - THETA + lidar_angle);
+ obstY = Y + offSetLidarY*thetaCs + offSetLidarX*thetaSn - lidar_dist*cos(lidar_angRet - THETA + lidar_angle);
+
+ posDifX = obstX - X;
+ posDifY = obstY - Y;
+ angDif = atan2(posDifY, posDifX);
+ lidar_angleCs = cos(angDif);
+ lidar_angleSn = sin(angDif);
+
+ obstX_ind = (int)floor(obstX/D_QUAD);
+ obstY_ind = (int)floor(obstY/D_QUAD);
+
+ if(obstX < X){
+ stepX = -1;
+ }else{
+ stepX = 1;
+ }
+ if(obstY < Y){
+ stepY = -1;
+ }else{
+ stepY = 1;
+ }
+
+ tMaxX = X;
+ tMaxY = Y;
+
+ while(1){
+ // tDeltaX = (abs(((X_ind+1*(stepX>0))*D_QUAD) - tMaxX))/lidar_angleCs;
+ // tDeltaY = (abs(((Y_ind+1*(stepY>0))*D_QUAD) - tMaxY))/lidar_angleSn;
+
+ tDeltaX = (X_ind+1*(stepX>0))*D_QUAD;
+ tDeltaY = (Y_ind+1*(stepY>0))*D_QUAD;
+ tDeltaX = abs(tDeltaX - tMaxX);
+ tDeltaY = abs(tDeltaY - tMaxY);
+ tDeltaX = (tDeltaX)/lidar_angleCs;
+ tDeltaY = (tDeltaY)/lidar_angleSn;
+
+ if(abs(tDeltaX) < abs(tDeltaY)){
+ X_ind += stepX;
+ tMaxX = (X_ind+1*(stepX<0))*D_QUAD;
+ tMaxY += tDeltaX*lidar_angleSn*stepX;
+ }else{
+ Y_ind += stepY;
+ tMaxY = (Y_ind+1*(stepY<0))*D_QUAD;
+ tMaxX += tDeltaY*lidar_angleCs*stepY;
+ }
+ //pc.printf("%d %d\n\r", X_ind, Y_ind);
+ if((X_ind >= 0) && (X_ind < t_mapax) && (Y_ind >= 0) && (Y_ind < t_mapay)){
+ if(X_ind == obstX_ind && Y_ind == obstY_ind){
+ mapa[X_ind][Y_ind] += 0.65;
+ //pc.printf("break\n\r");
+ break;
+ //}else if(X_ind == 0 || Y_ind == 0 || X_ind == t_mapax-1 || Y_ind == t_mapay-1 ){
+ // mapa[X_ind][Y_ind] += 0.65;
+ }else{
+ //pc.printf("livre\n\r");
+ mapa[X_ind][Y_ind] -= 0.65;
+ }
+ }else{
+ break;
+ }
+ }
+ //pc.printf("%d\n",cont);
+ cont++;
+ }
+ }
+ }
+}
+
+void update_control(){
+ xa = (int)floor(X/D_QUAD); // Indice célula posicao X
+ ya = (int)floor(Y/D_QUAD); // Indice célula posicao Y
+
+ frx = 0; fry = 0;
+ x_janela = xa - floor(T_JANELA/2.0);
+ for(int i = 0; i < T_JANELA; i++){
+ y_janela = ya - floor(T_JANELA/2.0);
+ for(int j = 0; j < T_JANELA; j++){
+
+ if((x_janela >= 0) && (x_janela < t_mapax) && (y_janela >= 0) && (y_janela < t_mapay)){
+
+ delta_dax = x_janela*D_QUAD;
+ delta_day = y_janela*D_QUAD;
+
+ if(X >= delta_dax+D_QUAD){
+ delta_dax = delta_dax+D_QUAD-X;
+ }else{
+ delta_dax = delta_dax-X;
+ }
+
+ if(Y >= delta_day+D_QUAD){
+ delta_day = delta_day+D_QUAD-Y;
+ }else{
+ delta_day = delta_day-Y;
+ }
+
+ if(delta_dax == 0){
+ delta_dax = 0.1;
+ }
+ if(delta_day == 0){
+ delta_day = 0.1;
+ }
+
+ exp_mapa = 1-(1/(1+exp(mapa[x_janela][y_janela])));
+ if(exp_mapa <= 0.5){
+ exp_mapa = 0;
+ }
+
+ da = sqrt(pow(delta_dax,2)+pow(delta_day,2));
+ frx += fcr*exp_mapa*delta_dax/pow(da,3);
+ fry += fcr*exp_mapa*delta_day/pow(da,3);
+ }
+ y_janela ++;
+ }
+ x_janela ++;
+ }
+
+ delta_fx = goal_x-X;
+ delta_fy = goal_y-Y;
+
+ df = sqrt(pow(delta_fx,2)+pow(delta_fy,2));
+
+ fax = fca*delta_fx/df;
+ fay = fca*delta_fy/df;
+
+ Fx = fax - frx;
+ Fy = fay - fry;
+
+ x_prox = X + Fx;
+ y_prox = Y + Fy;
+
+ erro = sqrt(pow(x_prox-X,2)+pow(y_prox-Y,2))-D_ERRO;
+ erro_int = erro_int + erro;
+
+ phi = atan2((y_prox-Y), (x_prox-X));
+ delta_theta = atan2(sin(phi - THETA), cos(phi - THETA));
+
+ v = KV*erro+KI*erro_int;
+ w = KS*delta_theta;
+}
+
+void update_vel(double v, double w){
+
+ w_e = (v-(D_EIXO/2.0)*w)/(D_RODA/2.0);
+ w_d = (v+(D_EIXO/2.0)*w)/(D_RODA/2.0);
+
+ sw_e = w_e;
+ sw_d = w_d;
+
+ if(abs(w_e) > MAX_SPEED || abs(w_d) > MAX_SPEED){
+ racio = fabs((double)w_e/w_d);
+
+ if( w_e < 0 ) dir_e = -1; else dir_e = 1;
+ if( w_d < 0 ) dir_d = -1; else dir_d = 1;
+
+ if(fabs(racio) >= 1.0){
+ sw_e = dir_e*MAX_SPEED;
+ sw_d = dir_d*MAX_SPEED/racio;
+ }else{
+ sw_e = dir_e*MAX_SPEED*racio;
+ sw_d = dir_d*MAX_SPEED;
+ }
+ }
+ if(abs(w_e) < MIN_SPEED || abs(w_d) < MIN_SPEED){
+ racio = fabs((double)w_e/w_d);
+
+ if( w_e < 0 ) dir_e = -1; else dir_e = 1;
+ if( w_d < 0 ) dir_d = -1; else dir_d = 1;
+
+ if(fabs(racio) >= 1.0){
+ if(MIN_SPEED*racio <= MIN_MAX_SPEED){
+ sw_e = dir_e*MIN_SPEED*racio;
+ sw_d = dir_d*MIN_SPEED;
+ }
+ }else{
+ if(MIN_SPEED/racio <= MIN_MAX_SPEED){
+ sw_e = dir_e*MIN_SPEED;
+ sw_d = dir_d*MIN_SPEED/racio;
+ }
+ }
+ }
+
+ setSpeeds(sw_e, sw_d);
+}
+
+void check_error(){
+ if(df > DIST_STOP){
+ running = true;
+ }else{
+ if(pos_cont < size_pos){
+ //pc.printf("Goal Completed!\n");
+ //pc.printf("Changing Goal...\n");
+ update_goal();
+ //pc.printf(" Done!\n");
+ erro_int = 0;
+ running = true;
+ }else{
+ running = false;
+ }
+ }
+}
+
+void print_mapa(){
+ for( int xx = 0; xx < 42; xx++){
+ for( int yy = 0; yy < 42; yy++){
+ pc.printf("%d/%d/%.2f \n",xx,yy,mapa[xx][yy]);
+ wait(0.0001);
+ }
+ }
+}
+
+void print_odo(){
+ pc.printf("%.2f/%.2f/%.2f/%.2f/%.2f \n", X - MAPBUFFER, Y - MAPBUFFER, THETA, x_prox - MAPBUFFER, y_prox - MAPBUFFER);
+}
+
+void wait_user(){
+ pc.printf("Press UserButton to continue...\n");
+ while(mybutton); // Espera por butao USER ser primido
+ pc.printf(" Done!\n");
+}
+
+void shutdown_system(){
+ setSpeeds(0,0);
+
+ rplidar_motor.write(0.0f);
+
+ print_mapa();
+ pc.printf("END!\n");
+ t.stop();
+}