João Sousa / Mbed OS romi-vff

sra-romi

Dependencies:   BufferedSerial Matrix

main.cpp@4:1defb279922a, 2021-05-11 (annotated)

Committer:
joaopsousa99
Date:
Tue May 11 18:10:22 2021 +0000
Revision:
4:1defb279922a
Parent:
3:0a718d139ed1 
as.djvblaskdvj

Who changed what in which revision?

UserRevisionLine numberNew contents of line
LuisRA 0:2b691d200d6f 1 #include "mbed.h"
fabiofaria 1:dc87724abce8 2 #include "Robot.h"
fabiofaria 1:dc87724abce8 3 #include "Communication.h"
joaopsousa99 4:1defb279922a 4 #include "control.h"
joaopsousa99 4:1defb279922a 5 //#include <Serial.h>
LuisRA 0:2b691d200d6f 6
joaopsousa99 4:1defb279922a 7 Serial pc(USBTX, USBRX);
joaopsousa99 4:1defb279922a 8 //PwmOut rplidar_motor(D3);
LuisRA 0:2b691d200d6f 9
joaopsousa99 4:1defb279922a 10 int main() {
joaopsousa99 4:1defb279922a 11 //pc.baud(115200);
joaopsousa99 4:1defb279922a 12 //init_communication(&pc);
joaopsousa99 4:1defb279922a 13 Matrix occupationGrid;
joaopsousa99 4:1defb279922a 14 occupationGrid = create_occupation_grid();
joaopsousa99 4:1defb279922a 15 float xObj = 99.9;
joaopsousa99 4:1defb279922a 16 float yObj = poseY;
joaopsousa99 4:1defb279922a 17 float error = 0;
joaopsousa99 4:1defb279922a 18 float intError = 0;
joaopsousa99 4:1defb279922a 19 float objAheadX = poseX;
joaopsousa99 4:1defb279922a 20 float objAheadY = poseY;
joaopsousa99 4:1defb279922a 21 float objCellCenter[2] = {floor(xObj / CELL_SIZE) * CELL_SIZE + (CELL_SIZE / 2),
joaopsousa99 4:1defb279922a 22 floor(yObj / CELL_SIZE) * CELL_SIZE + (CELL_SIZE / 2)};
LuisRA 0:2b691d200d6f 23
joaopsousa99 4:1defb279922a 24 do{
joaopsousa99 4:1defb279922a 25 updatePose();
joaopsousa99 4:1defb279922a 26 followPath(objAheadX, objAheadY, intError, objCellCenter, occupationGrid, pc);
joaopsousa99 4:1defb279922a 27 error = sqrt((xObj - poseX)*(xObj - poseX) + (yObj - poseY)*(yObj - poseY));
joaopsousa99 4:1defb279922a 28 //printf("error = %f ", error);
joaopsousa99 4:1defb279922a 29 printf("pose = [%.1f, %.1f]\n", poseX, poseY);
joaopsousa99 4:1defb279922a 30 wait_us(TIMESTEP*1000000);
joaopsousa99 4:1defb279922a 31 } while(error > CELL_SIZE);
joaopsousa99 4:1defb279922a 32
joaopsousa99 4:1defb279922a 33 setSpeeds(0, 0);
fabiofaria 1:dc87724abce8 34 }