Diff: main.cpp

Revision:

5:22ad3005bc11

Parent:

4:272090bf74fe

--- a/main.cpp	Wed Apr 28 11:19:28 2021 +0000
+++ b/main.cpp	Fri Apr 30 12:23:33 2021 +0100
@@ -1,41 +1,77 @@
 // Coded by Luís Afonso 11-04-2019
+#include "BufferedSerial.h"
+#include "Robot.h"
 #include "mbed.h"
-#include "BufferedSerial.h"
 #include "rplidar.h"
-#include "Robot.h"
-#include "Communication.h"
+#include <cstdio>
 
-Serial pc(SERIAL_TX, SERIAL_RX);
+BufferedSerial pc(USBTX, USBRX, 115200);
 RPLidar lidar;
 BufferedSerial se_lidar(PA_9, PA_10);
 PwmOut rplidar_motor(D3);
 
-int main()
-{
-    float odomX, odomY, odomTheta;
-    struct RPLidarMeasurement data;
-    
-    pc.baud(115200);
-    init_communication(&pc);
+#define rodaDist 7.0
+#define eixoDist 14.05
+#define PI 3.14159265358979323846f
+
+float distLeft = 0.0f;
+float distRight = 0.0f;
+float distCenter = 0.0f;
+float deltaTheta = 0.0f;
+double x = 0.0f;
+double y = 0.0f;
+double theta = 0.0f;
+float speedL = 0.0f;
+float speedR = 0.0f;
 
-    // Lidar initialization
-    rplidar_motor.period(0.001f);
-    rplidar_motor.write(0.85f);
-    lidar.begin(se_lidar);
-    lidar.setAngle(0,360);
-    setSpeeds(0,0);
+float currentTime = 0.0f;
+float lastTime = 0.0f;
+float timeStep = 0.0f;
+
+void updateOdometer(int countsLeft, int countsRight);
+Timer t;
+using namespace std::chrono;
+
+int main() {
+  float odomX, odomY, odomTheta;
+  // Lidar initialization
+  setSpeeds(100, 100);
+
+  while (1) {
+    getCountsAndReset();
+    updateOdometer(countsLeft, countsRight);
 
-    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.
-        pc.printf("%f\n",rplidar_motor.read());
-        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); 
-    }
+    printf("X: %f\tY: %f\t T: %f\n", x, y, theta);
+    printf("SpeedL: %f\tSpeedR: %f\n", speedL, speedR);
+  }
+}
+
+void updateOdometer(int countsLeft, int countsRight) {
+  currentTime = t.read_us();
+  timeStep = currentTime - lastTime;
+
+  lastTime = currentTime;
+
+  printf("Left: %d\tRight: %d\n", countsLeft, countsRight);
+
+  distLeft = (double)countsLeft * ((rodaDist * PI) / 1440.0);
+  distRight = (double)countsRight * ((rodaDist * PI) / 1440.0);
+  distCenter = (distLeft + distRight) / 2;
+  deltaTheta = (distRight - distLeft) / eixoDist;
+  deltaTheta = atan2(sin(deltaTheta), cos(deltaTheta));
+
+  if (deltaTheta == 0) {
+    x = x + distCenter * cos(theta);
+    y = y + distCenter * sin(theta);
+  } else {
+    x = x + distCenter * sin(theta / 2.0) * cos(theta + deltaTheta / 2.0) /
+                (deltaTheta / 2.0);
+    y = y + distCenter * sin(theta / 2.0) * sin(theta + deltaTheta / 2.0) /
+                (deltaTheta / 2.0);
+    theta += deltaTheta;
+    theta = atan2(sin(theta), cos(theta));
+  }
+
+  speedL = distLeft / float(timeStep / 1E6);
+  speedR = distRight / float(timeStep / 1E6);
 }
\ No newline at end of file