Diff: main.cpp

Revision:

4:1defb279922a

Parent:

3:0a718d139ed1

diff -r 0a718d139ed1 -r 1defb279922a main.cpp
--- 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