SRA_HenriquePovoa / Mbed OS Lidar_Rodas

Experiencias do Henrique na quinta/sexta a noite

Dependencies:   BufferedSerial

main.cpp@13:20e124fba426, 2021-05-15 (annotated)

Committer:
henkiwan
Date:
Sat May 15 03:17:01 2021 +0000
Revision:
13:20e124fba426
Parent:
12:348038b466a3 
Que merda;

Who changed what in which revision?

UserRevisionLine numberNew contents of line
LuisRA 0:2b691d200d6f 1 #include "mbed.h"
LuisRA 0:2b691d200d6f 2 #include "BufferedSerial.h"
LuisRA 0:2b691d200d6f 3 #include "rplidar.h"
fabiofaria 1:dc87724abce8 4 #include "Robot.h"
fabiofaria 1:dc87724abce8 5 #include "Communication.h"
ppovoa 4:256f2cbe3fdd 6 #include "Functions.h"
henkiwan 12:348038b466a3 7 #include <math.h>
henkiwan 12:348038b466a3 8 #include <stdio.h>
ppovoa 4:256f2cbe3fdd 9 #include <stdlib.h>
LuisRA 0:2b691d200d6f 10
henkiwan 12:348038b466a3 11 #define PI 3.141592654
ppovoa 5:bc42c03f2a23 12
fabiofaria 1:dc87724abce8 13 Serial pc(SERIAL_TX, SERIAL_RX);
LuisRA 0:2b691d200d6f 14 RPLidar lidar;
LuisRA 0:2b691d200d6f 15 BufferedSerial se_lidar(PA_9, PA_10);
fabiofaria 3:0a718d139ed1 16 PwmOut rplidar_motor(D3);
LuisRA 0:2b691d200d6f 17
henkiwan 12:348038b466a3 18 void bresenham(float poseX, float poseY, float xf, float yf, float z);
henkiwan 12:348038b466a3 19
ppovoa 8:ad8766cf2ec0 20 float MapaLog[40][40] = {0};
ppovoa 10:6c8ea68e9bac 21 float Mapa40[40][40];
henkiwan 9:76b59c5220f1 22
henkiwan 12:348038b466a3 23 int main() {
fabiofaria 1:dc87724abce8 24 pc.baud(115200);
fabiofaria 1:dc87724abce8 25 init_communication(&pc);
ppovoa 5:bc42c03f2a23 26
ppovoa 5:bc42c03f2a23 27 DigitalIn UserButton(USER_BUTTON); // Initialize Button
ppovoa 5:bc42c03f2a23 28 DigitalOut myled(LED1); // Initialize LED
ppovoa 5:bc42c03f2a23 29
henkiwan 13:20e124fba426 30 // PARAMETROS CONSTANSTES
henkiwan 13:20e124fba426 31 float T = 0.01;
henkiwan 12:348038b466a3 32 float wheelsRadius = 3.5;
henkiwan 12:348038b466a3 33 float wheelsDistance = 13.5;
henkiwan 12:348038b466a3 34
henkiwan 12:348038b466a3 35 // Variaveis
henkiwan 13:20e124fba426 36 float k_v = 20; // 15
henkiwan 13:20e124fba426 37 float k_i = 0.2; //0.1
henkiwan 13:20e124fba426 38 float k_s = 8; //20
henkiwan 12:348038b466a3 39 float v_erro = 15;
henkiwan 12:348038b466a3 40
henkiwan 13:20e124fba426 41 int DPontos = 3; //distância de erro
henkiwan 12:348038b466a3 42
henkiwan 13:20e124fba426 43 float RectSize = 16.0;
henkiwan 13:20e124fba426 44 float FCRepul = -8000; // Forca de Repulsao
henkiwan 13:20e124fba426 45 float FCAtracao = 0.02; // Forca de Atracao
henkiwan 12:348038b466a3 46
henkiwan 13:20e124fba426 47 float pose[3] = {30, 30, 0}; // Ponto Inicial
henkiwan 13:20e124fba426 48 float pose_f[3]; // = {130,130,0}; // Ponto Objetivo (definido mais em baixo)
henkiwan 12:348038b466a3 49
henkiwan 12:348038b466a3 50 float errot = 0, erro = 0, erro_old = -1;
henkiwan 12:348038b466a3 51 float Forcas[4] = {0, 0, 0, 0};
henkiwan 12:348038b466a3 52
henkiwan 12:348038b466a3 53 float vRobot, wRobot, Norma, angForca, phi, w[2], ForcaResult[2], PIntermedio[2];
henkiwan 12:348038b466a3 54
henkiwan 12:348038b466a3 55 float LidarP[2]; // pontos na plataforma
henkiwan 12:348038b466a3 56 float LidarW[2]; // pontos no mundo
henkiwan 12:348038b466a3 57
henkiwan 12:348038b466a3 58 // matriz rotacao world plataforma
henkiwan 12:348038b466a3 59 float R_WP[3][3]= {{cos(pose[2]), -sin(pose[2]), pose[0]},
henkiwan 12:348038b466a3 60 {sin(pose[2]), cos(pose[2]), pose[1]},
henkiwan 12:348038b466a3 61 {0, 0, 1}};
henkiwan 12:348038b466a3 62
ppovoa 5:bc42c03f2a23 63 struct RPLidarMeasurement data;
ppovoa 4:256f2cbe3fdd 64
ppovoa 10:6c8ea68e9bac 65 //Inicializar Mapa das probabilidades a 0.5
ppovoa 10:6c8ea68e9bac 66 for(int i=0; i<40; i++)
ppovoa 10:6c8ea68e9bac 67 for(int j=0; j<40; j++)
ppovoa 10:6c8ea68e9bac 68 Mapa40[i][j]=0.5;
ppovoa 10:6c8ea68e9bac 69
henkiwan 13:20e124fba426 70 int leituras = 0;
henkiwan 13:20e124fba426 71 float dist;
henkiwan 13:20e124fba426 72 float r;
henkiwan 12:348038b466a3 73
fabiofaria 1:dc87724abce8 74 // Lidar initialization
henkiwan 13:20e124fba426 75 rplidar_motor.period(0.01f);
ppovoa 5:bc42c03f2a23 76 //rplidar_motor.write(0.5f);
LuisRA 0:2b691d200d6f 77 lidar.begin(se_lidar);
henkiwan 9:76b59c5220f1 78 lidar.setAngle(0, 360);
henkiwan 12:348038b466a3 79
ppovoa 4:256f2cbe3fdd 80
henkiwan 9:76b59c5220f1 81 setSpeeds(0, 0);
ppovoa 5:bc42c03f2a23 82
henkiwan 13:20e124fba426 83 pc.printf("wating...\n\r");
henkiwan 12:348038b466a3 84
ppovoa 5:bc42c03f2a23 85 int start = 0;
ppovoa 5:bc42c03f2a23 86 while(start != 1) {
ppovoa 5:bc42c03f2a23 87 myled=1;
ppovoa 5:bc42c03f2a23 88 if (UserButton == 0) { // Button is pressed
ppovoa 5:bc42c03f2a23 89 myled = 0;
ppovoa 5:bc42c03f2a23 90 start = 1;
ppovoa 5:bc42c03f2a23 91 rplidar_motor.write(0.5f);
ppovoa 5:bc42c03f2a23 92 }
ppovoa 5:bc42c03f2a23 93 }
ppovoa 5:bc42c03f2a23 94
ppovoa 7:f1c122bc63c8 95 lidar.startThreadScan();
ppovoa 4:256f2cbe3fdd 96
henkiwan 13:20e124fba426 97 while(leituras < 1500){
ppovoa 5:bc42c03f2a23 98 if(lidar.pollSensorData(&data) == 0)
ppovoa 10:6c8ea68e9bac 99 {
henkiwan 9:76b59c5220f1 100 //pc.printf("%f\t%f\n\r", data.distance, data.angle); // Prints one lidar measurement.
ppovoa 4:256f2cbe3fdd 101
ppovoa 6:59fbbeaac2af 102 float radians = (data.angle * static_cast<float>(PI))/180.0f;
ppovoa 4:256f2cbe3fdd 103
henkiwan 13:20e124fba426 104 dist = static_cast<float>(data.distance/10.0f); //converte de mm para cm
ppovoa 11:58187c7de709 105
ppovoa 11:58187c7de709 106 LidarP[0] = -dist*sin(radians)- 2.8f;
ppovoa 11:58187c7de709 107 LidarP[1] = -dist*cos(radians)- 1.5f;
LuisRA 0:2b691d200d6f 108
ppovoa 4:256f2cbe3fdd 109 //W_P = R_WP * p_P
ppovoa 4:256f2cbe3fdd 110 LidarW[0] = LidarP[0]* R_WP[0][0] + LidarP[1]* R_WP[0][1] + R_WP[0][2]; // coordenadas no mundo, ou seja, cm
ppovoa 4:256f2cbe3fdd 111 LidarW[1] = LidarP[0]* R_WP[1][0] + LidarP[1]* R_WP[1][1] + R_WP[1][2];
ppovoa 4:256f2cbe3fdd 112
henkiwan 13:20e124fba426 113 /*if (data.angle > 0 && data.angle < 5){
henkiwan 13:20e124fba426 114 pc.printf("Pose[0]: %f Pose[1]: %f Dist: %f Ang: %f LidarP[0]: %f LidarP[1]: %f LidarW[0]: %f LidarW[1]: %f\n\r", pose[0], pose[1], data.distance, data.angle, LidarP[0], LidarP[1], LidarW[0], LidarW[1]);
henkiwan 13:20e124fba426 115 }*/
ppovoa 4:256f2cbe3fdd 116 // pontos onde o feixe passou
henkiwan 13:20e124fba426 117 bresenham(pose[0]/5.0f, pose[1]/5.0f, LidarW[0]/5.0f, LidarW[1]/5.0f, dist/5.0f);
ppovoa 4:256f2cbe3fdd 118
ppovoa 5:bc42c03f2a23 119 leituras++;
ppovoa 5:bc42c03f2a23 120 }
henkiwan 13:20e124fba426 121 wait(0.01);
henkiwan 12:348038b466a3 122 }
henkiwan 12:348038b466a3 123
henkiwan 13:20e124fba426 124 //Código para enviar para o matlab
henkiwan 13:20e124fba426 125 /*rplidar_motor.write(0.0f);
henkiwan 13:20e124fba426 126 for(int i=0; i<40; i++){
henkiwan 13:20e124fba426 127 for(int j=0; j<40; j++){
henkiwan 13:20e124fba426 128 //pc.printf("%0.3f ", Mapa40[i][j]);
henkiwan 13:20e124fba426 129 send_odometry(1, 2, j+1, i+1, Mapa40[j][i],10, 30); // faz prints estranhos no Putty
henkiwan 12:348038b466a3 130 }
henkiwan 13:20e124fba426 131 //pc.printf("\n\r");
henkiwan 13:20e124fba426 132 //pc.printf("\n-----------------------------\n\r");
henkiwan 12:348038b466a3 133 }
henkiwan 12:348038b466a3 134
henkiwan 12:348038b466a3 135 start = 0;
henkiwan 12:348038b466a3 136 while(start != 1) {
henkiwan 12:348038b466a3 137 myled=1;
henkiwan 12:348038b466a3 138 if (UserButton == 0) { // Button is pressed
henkiwan 12:348038b466a3 139 myled = 0;
henkiwan 12:348038b466a3 140 start = 1;
henkiwan 13:20e124fba426 141 }
henkiwan 13:20e124fba426 142 }*/
henkiwan 13:20e124fba426 143
henkiwan 13:20e124fba426 144 pc.printf("Running\n\r");
henkiwan 13:20e124fba426 145
henkiwan 13:20e124fba426 146 for(int i=0; i<1; i++){
henkiwan 13:20e124fba426 147
henkiwan 13:20e124fba426 148 // Pontos ao pe da secretaria do prof
henkiwan 13:20e124fba426 149
henkiwan 13:20e124fba426 150 // Primeiro Ponto
henkiwan 13:20e124fba426 151 if (i==0){
henkiwan 13:20e124fba426 152 pose_f[0] = 160.0f;
henkiwan 13:20e124fba426 153 pose_f[1] = 160.0f;
henkiwan 13:20e124fba426 154 }
henkiwan 13:20e124fba426 155
henkiwan 13:20e124fba426 156 // Segundo Ponto
henkiwan 13:20e124fba426 157 /*if (i==1){
henkiwan 13:20e124fba426 158 pose_f[0] = 30;
henkiwan 13:20e124fba426 159 pose_f[1] = 30;
henkiwan 13:20e124fba426 160 }*/
henkiwan 13:20e124fba426 161
henkiwan 13:20e124fba426 162 /*// Terceiro Ponto
henkiwan 13:20e124fba426 163 if (i==2){
henkiwan 13:20e124fba426 164 pose_f[0] = 30;
henkiwan 13:20e124fba426 165 pose_f[1] = 30;
henkiwan 13:20e124fba426 166 }*/
henkiwan 13:20e124fba426 167
henkiwan 13:20e124fba426 168
henkiwan 13:20e124fba426 169 while(sqrt(pow(pose_f[1]-pose[1],2)+pow(pose_f[0]-pose[0],2)) > 8.0f) {
henkiwan 13:20e124fba426 170
henkiwan 13:20e124fba426 171 getCountsAndReset(); // Call getCountsAndReset() to read the values of encoders
henkiwan 13:20e124fba426 172 // and reset them. The values become available on variables
henkiwan 13:20e124fba426 173 // "countsLeft" and "countsRight".
henkiwan 13:20e124fba426 174
henkiwan 13:20e124fba426 175 //if(sqrt(pow(pose_f[1]-pose[1],2)+pow(pose_f[0]-pose[0],2)) > 13){
henkiwan 13:20e124fba426 176
henkiwan 13:20e124fba426 177 JanelaAtivaVFF(pose, Mapa40, pose_f, FCRepul, FCAtracao, RectSize, Forcas);
henkiwan 13:20e124fba426 178
henkiwan 13:20e124fba426 179 ForcaResult[0] = Forcas[0]+Forcas[2]; // componente de x
henkiwan 13:20e124fba426 180 ForcaResult[1] = Forcas[1]+Forcas[3]; // componente de y
henkiwan 13:20e124fba426 181
henkiwan 13:20e124fba426 182 Norma = sqrt(pow(ForcaResult[0],2) + pow(ForcaResult[1],2));
henkiwan 13:20e124fba426 183 ForcaResult[0] = ForcaResult[0]/Norma; // Normalização da forca resultante entre a forca de repulsão e a forca de atracão
henkiwan 13:20e124fba426 184 ForcaResult[1] = ForcaResult[1]/Norma;
henkiwan 13:20e124fba426 185
henkiwan 13:20e124fba426 186 // Angulo da forca resultante
henkiwan 13:20e124fba426 187 angForca = atan2(ForcaResult[1], ForcaResult[0]);
henkiwan 13:20e124fba426 188
henkiwan 13:20e124fba426 189 //pc.printf("FR[0]: %f FR[1]: %f Ang: %f\n\r", ForcaResult[0], ForcaResult[1], angForca);
henkiwan 13:20e124fba426 190 //}
henkiwan 13:20e124fba426 191 //else
henkiwan 13:20e124fba426 192 //angForca = atan2(pose_f[1] - pose[1], pose_f[0] - pose[0]);
henkiwan 13:20e124fba426 193
henkiwan 13:20e124fba426 194
henkiwan 13:20e124fba426 195 PIntermedio[0] = pose[0] + v_erro * cos(angForca);
henkiwan 13:20e124fba426 196 PIntermedio[1] = pose[1] + v_erro * sin(angForca);
henkiwan 13:20e124fba426 197
henkiwan 13:20e124fba426 198 erro = sqrt(pow(PIntermedio[1]-pose[1], 2)+ pow(PIntermedio[0]-pose[0],2)) - DPontos;
henkiwan 13:20e124fba426 199
henkiwan 13:20e124fba426 200 if(erro_old == -1){
henkiwan 13:20e124fba426 201 erro_old = erro;
henkiwan 13:20e124fba426 202 }
henkiwan 13:20e124fba426 203
henkiwan 13:20e124fba426 204 errot = errot + (erro + erro_old) * T/2.0f;
henkiwan 13:20e124fba426 205
henkiwan 13:20e124fba426 206 //Velocidade do Robo
henkiwan 13:20e124fba426 207 vRobot = k_v * erro + k_i * errot;
henkiwan 13:20e124fba426 208
henkiwan 13:20e124fba426 209 erro_old = erro;
henkiwan 13:20e124fba426 210
henkiwan 13:20e124fba426 211 phi = atan2((PIntermedio[1]-pose[1]), (PIntermedio[0]-pose[0])); // Angulo entre o ponto intermedio e o robo
henkiwan 13:20e124fba426 212
henkiwan 13:20e124fba426 213 wRobot = k_s * atan2(sin(phi-pose[2]), cos(phi-pose[2])); //velocidade angular do robo;
henkiwan 13:20e124fba426 214
henkiwan 13:20e124fba426 215 // Velocidades das rodas
henkiwan 13:20e124fba426 216 velRobot2velWheels(vRobot,wRobot,wheelsRadius,wheelsDistance,w);
henkiwan 13:20e124fba426 217
henkiwan 13:20e124fba426 218 // Racio da Velocidade
henkiwan 13:20e124fba426 219 r = w[0]/w[1];
henkiwan 13:20e124fba426 220
henkiwan 13:20e124fba426 221 // nao permite velocidades menores que 50 e maiores que 100 mantendo o racio
henkiwan 13:20e124fba426 222 if(w[0] < 50 || w[1] < 50){
henkiwan 13:20e124fba426 223 if (r > 1){
henkiwan 13:20e124fba426 224 w[1] = 50;
henkiwan 13:20e124fba426 225 w[0] = 50*r;
henkiwan 13:20e124fba426 226 }
henkiwan 13:20e124fba426 227 else if(r < 1){
henkiwan 13:20e124fba426 228 w[0] = 50;
henkiwan 13:20e124fba426 229 w[1] = 50/r;
henkiwan 13:20e124fba426 230 }
henkiwan 13:20e124fba426 231 else{
henkiwan 13:20e124fba426 232 w[0] = 50;
henkiwan 13:20e124fba426 233 w[1] = 50;
henkiwan 13:20e124fba426 234 }
henkiwan 13:20e124fba426 235 }
henkiwan 13:20e124fba426 236
henkiwan 13:20e124fba426 237 if (w[0] > 100 || w[1] > 100){
henkiwan 13:20e124fba426 238 if (r > 1){
henkiwan 13:20e124fba426 239 w[0] = 100;
henkiwan 13:20e124fba426 240 w[1] = 100/r;
henkiwan 13:20e124fba426 241 }
henkiwan 13:20e124fba426 242 else if(r < 1){
henkiwan 13:20e124fba426 243 w[1] = 100;
henkiwan 13:20e124fba426 244 w[0] = 100*r;
henkiwan 13:20e124fba426 245 }
henkiwan 13:20e124fba426 246 else{
henkiwan 13:20e124fba426 247 w[0] = 100;
henkiwan 13:20e124fba426 248 w[1] = 100;
henkiwan 13:20e124fba426 249 }
henkiwan 13:20e124fba426 250 }
henkiwan 13:20e124fba426 251
henkiwan 13:20e124fba426 252
henkiwan 13:20e124fba426 253 /* (w[0] > 200)
henkiwan 13:20e124fba426 254 w[0] = 200;
henkiwan 13:20e124fba426 255
henkiwan 13:20e124fba426 256 if (w[1] > 200)
henkiwan 13:20e124fba426 257 w[1] = 200;*/
henkiwan 13:20e124fba426 258
henkiwan 13:20e124fba426 259 setSpeeds(w[0], w[1]);
henkiwan 13:20e124fba426 260
henkiwan 13:20e124fba426 261 nextPose(countsLeft, countsRight, wheelsRadius, wheelsDistance, pose);
henkiwan 13:20e124fba426 262
henkiwan 13:20e124fba426 263 leituras = 0;
henkiwan 13:20e124fba426 264
henkiwan 13:20e124fba426 265 while(leituras < 10){
henkiwan 13:20e124fba426 266 if(lidar.pollSensorData(&data) == 0)
henkiwan 13:20e124fba426 267 {
henkiwan 13:20e124fba426 268 //pc.printf("%f\t%f\n\r", data.distance, data.angle); // Prints one lidar measurement.
henkiwan 13:20e124fba426 269
henkiwan 13:20e124fba426 270 float radians = (data.angle * static_cast<float>(PI))/180.0f;
henkiwan 13:20e124fba426 271
henkiwan 13:20e124fba426 272 LidarP[0] = -data.distance*sin(radians)- 2.8f;
henkiwan 13:20e124fba426 273 LidarP[1] = -data.distance*cos(radians)- 1.5f;
henkiwan 13:20e124fba426 274
henkiwan 13:20e124fba426 275 //W_P = R_WP * p_P
henkiwan 13:20e124fba426 276 LidarW[0] = LidarP[0]* R_WP[0][0] + LidarP[1]* R_WP[0][1] + R_WP[0][2]; // coordenadas no mundo, ou seja, cm
henkiwan 13:20e124fba426 277 LidarW[1] = LidarP[0]* R_WP[1][0] + LidarP[1]* R_WP[1][1] + R_WP[1][2];
henkiwan 13:20e124fba426 278
henkiwan 13:20e124fba426 279 // pontos onde o feixe passou
henkiwan 13:20e124fba426 280 bresenham(pose[0]/5, pose[1]/5, LidarW[0]/5, LidarW[1]/5, data.distance/5);
henkiwan 13:20e124fba426 281
henkiwan 13:20e124fba426 282 leituras++;
henkiwan 13:20e124fba426 283 }
henkiwan 13:20e124fba426 284 }
henkiwan 13:20e124fba426 285
henkiwan 13:20e124fba426 286 wait(T); // Delay of 0.01 seconds.
henkiwan 12:348038b466a3 287 }
ppovoa 5:bc42c03f2a23 288 }
ppovoa 4:256f2cbe3fdd 289
henkiwan 12:348038b466a3 290 myled=1;
henkiwan 12:348038b466a3 291 setSpeeds(0, 0);
ppovoa 8:ad8766cf2ec0 292 rplidar_motor.write(0.0f);
henkiwan 13:20e124fba426 293
henkiwan 13:20e124fba426 294 /*// Fica a espera do botao
henkiwan 13:20e124fba426 295 start = 0;
henkiwan 13:20e124fba426 296 while(start != 1) {
henkiwan 13:20e124fba426 297 myled=1;
henkiwan 13:20e124fba426 298 if (UserButton == 0) { // Button is pressed
henkiwan 13:20e124fba426 299 myled = 0;
henkiwan 13:20e124fba426 300 start = 1;
henkiwan 13:20e124fba426 301 }
henkiwan 13:20e124fba426 302 }
henkiwan 13:20e124fba426 303
henkiwan 13:20e124fba426 304 //Código para enviar para o matlab
henkiwan 13:20e124fba426 305 for(int i=0; i<40; i++){
henkiwan 13:20e124fba426 306 for(int j=0; j<40; j++){
henkiwan 13:20e124fba426 307 send_odometry(1, 2, j+1, i+1, Mapa40[j][i],10, 30); // faz prints estranhos no Putty
henkiwan 13:20e124fba426 308 }
henkiwan 13:20e124fba426 309 }*/
henkiwan 13:20e124fba426 310
henkiwan 13:20e124fba426 311
henkiwan 9:76b59c5220f1 312 }
henkiwan 9:76b59c5220f1 313
henkiwan 9:76b59c5220f1 314 void bresenham(float poseX, float poseY, float xf, float yf, float z){
henkiwan 9:76b59c5220f1 315 int T, E, A, B;
henkiwan 9:76b59c5220f1 316 int x = static_cast<int>(poseX);
henkiwan 9:76b59c5220f1 317 int y = static_cast<int>(poseY);
henkiwan 12:348038b466a3 318 int dx = abs(static_cast<int>(xf - poseX));
henkiwan 12:348038b466a3 319 int dy = abs(static_cast<int>(yf - poseY));
henkiwan 9:76b59c5220f1 320
henkiwan 13:20e124fba426 321 int s1, s2;
henkiwan 13:20e124fba426 322
henkiwan 13:20e124fba426 323 if (dx == 0)
henkiwan 12:348038b466a3 324 s1 = 0;
henkiwan 12:348038b466a3 325 else
henkiwan 13:20e124fba426 326 s1 = static_cast<int>((xf - poseX)/dx); // substitui o sign() do matlab
henkiwan 12:348038b466a3 327
henkiwan 13:20e124fba426 328 if (dy == 0)
henkiwan 12:348038b466a3 329 s2 = 0;
henkiwan 12:348038b466a3 330 else
henkiwan 13:20e124fba426 331 s2 = static_cast<int>((yf - poseY)/dy);
henkiwan 9:76b59c5220f1 332
henkiwan 9:76b59c5220f1 333 int interchange = 0;
henkiwan 9:76b59c5220f1 334
henkiwan 9:76b59c5220f1 335 if (dy > dx){
henkiwan 9:76b59c5220f1 336 T = dx;
henkiwan 9:76b59c5220f1 337 dx = dy;
henkiwan 9:76b59c5220f1 338 dy = T;
henkiwan 9:76b59c5220f1 339 interchange = 1;
henkiwan 9:76b59c5220f1 340 }
henkiwan 9:76b59c5220f1 341
henkiwan 9:76b59c5220f1 342 E = 2*dy - dx;
henkiwan 9:76b59c5220f1 343 A = 2*dy;
henkiwan 9:76b59c5220f1 344 B = 2*dy - 2*dx;
henkiwan 9:76b59c5220f1 345
henkiwan 9:76b59c5220f1 346 for (int i = 0; i<dx; i++){
henkiwan 9:76b59c5220f1 347 if (E < 0){
henkiwan 9:76b59c5220f1 348 if (interchange == 1){
henkiwan 9:76b59c5220f1 349 y = y + s2;
henkiwan 9:76b59c5220f1 350 }
henkiwan 9:76b59c5220f1 351 else{
henkiwan 9:76b59c5220f1 352 x = x + s1;
henkiwan 9:76b59c5220f1 353 }
henkiwan 9:76b59c5220f1 354 E = E + A;
henkiwan 9:76b59c5220f1 355 }
henkiwan 9:76b59c5220f1 356
henkiwan 9:76b59c5220f1 357 else{
henkiwan 9:76b59c5220f1 358 y = y + s2;
henkiwan 9:76b59c5220f1 359 x = x + s1;
henkiwan 9:76b59c5220f1 360 E = E + B;
henkiwan 9:76b59c5220f1 361 }
henkiwan 9:76b59c5220f1 362
henkiwan 9:76b59c5220f1 363 if (x >= 0 && y >= 0 && x < 40 && y < 40){
henkiwan 9:76b59c5220f1 364 // Mapear mapa do Logaritmo
henkiwan 13:20e124fba426 365 MapaLog[y][x] = MapaLog[y][x] + Algorith_Inverse(poseX, poseY, x, y, z);
ppovoa 10:6c8ea68e9bac 366 //pc.printf("%f %f\n\r", MapaLog[x][y], 1 - 1/(1+exp(MapaLog[x][y])));
henkiwan 13:20e124fba426 367 Mapa40[y][x] = 1 - 1/(1+exp(MapaLog[y][x]));
henkiwan 9:76b59c5220f1 368 }
henkiwan 9:76b59c5220f1 369
ppovoa 4:256f2cbe3fdd 370 }
fabiofaria 1:dc87724abce8 371 }