Removed the gyro sensor and replaced it with an

inertial sensor

- The gyro wasn't updating properly and we couldn't understand how to
  view the angles it was returning, so we simply replaced it with a
  theoretically more precise inertial sensor
- This includes a calibrateOnce function that calibrates the sensor
  every time a turnTask is called in the task tree
- As it stands now, the robot rotates about 90 degrees in the wrong
  direction before correcting itself 180 degrees in the opposite
  direciton when a 90 degree TurnTask is called, this needs to be fixed
  later

Author: RushilGhosh

2024/prototype/intake_mechanisms/include/autonomous_task_tree.h

@@ -164,12 +164,12 @@ class TurnTask: public Task {
      *
      * @param angleDegrees the amount to rotate in degrees: positive numbers
      * rotate clockwise and negative numbers rotate counterclockwise
-     * @param gyro_ a yaw rate gyroscope used to measure the current chasis's
-     * bearing
+     * @param inertialSensor_ an inertial sensor that we use to measure the
+     *                        robot's yaw angle
      * @param drive a reference to an Idrive object which actually does the
      * driving
      */
-    TurnTask(double angleDegrees, vex::gyro gyro_, Idrive& drive);
+    TurnTask(double angleDegrees, vex::inertial inertialSensor_, Idrive& drive);
 
     bool done() const;
 
@@ -182,7 +182,7 @@ class TurnTask: public Task {
     double startAngle;
 
     double desiredAngle_;
-    vex::gyro gyro_;
+    vex::inertial inertialSensor_;
     Idrive& drive;
     PidController pidController;
 };

2024/prototype/intake_mechanisms/include/robot-config.h

@@ -74,4 +74,7 @@ const double TURN_TASK_EPSILON_DEGREES = 1;
  */
 void vexcodeInit(void);
 
+// Port number for the inertial sensor
+const int INERTIAL_PORT = 20-1;
+
 #endif // (ifndef __ROBOT_CONFIG_INCLUDED__)

2024/prototype/intake_mechanisms/src/autonomous_task_tree.cpp

@@ -15,6 +15,16 @@
 using namespace vex;
 using namespace std;
 
+
+
+void calibrateOnce(vex::inertial& sensor) {
+  static bool initialized = false;
+  if (!initialized) {
+    sensor.calibrate();
+    initialized = true;
+  }
+}
+
 /**
  * Returns the sign of a number.
  * @param number the number to check the sign of
@@ -239,16 +249,16 @@ bool DriveStraightTask::done() const {
      * Definitions for the TurnTask. *
      *********************************/
 
-    TurnTask::TurnTask(
-        double desiredAngle,
-        vex::gyro gyroscope,
-        Idrive& driveObject
-    ) :
-        Task("r"),
+    TurnTask::TurnTask(double desiredAngle,
+                       vex::inertial inertialSensor,
+                       Idrive& driveObject) 
+      : Task("r"),
         desiredAngle_ {desiredAngle},
-        gyro_ {gyroscope},
+        inertialSensor_ {inertialSensor},
         drive {driveObject},
-        pidController {TURN_TASK_P_GAIN, TURN_TASK_I_GAIN, TURN_TASK_D_GAIN} {}
+        pidController {TURN_TASK_P_GAIN, TURN_TASK_I_GAIN, TURN_TASK_D_GAIN} {
+          calibrateOnce(inertialSensor);
+    }
 
     // Scenario: Your robot's start angle is 45 degrees.
     // The desired angle is 40 degrees.
@@ -274,8 +284,6 @@ bool DriveStraightTask::done() const {
     //
 
     void TurnTask::start() {
-        gyro_.resetAngle();
-        startAngle = gyro_.angle();
         // drive.drive(0.0, 0.6);
     }
 
@@ -285,17 +293,17 @@ bool DriveStraightTask::done() const {
     }
 
     bool TurnTask::done() const {
-        double currentAngle = const_cast<TurnTask*>(this)->gyro_.angle();
+        double currentAngle = const_cast<TurnTask*>(this)->inertialSensor_.heading();
         double delta = signedDelta(currentAngle, desiredAngle_);
 
         if (fabs(delta) < TURN_TASK_EPSILON_DEGREES) {
             drive.drive(0, 0);
             return true;
         } else {
-            // double setPoint = currentAngle + delta;
-            // double power = pidController.calculate(currentAngle, setPoint);
-            double power = pidController.calculate(currentAngle, desiredAngle_);
-            drive.drive(0, power);
+            double setPoint = currentAngle + delta;
+            double power = pidController.calculate(0, delta);
+            // double power = pidController.calculate(currentAngle, desiredAngle_);
+            drive.drive(0, -power);
             return false;
         }
     }

2024/prototype/intake_mechanisms/src/main.cpp

@@ -201,7 +201,7 @@ void pre_auton() {
  */
 void autonomous() {
   auto prototype = makePivotRampPrototype();
-  auto gyro = vex::gyro(Seventeen59A.ThreeWirePort.G);
+  vex::inertial inertialSensor(INERTIAL_PORT);
 
   // auto fullAutonRootTask = make_shared<WaitMillisecondsTask>(0);
   // auto B = make_shared<DriveStraightTask>(-0.4572 * 100, prototype);
@@ -256,7 +256,7 @@ void autonomous() {
   //auto simpleRootTask = make_shared<TurnTask>(36.56, gyro, prototype);
 
   auto rootTask = make_shared<WaitMillisecondsTask>(0);
-  auto turnTest = make_shared<TurnTask>(90, gyro, prototype);
+  auto turnTest = make_shared<TurnTask>(90, inertialSensor, prototype);
   // auto DriveTask = make_shared<TestDriveTask>(2, prototype);
   // auto stopTask = make_shared<DriveStraightTask>(0, prototype);
   // addTask(rootTask, DriveTask);