Turn Only Braking Method

include/subsystems/odometry/odometry_base.h

@@ -40,7 +40,7 @@ class OdometryBase {
    * Gets the current position and rotation
    * @return the position that the odometry believes the robot is at
    */
-  pose_t get_position(void);
+  virtual pose_t get_position(void);
 
   /**
    * Sets the current position of the robot

include/subsystems/odometry/odometry_nwheel.h

@@ -9,6 +9,7 @@
 #include "../core/include/subsystems/custom_encoder.h"
 #include "../core/include/subsystems/odometry/odometry_base.h"
 #include "../core/include/utils/math_util.h"
+#include <iostream>
 
 /**
  * OdometryNWheel
@@ -116,6 +117,8 @@ template <int WHEELS> class OdometryNWheel : public OdometryBase {
 
     pose_t updated_pos = calculate_new_pos(radian_deltas, current_pos);
 
+    // std::cout << "x: " << updated_pos.x << " y: " << updated_pos.y << " rot: " << updated_pos.rot << std::endl;
+
     // if we do not pass in an IMU we use the wheels for rotation
     if (imu != nullptr) {
       angle = 0;
@@ -186,7 +189,7 @@ template <int WHEELS> class OdometryNWheel : public OdometryBase {
    * Gets the current position and rotation
    * @return the position that the odometry believes the robot is at
    */
-  pose_t get_position(void) {
+  pose_t get_position(void) override {
     pose_t unwrapped_radians = OdometryBase::get_position();
     pose_t wrapped_degrees = {unwrapped_radians.x, unwrapped_radians.y, wrap_angle_deg((unwrapped_radians.rot / (2 * M_PI)) * 360)};
     return wrapped_degrees;

include/subsystems/tank_drive.h

@@ -26,6 +26,7 @@ class TankDrive {
     None,         ///< just send 0 volts to the motors
     ZeroVelocity, ///< try to bring the robot to rest. But don't try to hold position
     Smart,        ///< bring the robot to rest and once it's stopped, try to hold that position
+    TurnOnly,
   };
   /**
    * Create the TankDrive object

src/subsystems/tank_drive.cpp

@@ -142,11 +142,24 @@ void TankDrive::drive_tank_raw(double left_norm, double right_norm) {
  */
 bool captured_position = false;
 bool was_breaking = false;
+
 void TankDrive::drive_tank(double left, double right, int power, BrakeType bt) {
 
   left = modify_inputs(left, power);
   right = modify_inputs(right, power);
   double brake_threshold = 0.05;
+  if(((left_motors.velocity(vex::velocityUnits::rpm) * (right_motors.velocity(vex::velocityUnits::rpm))) < 0)){
+    bt = BrakeType::ZeroVelocity;
+  }
+  else{
+    bt = BrakeType::None;
+  }
+  if(bt == BrakeType::None){
+    printf("bt None \n");
+  }
+  else if(bt == BrakeType::ZeroVelocity){
+    printf("bt zerovel \n");
+  }
   bool should_brake = (bt != BrakeType::None) && fabs(left) < brake_threshold && fabs(right) < brake_threshold;
 
   if (!should_brake) {