![](/media/cache/img/default_profile.jpg.50x50_q85.jpg)
sra-romi
Dependencies: BufferedSerial Matrix
Diff: main.cpp
- Revision:
- 4:1defb279922a
- Parent:
- 3:0a718d139ed1
--- a/main.cpp Wed Apr 24 16:00:07 2019 +0000 +++ b/main.cpp Tue May 11 18:10:22 2021 +0000 @@ -1,38 +1,34 @@ -// Coded by Luís Afonso 11-04-2019 #include "mbed.h" -#include "BufferedSerial.h" -#include "rplidar.h" #include "Robot.h" #include "Communication.h" - -Serial pc(SERIAL_TX, SERIAL_RX); -RPLidar lidar; -BufferedSerial se_lidar(PA_9, PA_10); -PwmOut rplidar_motor(D3); +#include "control.h" +//#include <Serial.h> -int main() -{ - float odomX, odomY, odomTheta; - struct RPLidarMeasurement data; - - pc.baud(115200); - init_communication(&pc); +Serial pc(USBTX, USBRX); +//PwmOut rplidar_motor(D3); - // Lidar initialization - rplidar_motor.period(0.001f); - lidar.begin(se_lidar); - lidar.setAngle(0,360); +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)}; - pc.printf("Program started.\n"); - - lidar.startThreadScan(); - - while(1) { - // poll for measurements. Returns -1 if no new measurements are available. returns 0 if found one. - if(lidar.pollSensorData(&data) == 0) - { - pc.printf("%f\t%f\t%d\t%c\n", data.distance, data.angle, data.quality, data.startBit); // Prints one lidar measurement. - } - wait(0.01); - } + 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); } \ No newline at end of file