Revision 20:d9a373fecd42, committed 2019-04-25

Comitter:

rpatelpj

Date:

Thu Apr 25 22:13:21 2019 +0000

Parent:

19:41d0cc14edc5

Commit message:

Added shortest rotation and conversion constants to RobotController

Changed in this revision

--- a/RobotController.cpp	Thu Apr 25 18:53:13 2019 +0000
+++ b/RobotController.cpp	Thu Apr 25 22:13:21 2019 +0000
@@ -32,13 +32,13 @@
     for (int i = 0; i < 360; i++) {
         _leftEncoder.reset();
         _rightEncoder.reset();
-        leftWheel.speed(0.2);
-        rightWheel.speed(-0.2);
-        Thread::wait(10);
+        leftWheel.speed(0.205);
+        rightWheel.speed(-0.205);
+        Thread::wait(9);
         while((_leftEncoder.read() < 1) && (_rightEncoder.read() < 1));
         leftWheel.speed(0);
         rightWheel.speed(0);
-        obstacles[i] = (int)(lidarDistance / 10);
+        obstacles[i] = (int)(lidarDistance*MMTOCM);
     }
     led = 0;
 }
@@ -55,23 +55,27 @@
         t1 = 0.0;
         t2 = 0.0;
         int angle = trajectory[i] % 360;
+        float leftWheelSpeed = 0.2;
+        float rightWheelSpeed = -0.2;
         if ((trajectory[i] >= 0) && (trajectory[i] <= 90)) {
             angle = angle*ROTERRI;
         } else if (trajectory[i] <= 180) {
             angle = angle*ROTERRII;
-        } else if (trajectory[i] < 360) {
-            angle = angle*ROTERRIII;
+        } else if (trajectory[i] <= 360) {
+            angle = (360 - angle)*ROTERRIII;
+            leftWheelSpeed = -leftWheelSpeed;
+            rightWheelSpeed = -rightWheelSpeed;
         }
         useImu = true;
         t.start();
-        leftWheel.speed(0.2);
-        rightWheel.speed(-0.2);
-        while(yaw > -angle) {
+        leftWheel.speed(leftWheelSpeed);
+        rightWheel.speed(rightWheelSpeed);
+        while(yaw < angle) {
             yaw = yaw + (((w2+w1)/2.0)*(t2-t1));
             while(!imu.gyroAvailable());
             imu.readGyro();
             w1 = w2;
-            w2 = imu.calcGyro(imu.gz);
+            w2 = abs(imu.calcGyro(imu.gz));
             t1 = t2;
             t2 = t.read();
         }
@@ -81,7 +85,7 @@
         useImu = false;
         _leftEncoder.reset();
         _rightEncoder.reset();
-        int distance = (int)(trajectory[i + 1] * COUNTPERCM / 3);
+        int distance = (int)(trajectory[i + 1] * COUNTTOMM);
         leftWheel.speed(0.2);
         rightWheel.speed(0.2);
         while((_leftEncoder.read() < distance) && (_rightEncoder.read() < distance));

--- a/RobotController.h	Thu Apr 25 18:53:13 2019 +0000
+++ b/RobotController.h	Thu Apr 25 22:13:21 2019 +0000
@@ -6,11 +6,11 @@
 #include "Encoder.h"
 #include "LSM9DS1.h"
 
-#define COUNTPERCM 29.538 ///< Encoder counts 192 every 65 mm of wheel rotation
+#define COUNTTOMM 9.846 ///< Encoder count to distance conversion
 #define ROTERRI 0.730 ///< 90 commanded rotation overshoot compensation
 #define ROTERRII 0.770 ///< 180 commanded rotation overshoot compensation
 #define ROTERRIII 0.810 ///< 270, 360 commanded rotation overshoot compensation
-#define MMTOIN 0.0393701 ///< Metric to emperical length conversion
+#define MMTOCM 0.10 ///< Metric to emperical length conversion
 
 /**
  * Robot Controller class.

--- a/globals.h	Thu Apr 25 18:53:13 2019 +0000
+++ b/globals.h	Thu Apr 25 22:13:21 2019 +0000
@@ -18,6 +18,6 @@
 extern int trajectoryLength;
 extern int* trajectory;
 extern uint32_t lidarDistance;
-extern bool useImu;
+extern volatile bool useImu;
 
 #endif /* GLOBALS_H */
\ No newline at end of file

--- a/main.cpp	Thu Apr 25 18:53:13 2019 +0000
+++ b/main.cpp	Thu Apr 25 22:13:21 2019 +0000
@@ -12,7 +12,7 @@
 int trajectoryLength = 0;
 int* trajectory = new int[0];
 uint32_t lidarDistance = 0;
-bool useImu = false;
+volatile bool useImu = false;
 
 void getLidarDistance() {
     DevI2C* lidarDevice = new DevI2C(p28, p27);