João Pedro Castilho / Mbed OS Lab4

Dependencies:   BufferedSerial

main.cpp@0:0d3a25d4697e, 2021-05-27 (annotated)

Committer:
ccpjboss
Date:
Thu May 27 08:53:19 2021 +0000
Revision:
0:0d3a25d4697e
tet

Who changed what in which revision?

UserRevisionLine numberNew contents of line
ccpjboss 0:0d3a25d4697e 1 #include "mbed.h"
ccpjboss 0:0d3a25d4697e 2 #include "BufferedSerial.h"
ccpjboss 0:0d3a25d4697e 3 #include "rplidar.h"
ccpjboss 0:0d3a25d4697e 4 #include "Robot.h"
ccpjboss 0:0d3a25d4697e 5 #include "Communication.h"
ccpjboss 0:0d3a25d4697e 6
ccpjboss 0:0d3a25d4697e 7 #define D_EIXO 14.05 // Distancia entre rodas
ccpjboss 0:0d3a25d4697e 8 #define D_RODA 7.0 // Raio da roda ( cm )
ccpjboss 0:0d3a25d4697e 9 #define KV 10.0 //uperior a zero
ccpjboss 0:0d3a25d4697e 10 #define KS 70.0 //superior a KV
ccpjboss 0:0d3a25d4697e 11 #define KI 0.05 // superior a KV
ccpjboss 0:0d3a25d4697e 12 #define D_ERRO 0.5 // cm
ccpjboss 0:0d3a25d4697e 13 #define MAX_SPEED 70.0
ccpjboss 0:0d3a25d4697e 14 #define MIN_MAX_SPEED 70.0
ccpjboss 0:0d3a25d4697e 15 #define MIN_SPEED 50.0
ccpjboss 0:0d3a25d4697e 16 #define DIST_STOP 8.0
ccpjboss 0:0d3a25d4697e 17
ccpjboss 0:0d3a25d4697e 18 // Mapa
ccpjboss 0:0d3a25d4697e 19 #define D_QUAD 5.0 // tamanho do quadrado ... cm
ccpjboss 0:0d3a25d4697e 20 #define D_JANELA 70.0 // tamanho da janela ... cm (tem de ser multiplo impar de D_QUAD)
ccpjboss 0:0d3a25d4697e 21 #define D_MAPAX 200.0 // comprimento mapa em x ... cm
ccpjboss 0:0d3a25d4697e 22 #define D_MAPAY 200.0 // comprimento mapa em y ... cm
ccpjboss 0:0d3a25d4697e 23 #define MAPBUFFER 5.0 // buffer de 5cm para cada direcao
ccpjboss 0:0d3a25d4697e 24
ccpjboss 0:0d3a25d4697e 25 double PI_val = 3.14159265358979323846f;
ccpjboss 0:0d3a25d4697e 26
ccpjboss 0:0d3a25d4697e 27 // Configuracao GPIO
ccpjboss 0:0d3a25d4697e 28 DigitalIn mybutton(USER_BUTTON);
ccpjboss 0:0d3a25d4697e 29
ccpjboss 0:0d3a25d4697e 30 // Configuracao comunicacao
ccpjboss 0:0d3a25d4697e 31 Serial bt(PA_0, PA_1, 38400); // Bluetooth
ccpjboss 0:0d3a25d4697e 32 Serial pc(SERIAL_TX, SERIAL_RX, 115200); // USB
ccpjboss 0:0d3a25d4697e 33
ccpjboss 0:0d3a25d4697e 34 // Lidar
ccpjboss 0:0d3a25d4697e 35 RPLidar lidar;
ccpjboss 0:0d3a25d4697e 36 BufferedSerial se_lidar(PA_9, PA_10);
ccpjboss 0:0d3a25d4697e 37 PwmOut rplidar_motor(D3);
ccpjboss 0:0d3a25d4697e 38 struct RPLidarMeasurement data;
ccpjboss 0:0d3a25d4697e 39 double lidar_angle, lidar_dist;
ccpjboss 0:0d3a25d4697e 40 double lidar_distMin = 7.5f;
ccpjboss 0:0d3a25d4697e 41 double lidar_distMax = 200.0f;
ccpjboss 0:0d3a25d4697e 42 double offSetLidarX = -2.8, offSetLidarY = -1.5, lidar_angRet = 1.1*PI_val/180.0;
ccpjboss 0:0d3a25d4697e 43 double lidar_angleCs, lidar_angleSn, tMaxX, tMaxY, tDeltaX, tDeltaY;
ccpjboss 0:0d3a25d4697e 44 int stepX, stepY;
ccpjboss 0:0d3a25d4697e 45 double angDif, posDifX, posDifY;
ccpjboss 0:0d3a25d4697e 46
ccpjboss 0:0d3a25d4697e 47
ccpjboss 0:0d3a25d4697e 48 // Dados a modificar
ccpjboss 0:0d3a25d4697e 49 double pos_x[] = {30.0f,130.0f,30.0f,30.0f};
ccpjboss 0:0d3a25d4697e 50 double pos_y[] = {30.0f,130.0f,130.0f,30.0f};
ccpjboss 0:0d3a25d4697e 51 double THETA_Start = 0.0f;
ccpjboss 0:0d3a25d4697e 52
ccpjboss 0:0d3a25d4697e 53 // Periodos
ccpjboss 0:0d3a25d4697e 54 int T_printOdo = 250; int flag_printOdo = 1; // Periodo para print da odometria
ccpjboss 0:0d3a25d4697e 55 int T_upOdo = 10; // Periodo para update dos Encoders
ccpjboss 0:0d3a25d4697e 56
ccpjboss 0:0d3a25d4697e 57 // Variaveis Globais
ccpjboss 0:0d3a25d4697e 58 float X = 0.0f;
ccpjboss 0:0d3a25d4697e 59 float Y = 0.0f;
ccpjboss 0:0d3a25d4697e 60 float THETA = 0.0f;
ccpjboss 0:0d3a25d4697e 61 double delta_D = 0.0f;
ccpjboss 0:0d3a25d4697e 62 double delta_TH = 0.0f;
ccpjboss 0:0d3a25d4697e 63 double goal_x, goal_y;
ccpjboss 0:0d3a25d4697e 64 double v, w, phi, racio, df, delta_theta;
ccpjboss 0:0d3a25d4697e 65 double DR_cm, DL_cm;
ccpjboss 0:0d3a25d4697e 66 int lastEncR = -1, lastEncL = -1, cont, pos_cont = 1;
ccpjboss 0:0d3a25d4697e 67 int w_e, w_d, dir_e, dir_d, sw_e, sw_d;
ccpjboss 0:0d3a25d4697e 68 int size_pos;
ccpjboss 0:0d3a25d4697e 69 bool running = true;
ccpjboss 0:0d3a25d4697e 70 double thetaCs, thetaSn;
ccpjboss 0:0d3a25d4697e 71
ccpjboss 0:0d3a25d4697e 72 // Mapa
ccpjboss 0:0d3a25d4697e 73 double fca = 30; double fcr = 2000;
ccpjboss 0:0d3a25d4697e 74 float mapa[42][42];
ccpjboss 0:0d3a25d4697e 75 int t_mapax = floor((D_MAPAX + MAPBUFFER*2.0)/D_QUAD);
ccpjboss 0:0d3a25d4697e 76 int t_mapay = floor((D_MAPAY + MAPBUFFER*2.0)/D_QUAD);
ccpjboss 0:0d3a25d4697e 77 int xa, ya, x_janela, y_janela;
ccpjboss 0:0d3a25d4697e 78 double frx, fry, da, delta_dax, delta_day, T_JANELA = D_JANELA/D_QUAD; // tamanho da janela em quadrados (janela) ;
ccpjboss 0:0d3a25d4697e 79 double delta_fx, delta_fy, fax, fay, Fx, Fy, x_prox, y_prox;
ccpjboss 0:0d3a25d4697e 80 double erro, erro_int = 0;
ccpjboss 0:0d3a25d4697e 81 int X_ind, Y_ind;
ccpjboss 0:0d3a25d4697e 82 double exp_mapa;
ccpjboss 0:0d3a25d4697e 83
ccpjboss 0:0d3a25d4697e 84 // Obstaculos
ccpjboss 0:0d3a25d4697e 85 double obstX, obstY;
ccpjboss 0:0d3a25d4697e 86 int obstX_ind, obstY_ind;
ccpjboss 0:0d3a25d4697e 87
ccpjboss 0:0d3a25d4697e 88 // Tempos
ccpjboss 0:0d3a25d4697e 89 uint32_t last_printOdo = 0, now_time = 0; // Referencia do ultimo print
ccpjboss 0:0d3a25d4697e 90 uint32_t last_upOdo = 0, t_control = 0; // Referencia do ultimo print
ccpjboss 0:0d3a25d4697e 91 uint32_t last_upMap = 0, t_map = 0; // Referencia do ultimo updateMap
ccpjboss 0:0d3a25d4697e 92 uint32_t start_upLid = 0; // Referencia do ultimo print
ccpjboss 0:0d3a25d4697e 93 uint32_t last_printMap =0;
ccpjboss 0:0d3a25d4697e 94 Timer t; // Variavel print
ccpjboss 0:0d3a25d4697e 95
ccpjboss 0:0d3a25d4697e 96 // Declaracao de funcoes
ccpjboss 0:0d3a25d4697e 97 void start_system();
ccpjboss 0:0d3a25d4697e 98 void lidar_setUp();
ccpjboss 0:0d3a25d4697e 99 int check_enc();
ccpjboss 0:0d3a25d4697e 100 void wait_user();
ccpjboss 0:0d3a25d4697e 101 void print_odo();
ccpjboss 0:0d3a25d4697e 102 void update_goal();
ccpjboss 0:0d3a25d4697e 103 void update_odo();
ccpjboss 0:0d3a25d4697e 104 void update_map(int, int);
ccpjboss 0:0d3a25d4697e 105 void update_control();
ccpjboss 0:0d3a25d4697e 106 void update_vel(double v, double w);
ccpjboss 0:0d3a25d4697e 107 void update_pos();
ccpjboss 0:0d3a25d4697e 108 void check_error();
ccpjboss 0:0d3a25d4697e 109 void shutdown_system();
ccpjboss 0:0d3a25d4697e 110 void print_mapa();
ccpjboss 0:0d3a25d4697e 111
ccpjboss 0:0d3a25d4697e 112 int main(){
ccpjboss 0:0d3a25d4697e 113 size_pos = sizeof(pos_x)/sizeof(pos_x[0]);
ccpjboss 0:0d3a25d4697e 114 const double v = 175;
ccpjboss 0:0d3a25d4697e 115 const double w = 4.5;
ccpjboss 0:0d3a25d4697e 116
ccpjboss 0:0d3a25d4697e 117 start_system();
ccpjboss 0:0d3a25d4697e 118 update_vel(v,w);
ccpjboss 0:0d3a25d4697e 119
ccpjboss 0:0d3a25d4697e 120 /*while(running) {
ccpjboss 0:0d3a25d4697e 121 now_time = t.read_ms();
ccpjboss 0:0d3a25d4697e 122 t_control = now_time - last_upOdo;
ccpjboss 0:0d3a25d4697e 123 if( t_control >= T_upOdo){
ccpjboss 0:0d3a25d4697e 124 if(check_enc()){
ccpjboss 0:0d3a25d4697e 125 update_odo();
ccpjboss 0:0d3a25d4697e 126 update_pos();
ccpjboss 0:0d3a25d4697e 127 update_map(2, 2);
ccpjboss 0:0d3a25d4697e 128 update_control();
ccpjboss 0:0d3a25d4697e 129 update_vel();
ccpjboss 0:0d3a25d4697e 130 check_error();
ccpjboss 0:0d3a25d4697e 131 last_upOdo = now_time;
ccpjboss 0:0d3a25d4697e 132 }
ccpjboss 0:0d3a25d4697e 133 }
ccpjboss 0:0d3a25d4697e 134
ccpjboss 0:0d3a25d4697e 135 now_time = t.read_ms();
ccpjboss 0:0d3a25d4697e 136 if((now_time - last_printOdo >= T_printOdo) && (flag_printOdo)){
ccpjboss 0:0d3a25d4697e 137 print_odo();
ccpjboss 0:0d3a25d4697e 138 last_printOdo = now_time;
ccpjboss 0:0d3a25d4697e 139 }
ccpjboss 0:0d3a25d4697e 140 }*/
ccpjboss 0:0d3a25d4697e 141 //shutdown_system();
ccpjboss 0:0d3a25d4697e 142 }
ccpjboss 0:0d3a25d4697e 143
ccpjboss 0:0d3a25d4697e 144 void update_goal(){
ccpjboss 0:0d3a25d4697e 145 goal_x = pos_x[pos_cont] + MAPBUFFER;
ccpjboss 0:0d3a25d4697e 146 goal_y = pos_y[pos_cont] + MAPBUFFER;
ccpjboss 0:0d3a25d4697e 147 pos_cont++;
ccpjboss 0:0d3a25d4697e 148 }
ccpjboss 0:0d3a25d4697e 149
ccpjboss 0:0d3a25d4697e 150 void start_system(){
ccpjboss 0:0d3a25d4697e 151 init_communication(&pc);
ccpjboss 0:0d3a25d4697e 152
ccpjboss 0:0d3a25d4697e 153 X = pos_x[0] + MAPBUFFER;
ccpjboss 0:0d3a25d4697e 154 Y = pos_y[0] + MAPBUFFER;
ccpjboss 0:0d3a25d4697e 155 THETA = THETA_Start;
ccpjboss 0:0d3a25d4697e 156
ccpjboss 0:0d3a25d4697e 157 update_goal();
ccpjboss 0:0d3a25d4697e 158
ccpjboss 0:0d3a25d4697e 159 pc.printf("Robo Speed to Zero...\n");
ccpjboss 0:0d3a25d4697e 160 setSpeeds(0, 0);
ccpjboss 0:0d3a25d4697e 161 pc.printf(" Done!\n");
ccpjboss 0:0d3a25d4697e 162
ccpjboss 0:0d3a25d4697e 163 pc.printf("Initiating Lidar...\n");
ccpjboss 0:0d3a25d4697e 164 lidar_setUp();
ccpjboss 0:0d3a25d4697e 165 pc.printf(" Done!\n");
ccpjboss 0:0d3a25d4697e 166
ccpjboss 0:0d3a25d4697e 167 wait_user();
ccpjboss 0:0d3a25d4697e 168
ccpjboss 0:0d3a25d4697e 169 pc.printf("START\n");
ccpjboss 0:0d3a25d4697e 170 pc.printf("LAB3\n");
ccpjboss 0:0d3a25d4697e 171 pc.printf("VFF\n");
ccpjboss 0:0d3a25d4697e 172
ccpjboss 0:0d3a25d4697e 173 char str[100], str2[10];
ccpjboss 0:0d3a25d4697e 174 for(int posInt = 1; posInt < size_pos; posInt++){
ccpjboss 0:0d3a25d4697e 175 sprintf(str2,"/%.2f/%2.f", pos_x[posInt], pos_y[posInt]);
ccpjboss 0:0d3a25d4697e 176 strcat(str, str2);
ccpjboss 0:0d3a25d4697e 177 }
ccpjboss 0:0d3a25d4697e 178 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);
ccpjboss 0:0d3a25d4697e 179
ccpjboss 0:0d3a25d4697e 180 pc.printf("BEGIN\n");
ccpjboss 0:0d3a25d4697e 181
ccpjboss 0:0d3a25d4697e 182 pc.printf("Lidar Speed to 100%%...\n");
ccpjboss 0:0d3a25d4697e 183 //rplidar_motor.write(0.35f);
ccpjboss 0:0d3a25d4697e 184 //pc.printf(" Done!\n");
ccpjboss 0:0d3a25d4697e 185
ccpjboss 0:0d3a25d4697e 186 pc.printf("Stabilizing for 0.7 Second...\n");
ccpjboss 0:0d3a25d4697e 187 wait(0.7);
ccpjboss 0:0d3a25d4697e 188 pc.printf(" Done!\n");
ccpjboss 0:0d3a25d4697e 189
ccpjboss 0:0d3a25d4697e 190 pc.printf("Reseting Encoders...\n");
ccpjboss 0:0d3a25d4697e 191 getCountsAndReset();
ccpjboss 0:0d3a25d4697e 192 pc.printf(" Done!\n");
ccpjboss 0:0d3a25d4697e 193
ccpjboss 0:0d3a25d4697e 194 print_odo();
ccpjboss 0:0d3a25d4697e 195
ccpjboss 0:0d3a25d4697e 196 t.start();
ccpjboss 0:0d3a25d4697e 197
ccpjboss 0:0d3a25d4697e 198 pc.printf("Updating Map with 250 Measures...\n");
ccpjboss 0:0d3a25d4697e 199 last_upMap = t.read_ms();
ccpjboss 0:0d3a25d4697e 200 //update_map(2, 700);
ccpjboss 0:0d3a25d4697e 201 t_map = t.read_ms() - last_upMap;
ccpjboss 0:0d3a25d4697e 202 pc.printf(" Done! %f\n", t_map/250.0);
ccpjboss 0:0d3a25d4697e 203 }
ccpjboss 0:0d3a25d4697e 204
ccpjboss 0:0d3a25d4697e 205 void lidar_setUp(){
ccpjboss 0:0d3a25d4697e 206 // Lidar initialization
ccpjboss 0:0d3a25d4697e 207
ccpjboss 0:0d3a25d4697e 208 rplidar_motor.period(0.01f);
ccpjboss 0:0d3a25d4697e 209 lidar.begin(se_lidar);
ccpjboss 0:0d3a25d4697e 210 lidar.setAngle(0,360);
ccpjboss 0:0d3a25d4697e 211 lidar.startThreadScan();
ccpjboss 0:0d3a25d4697e 212 rplidar_motor.write(0.0f);
ccpjboss 0:0d3a25d4697e 213 }
ccpjboss 0:0d3a25d4697e 214
ccpjboss 0:0d3a25d4697e 215 int check_enc(){
ccpjboss 0:0d3a25d4697e 216
ccpjboss 0:0d3a25d4697e 217 getCountsAndReset(); // Obtem encoder e reset encoder
ccpjboss 0:0d3a25d4697e 218 //pc.printf("%d %d ", countsRight, countsLeft);
ccpjboss 0:0d3a25d4697e 219 if(!(abs(countsRight) == 1 && countsLeft == 255)){
ccpjboss 0:0d3a25d4697e 220 lastEncR = countsRight;
ccpjboss 0:0d3a25d4697e 221 lastEncL = countsLeft;
ccpjboss 0:0d3a25d4697e 222 //pc.printf("|1| %d %d\n\r", countsRight, countsLeft);
ccpjboss 0:0d3a25d4697e 223 return 1;
ccpjboss 0:0d3a25d4697e 224 }else{
ccpjboss 0:0d3a25d4697e 225 if(lastEncR == -1 && lastEncL == -1){
ccpjboss 0:0d3a25d4697e 226 //pc.printf("|2| %d %d\n\r", countsRight, countsLeft);
ccpjboss 0:0d3a25d4697e 227 return 0;
ccpjboss 0:0d3a25d4697e 228 }else{
ccpjboss 0:0d3a25d4697e 229 countsRight = lastEncR;
ccpjboss 0:0d3a25d4697e 230 countsLeft = lastEncL;
ccpjboss 0:0d3a25d4697e 231 //pc.printf("|3| %d %d\n\r", countsRight, countsLeft);
ccpjboss 0:0d3a25d4697e 232 return 1;
ccpjboss 0:0d3a25d4697e 233 }
ccpjboss 0:0d3a25d4697e 234 }
ccpjboss 0:0d3a25d4697e 235 }
ccpjboss 0:0d3a25d4697e 236
ccpjboss 0:0d3a25d4697e 237 void update_odo(){
ccpjboss 0:0d3a25d4697e 238
ccpjboss 0:0d3a25d4697e 239 DR_cm = (double)countsRight * ((D_RODA*PI_val)/1440.0);
ccpjboss 0:0d3a25d4697e 240 DL_cm = (double)countsLeft * ((D_RODA*PI_val)/1440.0);
ccpjboss 0:0d3a25d4697e 241
ccpjboss 0:0d3a25d4697e 242 delta_D = (DR_cm + DL_cm)/2.0;
ccpjboss 0:0d3a25d4697e 243 delta_TH = (DR_cm - DL_cm)/D_EIXO;
ccpjboss 0:0d3a25d4697e 244 delta_TH = atan2(sin(delta_TH), cos(delta_TH));
ccpjboss 0:0d3a25d4697e 245 }
ccpjboss 0:0d3a25d4697e 246
ccpjboss 0:0d3a25d4697e 247 void update_pos(){
ccpjboss 0:0d3a25d4697e 248 if(delta_TH == 0){
ccpjboss 0:0d3a25d4697e 249 X = X + delta_D * cos(THETA);
ccpjboss 0:0d3a25d4697e 250 Y = Y + delta_D * sin(THETA);
ccpjboss 0:0d3a25d4697e 251 }else{
ccpjboss 0:0d3a25d4697e 252 X = X + delta_D*sin(delta_TH/2.0)*cos(THETA+delta_TH/2.0)/(delta_TH/2.0);
ccpjboss 0:0d3a25d4697e 253 Y = Y + delta_D*sin(delta_TH/2.0)*sin(THETA+delta_TH/2.0)/(delta_TH/2.0);
ccpjboss 0:0d3a25d4697e 254 THETA += delta_TH;
ccpjboss 0:0d3a25d4697e 255 THETA = atan2(sin(THETA), cos(THETA));
ccpjboss 0:0d3a25d4697e 256 }
ccpjboss 0:0d3a25d4697e 257 }
ccpjboss 0:0d3a25d4697e 258
ccpjboss 0:0d3a25d4697e 259 void update_map(int update_mode, int qnt){
ccpjboss 0:0d3a25d4697e 260 // update_mode = 1 -> Timer
ccpjboss 0:0d3a25d4697e 261 // update_mode = 2 -> Contador
ccpjboss 0:0d3a25d4697e 262
ccpjboss 0:0d3a25d4697e 263 X_ind = (int)floor(X/D_QUAD); // Indice célula posicao X
ccpjboss 0:0d3a25d4697e 264 Y_ind = (int)floor(Y/D_QUAD); // Indice célula posicao Y
ccpjboss 0:0d3a25d4697e 265
ccpjboss 0:0d3a25d4697e 266 thetaCs = cos(THETA);
ccpjboss 0:0d3a25d4697e 267 thetaSn = sin(THETA);
ccpjboss 0:0d3a25d4697e 268 mapa[X_ind][Y_ind] -= 0.65; // Posição inicial está livre
ccpjboss 0:0d3a25d4697e 269
ccpjboss 0:0d3a25d4697e 270 cont = 0;
ccpjboss 0:0d3a25d4697e 271 start_upLid = t.read_ms();
ccpjboss 0:0d3a25d4697e 272 while((cont < qnt)){
ccpjboss 0:0d3a25d4697e 273 if(lidar.pollSensorData(&data)==0){
ccpjboss 0:0d3a25d4697e 274 X_ind = (int)floor(X/D_QUAD); // Indice célula posicao X
ccpjboss 0:0d3a25d4697e 275 Y_ind = (int)floor(Y/D_QUAD); // Indice célula posicao Y
ccpjboss 0:0d3a25d4697e 276
ccpjboss 0:0d3a25d4697e 277 lidar_dist = (double) data.distance/10.0;
ccpjboss 0:0d3a25d4697e 278 lidar_angle = (double) data.angle;
ccpjboss 0:0d3a25d4697e 279 if((lidar_dist > lidar_distMin) && (lidar_dist < lidar_distMax)){
ccpjboss 0:0d3a25d4697e 280
ccpjboss 0:0d3a25d4697e 281 lidar_angle = lidar_angle*PI_val/180.0 ;
ccpjboss 0:0d3a25d4697e 282
ccpjboss 0:0d3a25d4697e 283 obstX = X + offSetLidarX*thetaCs - offSetLidarY*thetaSn - lidar_dist*sin(lidar_angRet - THETA + lidar_angle);
ccpjboss 0:0d3a25d4697e 284 obstY = Y + offSetLidarY*thetaCs + offSetLidarX*thetaSn - lidar_dist*cos(lidar_angRet - THETA + lidar_angle);
ccpjboss 0:0d3a25d4697e 285
ccpjboss 0:0d3a25d4697e 286 posDifX = obstX - X;
ccpjboss 0:0d3a25d4697e 287 posDifY = obstY - Y;
ccpjboss 0:0d3a25d4697e 288 angDif = atan2(posDifY, posDifX);
ccpjboss 0:0d3a25d4697e 289 lidar_angleCs = cos(angDif);
ccpjboss 0:0d3a25d4697e 290 lidar_angleSn = sin(angDif);
ccpjboss 0:0d3a25d4697e 291
ccpjboss 0:0d3a25d4697e 292 obstX_ind = (int)floor(obstX/D_QUAD);
ccpjboss 0:0d3a25d4697e 293 obstY_ind = (int)floor(obstY/D_QUAD);
ccpjboss 0:0d3a25d4697e 294
ccpjboss 0:0d3a25d4697e 295 if(obstX < X){
ccpjboss 0:0d3a25d4697e 296 stepX = -1;
ccpjboss 0:0d3a25d4697e 297 }else{
ccpjboss 0:0d3a25d4697e 298 stepX = 1;
ccpjboss 0:0d3a25d4697e 299 }
ccpjboss 0:0d3a25d4697e 300 if(obstY < Y){
ccpjboss 0:0d3a25d4697e 301 stepY = -1;
ccpjboss 0:0d3a25d4697e 302 }else{
ccpjboss 0:0d3a25d4697e 303 stepY = 1;
ccpjboss 0:0d3a25d4697e 304 }
ccpjboss 0:0d3a25d4697e 305
ccpjboss 0:0d3a25d4697e 306 tMaxX = X;
ccpjboss 0:0d3a25d4697e 307 tMaxY = Y;
ccpjboss 0:0d3a25d4697e 308
ccpjboss 0:0d3a25d4697e 309 while(1){
ccpjboss 0:0d3a25d4697e 310 // tDeltaX = (abs(((X_ind+1*(stepX>0))*D_QUAD) - tMaxX))/lidar_angleCs;
ccpjboss 0:0d3a25d4697e 311 // tDeltaY = (abs(((Y_ind+1*(stepY>0))*D_QUAD) - tMaxY))/lidar_angleSn;
ccpjboss 0:0d3a25d4697e 312
ccpjboss 0:0d3a25d4697e 313 tDeltaX = (X_ind+1*(stepX>0))*D_QUAD;
ccpjboss 0:0d3a25d4697e 314 tDeltaY = (Y_ind+1*(stepY>0))*D_QUAD;
ccpjboss 0:0d3a25d4697e 315 tDeltaX = abs(tDeltaX - tMaxX);
ccpjboss 0:0d3a25d4697e 316 tDeltaY = abs(tDeltaY - tMaxY);
ccpjboss 0:0d3a25d4697e 317 tDeltaX = (tDeltaX)/lidar_angleCs;
ccpjboss 0:0d3a25d4697e 318 tDeltaY = (tDeltaY)/lidar_angleSn;
ccpjboss 0:0d3a25d4697e 319
ccpjboss 0:0d3a25d4697e 320 if(abs(tDeltaX) < abs(tDeltaY)){
ccpjboss 0:0d3a25d4697e 321 X_ind += stepX;
ccpjboss 0:0d3a25d4697e 322 tMaxX = (X_ind+1*(stepX<0))*D_QUAD;
ccpjboss 0:0d3a25d4697e 323 tMaxY += tDeltaX*lidar_angleSn*stepX;
ccpjboss 0:0d3a25d4697e 324 }else{
ccpjboss 0:0d3a25d4697e 325 Y_ind += stepY;
ccpjboss 0:0d3a25d4697e 326 tMaxY = (Y_ind+1*(stepY<0))*D_QUAD;
ccpjboss 0:0d3a25d4697e 327 tMaxX += tDeltaY*lidar_angleCs*stepY;
ccpjboss 0:0d3a25d4697e 328 }
ccpjboss 0:0d3a25d4697e 329 //pc.printf("%d %d\n\r", X_ind, Y_ind);
ccpjboss 0:0d3a25d4697e 330 if((X_ind >= 0) && (X_ind < t_mapax) && (Y_ind >= 0) && (Y_ind < t_mapay)){
ccpjboss 0:0d3a25d4697e 331 if(X_ind == obstX_ind && Y_ind == obstY_ind){
ccpjboss 0:0d3a25d4697e 332 mapa[X_ind][Y_ind] += 0.65;
ccpjboss 0:0d3a25d4697e 333 //pc.printf("break\n\r");
ccpjboss 0:0d3a25d4697e 334 break;
ccpjboss 0:0d3a25d4697e 335 //}else if(X_ind == 0 || Y_ind == 0 || X_ind == t_mapax-1 || Y_ind == t_mapay-1 ){
ccpjboss 0:0d3a25d4697e 336 // mapa[X_ind][Y_ind] += 0.65;
ccpjboss 0:0d3a25d4697e 337 }else{
ccpjboss 0:0d3a25d4697e 338 //pc.printf("livre\n\r");
ccpjboss 0:0d3a25d4697e 339 mapa[X_ind][Y_ind] -= 0.65;
ccpjboss 0:0d3a25d4697e 340 }
ccpjboss 0:0d3a25d4697e 341 }else{
ccpjboss 0:0d3a25d4697e 342 break;
ccpjboss 0:0d3a25d4697e 343 }
ccpjboss 0:0d3a25d4697e 344 }
ccpjboss 0:0d3a25d4697e 345 //pc.printf("%d\n",cont);
ccpjboss 0:0d3a25d4697e 346 cont++;
ccpjboss 0:0d3a25d4697e 347 }
ccpjboss 0:0d3a25d4697e 348 }
ccpjboss 0:0d3a25d4697e 349 }
ccpjboss 0:0d3a25d4697e 350 }
ccpjboss 0:0d3a25d4697e 351
ccpjboss 0:0d3a25d4697e 352 void update_control(){
ccpjboss 0:0d3a25d4697e 353 xa = (int)floor(X/D_QUAD); // Indice célula posicao X
ccpjboss 0:0d3a25d4697e 354 ya = (int)floor(Y/D_QUAD); // Indice célula posicao Y
ccpjboss 0:0d3a25d4697e 355
ccpjboss 0:0d3a25d4697e 356 frx = 0; fry = 0;
ccpjboss 0:0d3a25d4697e 357 x_janela = xa - floor(T_JANELA/2.0);
ccpjboss 0:0d3a25d4697e 358 for(int i = 0; i < T_JANELA; i++){
ccpjboss 0:0d3a25d4697e 359 y_janela = ya - floor(T_JANELA/2.0);
ccpjboss 0:0d3a25d4697e 360 for(int j = 0; j < T_JANELA; j++){
ccpjboss 0:0d3a25d4697e 361
ccpjboss 0:0d3a25d4697e 362 if((x_janela >= 0) && (x_janela < t_mapax) && (y_janela >= 0) && (y_janela < t_mapay)){
ccpjboss 0:0d3a25d4697e 363
ccpjboss 0:0d3a25d4697e 364 delta_dax = x_janela*D_QUAD;
ccpjboss 0:0d3a25d4697e 365 delta_day = y_janela*D_QUAD;
ccpjboss 0:0d3a25d4697e 366
ccpjboss 0:0d3a25d4697e 367 if(X >= delta_dax+D_QUAD){
ccpjboss 0:0d3a25d4697e 368 delta_dax = delta_dax+D_QUAD-X;
ccpjboss 0:0d3a25d4697e 369 }else{
ccpjboss 0:0d3a25d4697e 370 delta_dax = delta_dax-X;
ccpjboss 0:0d3a25d4697e 371 }
ccpjboss 0:0d3a25d4697e 372
ccpjboss 0:0d3a25d4697e 373 if(Y >= delta_day+D_QUAD){
ccpjboss 0:0d3a25d4697e 374 delta_day = delta_day+D_QUAD-Y;
ccpjboss 0:0d3a25d4697e 375 }else{
ccpjboss 0:0d3a25d4697e 376 delta_day = delta_day-Y;
ccpjboss 0:0d3a25d4697e 377 }
ccpjboss 0:0d3a25d4697e 378
ccpjboss 0:0d3a25d4697e 379 if(delta_dax == 0){
ccpjboss 0:0d3a25d4697e 380 delta_dax = 0.1;
ccpjboss 0:0d3a25d4697e 381 }
ccpjboss 0:0d3a25d4697e 382 if(delta_day == 0){
ccpjboss 0:0d3a25d4697e 383 delta_day = 0.1;
ccpjboss 0:0d3a25d4697e 384 }
ccpjboss 0:0d3a25d4697e 385
ccpjboss 0:0d3a25d4697e 386 exp_mapa = 1-(1/(1+exp(mapa[x_janela][y_janela])));
ccpjboss 0:0d3a25d4697e 387 if(exp_mapa <= 0.5){
ccpjboss 0:0d3a25d4697e 388 exp_mapa = 0;
ccpjboss 0:0d3a25d4697e 389 }
ccpjboss 0:0d3a25d4697e 390
ccpjboss 0:0d3a25d4697e 391 da = sqrt(pow(delta_dax,2)+pow(delta_day,2));
ccpjboss 0:0d3a25d4697e 392 frx += fcr*exp_mapa*delta_dax/pow(da,3);
ccpjboss 0:0d3a25d4697e 393 fry += fcr*exp_mapa*delta_day/pow(da,3);
ccpjboss 0:0d3a25d4697e 394 }
ccpjboss 0:0d3a25d4697e 395 y_janela ++;
ccpjboss 0:0d3a25d4697e 396 }
ccpjboss 0:0d3a25d4697e 397 x_janela ++;
ccpjboss 0:0d3a25d4697e 398 }
ccpjboss 0:0d3a25d4697e 399
ccpjboss 0:0d3a25d4697e 400 delta_fx = goal_x-X;
ccpjboss 0:0d3a25d4697e 401 delta_fy = goal_y-Y;
ccpjboss 0:0d3a25d4697e 402
ccpjboss 0:0d3a25d4697e 403 df = sqrt(pow(delta_fx,2)+pow(delta_fy,2));
ccpjboss 0:0d3a25d4697e 404
ccpjboss 0:0d3a25d4697e 405 fax = fca*delta_fx/df;
ccpjboss 0:0d3a25d4697e 406 fay = fca*delta_fy/df;
ccpjboss 0:0d3a25d4697e 407
ccpjboss 0:0d3a25d4697e 408 Fx = fax - frx;
ccpjboss 0:0d3a25d4697e 409 Fy = fay - fry;
ccpjboss 0:0d3a25d4697e 410
ccpjboss 0:0d3a25d4697e 411 x_prox = X + Fx;
ccpjboss 0:0d3a25d4697e 412 y_prox = Y + Fy;
ccpjboss 0:0d3a25d4697e 413
ccpjboss 0:0d3a25d4697e 414 erro = sqrt(pow(x_prox-X,2)+pow(y_prox-Y,2))-D_ERRO;
ccpjboss 0:0d3a25d4697e 415 erro_int = erro_int + erro;
ccpjboss 0:0d3a25d4697e 416
ccpjboss 0:0d3a25d4697e 417 phi = atan2((y_prox-Y), (x_prox-X));
ccpjboss 0:0d3a25d4697e 418 delta_theta = atan2(sin(phi - THETA), cos(phi - THETA));
ccpjboss 0:0d3a25d4697e 419
ccpjboss 0:0d3a25d4697e 420 v = KV*erro+KI*erro_int;
ccpjboss 0:0d3a25d4697e 421 w = KS*delta_theta;
ccpjboss 0:0d3a25d4697e 422 }
ccpjboss 0:0d3a25d4697e 423
ccpjboss 0:0d3a25d4697e 424 void update_vel(double v, double w){
ccpjboss 0:0d3a25d4697e 425
ccpjboss 0:0d3a25d4697e 426 w_e = (v-(D_EIXO/2.0)*w)/(D_RODA/2.0);
ccpjboss 0:0d3a25d4697e 427 w_d = (v+(D_EIXO/2.0)*w)/(D_RODA/2.0);
ccpjboss 0:0d3a25d4697e 428
ccpjboss 0:0d3a25d4697e 429 sw_e = w_e;
ccpjboss 0:0d3a25d4697e 430 sw_d = w_d;
ccpjboss 0:0d3a25d4697e 431
ccpjboss 0:0d3a25d4697e 432 if(abs(w_e) > MAX_SPEED || abs(w_d) > MAX_SPEED){
ccpjboss 0:0d3a25d4697e 433 racio = fabs((double)w_e/w_d);
ccpjboss 0:0d3a25d4697e 434
ccpjboss 0:0d3a25d4697e 435 if( w_e < 0 ) dir_e = -1; else dir_e = 1;
ccpjboss 0:0d3a25d4697e 436 if( w_d < 0 ) dir_d = -1; else dir_d = 1;
ccpjboss 0:0d3a25d4697e 437
ccpjboss 0:0d3a25d4697e 438 if(fabs(racio) >= 1.0){
ccpjboss 0:0d3a25d4697e 439 sw_e = dir_e*MAX_SPEED;
ccpjboss 0:0d3a25d4697e 440 sw_d = dir_d*MAX_SPEED/racio;
ccpjboss 0:0d3a25d4697e 441 }else{
ccpjboss 0:0d3a25d4697e 442 sw_e = dir_e*MAX_SPEED*racio;
ccpjboss 0:0d3a25d4697e 443 sw_d = dir_d*MAX_SPEED;
ccpjboss 0:0d3a25d4697e 444 }
ccpjboss 0:0d3a25d4697e 445 }
ccpjboss 0:0d3a25d4697e 446 if(abs(w_e) < MIN_SPEED || abs(w_d) < MIN_SPEED){
ccpjboss 0:0d3a25d4697e 447 racio = fabs((double)w_e/w_d);
ccpjboss 0:0d3a25d4697e 448
ccpjboss 0:0d3a25d4697e 449 if( w_e < 0 ) dir_e = -1; else dir_e = 1;
ccpjboss 0:0d3a25d4697e 450 if( w_d < 0 ) dir_d = -1; else dir_d = 1;
ccpjboss 0:0d3a25d4697e 451
ccpjboss 0:0d3a25d4697e 452 if(fabs(racio) >= 1.0){
ccpjboss 0:0d3a25d4697e 453 if(MIN_SPEED*racio <= MIN_MAX_SPEED){
ccpjboss 0:0d3a25d4697e 454 sw_e = dir_e*MIN_SPEED*racio;
ccpjboss 0:0d3a25d4697e 455 sw_d = dir_d*MIN_SPEED;
ccpjboss 0:0d3a25d4697e 456 }
ccpjboss 0:0d3a25d4697e 457 }else{
ccpjboss 0:0d3a25d4697e 458 if(MIN_SPEED/racio <= MIN_MAX_SPEED){
ccpjboss 0:0d3a25d4697e 459 sw_e = dir_e*MIN_SPEED;
ccpjboss 0:0d3a25d4697e 460 sw_d = dir_d*MIN_SPEED/racio;
ccpjboss 0:0d3a25d4697e 461 }
ccpjboss 0:0d3a25d4697e 462 }
ccpjboss 0:0d3a25d4697e 463 }
ccpjboss 0:0d3a25d4697e 464
ccpjboss 0:0d3a25d4697e 465 setSpeeds(sw_e, sw_d);
ccpjboss 0:0d3a25d4697e 466 }
ccpjboss 0:0d3a25d4697e 467
ccpjboss 0:0d3a25d4697e 468 void check_error(){
ccpjboss 0:0d3a25d4697e 469 if(df > DIST_STOP){
ccpjboss 0:0d3a25d4697e 470 running = true;
ccpjboss 0:0d3a25d4697e 471 }else{
ccpjboss 0:0d3a25d4697e 472 if(pos_cont < size_pos){
ccpjboss 0:0d3a25d4697e 473 //pc.printf("Goal Completed!\n");
ccpjboss 0:0d3a25d4697e 474 //pc.printf("Changing Goal...\n");
ccpjboss 0:0d3a25d4697e 475 update_goal();
ccpjboss 0:0d3a25d4697e 476 //pc.printf(" Done!\n");
ccpjboss 0:0d3a25d4697e 477 erro_int = 0;
ccpjboss 0:0d3a25d4697e 478 running = true;
ccpjboss 0:0d3a25d4697e 479 }else{
ccpjboss 0:0d3a25d4697e 480 running = false;
ccpjboss 0:0d3a25d4697e 481 }
ccpjboss 0:0d3a25d4697e 482 }
ccpjboss 0:0d3a25d4697e 483 }
ccpjboss 0:0d3a25d4697e 484
ccpjboss 0:0d3a25d4697e 485 void print_mapa(){
ccpjboss 0:0d3a25d4697e 486 for( int xx = 0; xx < 42; xx++){
ccpjboss 0:0d3a25d4697e 487 for( int yy = 0; yy < 42; yy++){
ccpjboss 0:0d3a25d4697e 488 pc.printf("%d/%d/%.2f \n",xx,yy,mapa[xx][yy]);
ccpjboss 0:0d3a25d4697e 489 wait(0.0001);
ccpjboss 0:0d3a25d4697e 490 }
ccpjboss 0:0d3a25d4697e 491 }
ccpjboss 0:0d3a25d4697e 492 }
ccpjboss 0:0d3a25d4697e 493
ccpjboss 0:0d3a25d4697e 494 void print_odo(){
ccpjboss 0:0d3a25d4697e 495 pc.printf("%.2f/%.2f/%.2f/%.2f/%.2f \n", X - MAPBUFFER, Y - MAPBUFFER, THETA, x_prox - MAPBUFFER, y_prox - MAPBUFFER);
ccpjboss 0:0d3a25d4697e 496 }
ccpjboss 0:0d3a25d4697e 497
ccpjboss 0:0d3a25d4697e 498 void wait_user(){
ccpjboss 0:0d3a25d4697e 499 pc.printf("Press UserButton to continue...\n");
ccpjboss 0:0d3a25d4697e 500 while(mybutton); // Espera por butao USER ser primido
ccpjboss 0:0d3a25d4697e 501 pc.printf(" Done!\n");
ccpjboss 0:0d3a25d4697e 502 }
ccpjboss 0:0d3a25d4697e 503
ccpjboss 0:0d3a25d4697e 504 void shutdown_system(){
ccpjboss 0:0d3a25d4697e 505 setSpeeds(0,0);
ccpjboss 0:0d3a25d4697e 506
ccpjboss 0:0d3a25d4697e 507 rplidar_motor.write(0.0f);
ccpjboss 0:0d3a25d4697e 508
ccpjboss 0:0d3a25d4697e 509 print_mapa();
ccpjboss 0:0d3a25d4697e 510 pc.printf("END!\n");
ccpjboss 0:0d3a25d4697e 511 t.stop();
ccpjboss 0:0d3a25d4697e 512 }