![](/media/cache/img/default_profile.jpg.50x50_q85.jpg)
sra-romi
Dependencies: BufferedSerial Matrix
main.cpp
- Committer:
- joaopsousa99
- Date:
- 2021-05-11
- Revision:
- 4:1defb279922a
- Parent:
- 3:0a718d139ed1
File content as of revision 4:1defb279922a:
#include "mbed.h" #include "Robot.h" #include "Communication.h" #include "control.h" //#include <Serial.h> Serial pc(USBTX, USBRX); //PwmOut rplidar_motor(D3); int main() { //pc.baud(115200); //init_communication(&pc); Matrix occupationGrid; occupationGrid = create_occupation_grid(); float xObj = 99.9; float yObj = poseY; float error = 0; float intError = 0; float objAheadX = poseX; float objAheadY = poseY; float objCellCenter[2] = {floor(xObj / CELL_SIZE) * CELL_SIZE + (CELL_SIZE / 2), floor(yObj / CELL_SIZE) * CELL_SIZE + (CELL_SIZE / 2)}; do{ updatePose(); followPath(objAheadX, objAheadY, intError, objCellCenter, occupationGrid, pc); error = sqrt((xObj - poseX)*(xObj - poseX) + (yObj - poseY)*(yObj - poseY)); //printf("error = %f ", error); printf("pose = [%.1f, %.1f]\n", poseX, poseY); wait_us(TIMESTEP*1000000); } while(error > CELL_SIZE); setSpeeds(0, 0); }