Better lift variable names

Competition/src/main/cpp/Robot.cpp

@@ -7,6 +7,8 @@
 
 #include "Robot.h"
 
+
+
 #include <frc/smartdashboard/SmartDashboard.h>
 #include <frc2/command/CommandScheduler.h>
 
@@ -37,7 +39,7 @@ void Robot::DisabledInit() {
     m_container.m_drivetrain.robotMode = ValorSubsystem::RobotMode::DISABLED;
     m_container.m_shooter.robotMode = ValorSubsystem::RobotMode::DISABLED;
     m_container.m_turretTracker.robotMode = ValorSubsystem::RobotMode::DISABLED;
-    //m_container.m_lift.robotMode = ValorSubsystem::RobotMode::DISABLED; //just added, not tested
+    m_container.m_lift.robotMode = ValorSubsystem::RobotMode::DISABLED; //just added, not tested
 
     m_container.m_shooter.resetState();
     m_container.m_drivetrain.resetState();
@@ -72,6 +74,7 @@ void Robot::AutonomousInit() {
     m_container.m_shooter.robotMode = ValorSubsystem::RobotMode::AUTO;
     m_container.m_lift.robotMode = ValorSubsystem::RobotMode::AUTO;
     m_container.m_turretTracker.robotMode = ValorSubsystem::RobotMode::AUTO;
+    m_container.m_turretTracker.disableWrapAround();
 
     m_container.m_drivetrain.pullSwerveModuleZeroReference();
 }
@@ -94,8 +97,13 @@ void Robot::TeleopInit() {
     m_container.m_feeder.robotMode = ValorSubsystem::RobotMode::TELEOP;
     m_container.m_drivetrain.robotMode = ValorSubsystem::RobotMode::TELEOP;
     m_container.m_shooter.robotMode = ValorSubsystem::RobotMode::TELEOP;
+    m_container.m_shooter.state.turretState = m_container.m_shooter.TURRET_TRACK;
+    m_container.m_shooter.state.hoodState = m_container.m_shooter.HOOD_TRACK;
+    m_container.m_shooter.state.flywheelState = m_container.m_shooter.FLYWHEEL_TRACK;
+
     m_container.m_lift.robotMode = ValorSubsystem::RobotMode::TELEOP;
     m_container.m_turretTracker.robotMode = ValorSubsystem::RobotMode::TELEOP; 
+    m_container.m_turretTracker.enableWrapAround();
 
 }
 

Competition/src/main/cpp/RobotContainer.cpp

@@ -7,7 +7,7 @@
 
 #include "RobotContainer.h"
 
-RobotContainer::RobotContainer() : m_auto(&m_drivetrain, &m_shooter, &m_feeder) {
+RobotContainer::RobotContainer() : m_auto(&m_drivetrain, &m_shooter, &m_feeder, &m_turretTracker) {
     ConfigureButtonBindings();
     m_shooter.setDrivetrain(&m_drivetrain);
     m_turretTracker.setDrivetrain(&m_drivetrain);

Competition/src/main/cpp/ValorGamepad.cpp

@@ -0,0 +1,118 @@
+#include "ValorGamepad.h"
+
+#include <math.h>
+
+#define DPAD_UP 0
+#define DPAD_DOWN 180
+#define DPAD_RIGHT 90
+#define DPAD_LEFT 270
+
+#define DEADBAND_X 0.05f
+#define DEADBAND_Y 0.1f
+#define DEADBAND_TRIGGER 0.05f
+
+ValorGamepad::ValorGamepad(int id) :
+    frc::XboxController(id),
+    deadbandX(DEADBAND_X),
+    deadbandY(DEADBAND_Y)
+{
+}
+
+void ValorGamepad::setDeadbandX(double deadband)
+{
+    deadbandX = deadband;
+}
+
+void ValorGamepad::setDeadbandY(double deadband)
+{
+    deadbandY = deadband;
+}
+
+// Get the sign of an input
+template <typename T> int sgn(T val) {
+    return (T(0) < val) - (val < T(0));
+}
+
+double ValorGamepad::deadband(double input, double deadband, int polynomial)
+{
+    // If input is negative and polynomial is even, output would be positive which is incorrect
+    // Therefore if polynomial is even, multiply pow by the sign of the input
+    return std::fabs(input) > deadband ? ((polynomial % 2 == 0 ? sgn(input) : 1) * std::pow(input, polynomial)) : 0;
+}
+
+double ValorGamepad::leftStickX(int polynomial)
+{
+    return -deadband(GetLeftX(), deadbandX, polynomial);
+}
+
+bool ValorGamepad::leftStickXActive(int polynomial)
+{
+    return leftStickX(polynomial) != 0;
+}
+
+double ValorGamepad::leftStickY(int polynomial)
+{
+    return -deadband(GetLeftY(), deadbandY, polynomial);
+}
+
+bool ValorGamepad::leftStickYActive(int polynomial)
+{
+    return leftStickY(polynomial) != 0;
+}
+
+double ValorGamepad::rightStickX(int polynomial)
+{
+    return -deadband(GetRightX(), deadbandX, polynomial);
+}
+
+bool ValorGamepad::rightStickXActive(int polynomial)
+{
+    return rightStickX(polynomial) != 0;
+}
+
+double ValorGamepad::rightStickY(int polynomial)
+{
+    return -deadband(GetRightY(), deadbandY, polynomial);
+}
+
+bool ValorGamepad::rightStickYActive(int polynomial)
+{
+    return rightStickY(polynomial) != 0;
+}
+
+double ValorGamepad::leftTrigger()
+{
+    return GetLeftTriggerAxis() > DEADBAND_TRIGGER ? GetLeftTriggerAxis() : 0;
+}
+
+bool ValorGamepad::leftTriggerActive()
+{
+    return leftTrigger() != 0;
+}
+
+double ValorGamepad::rightTrigger()
+{
+    return GetRightTriggerAxis() > DEADBAND_TRIGGER ? GetRightTriggerAxis() : 0;
+}
+
+bool ValorGamepad::rightTriggerActive()
+{
+    return rightTrigger() != 0;
+}
+
+bool ValorGamepad::DPadUp()
+{
+    return GetPOV() == DPAD_UP;
+}
+bool ValorGamepad::DPadDown()
+{
+    return GetPOV() == DPAD_DOWN;
+}
+bool ValorGamepad::DPadLeft()
+{
+    return GetPOV() == DPAD_LEFT;
+}
+bool ValorGamepad::DPadRight()
+{
+    return GetPOV() == DPAD_RIGHT;
+}

Competition/src/main/cpp/ValorSubsystem.cpp

@@ -32,7 +32,7 @@ ValorSubsystem& ValorSubsystem::GetInstance() {
 
 void ValorSubsystem::init() {
     // init subsystem
-    std::cout << "init valor subsytem" << std::endl;
+    //std::cout << "init valor subsytem" << std::endl;
 }
 
 void ValorSubsystem::analyzeDashboard() {

Competition/src/main/cpp/sensors/ValorCurrentSensor.cpp

@@ -0,0 +1,42 @@
+#include "sensors/ValorCurrentSensor.h"
+
+#define CACHE_SIZE 20
+#define DEFAULT_SPIKE_VALUE 22
+
+ValorCurrentSensor::ValorCurrentSensor() :
+    spikedSetpoint(DEFAULT_SPIKE_VALUE)
+{
+    reset();
+}
+
+void ValorCurrentSensor::setSpikeSetpoint(double _setpoint)
+{
+    spikedSetpoint = _setpoint;
+}
+
+bool ValorCurrentSensor::spiked() {
+    return currState > spikedSetpoint;
+}
+
+void ValorCurrentSensor::reset() {
+    cache.clear();
+    for (int i = 0; i < CACHE_SIZE; i++) {
+        cache.push_back(0);
+    }
+    prevState = 0;
+    currState = 0;
+}
+
+void ValorCurrentSensor::calculate() {
+    cache.pop_front();
+    cache.push_back(getSensor());
+
+    // Calculate average current over the cache size, or circular buffer window
+    double sum = 0;
+    for (int i = 0; i < CACHE_SIZE; i++) {
+        sum += cache.at(i);
+    }
+
+    currState = sum / CACHE_SIZE;
+    prevState = currState;
+}

Competition/src/main/cpp/sensors/ValorDebounceSensor.cpp

@@ -0,0 +1,24 @@
+#include "sensors/ValorDebounceSensor.h"
+
+ValorDebounceSensor::ValorDebounceSensor()
+{
+}
+
+void ValorDebounceSensor::reset()
+{
+    prevState = false;
+    currState = false;
+    isSpiked = false;
+}
+
+bool ValorDebounceSensor::spiked()
+{
+    return isSpiked;
+}
+
+void ValorDebounceSensor::calculate()
+{
+    currState = getSensor();
+    isSpiked = currState != prevState;
+    prevState = currState;
+}

Competition/src/main/cpp/subsystems/Drivetrain.cpp

@@ -16,7 +16,7 @@
 
 Drivetrain::Drivetrain() : ValorSubsystem(),
                            driverController(NULL),
-                           pigeon(DriveConstants::PIGEON_CAN),
+                           pigeon(DriveConstants::PIGEON_CAN, "baseCAN"),
                            kinematics(motorLocations[0], motorLocations[1], motorLocations[2], motorLocations[3]),
                            odometry(kinematics, frc::Rotation2d{units::radian_t{0}}),
                            config(units::velocity::meters_per_second_t{SwerveConstants::AUTO_MAX_SPEED_MPS}, units::acceleration::meters_per_second_squared_t{SwerveConstants::AUTO_MAX_ACCEL_MPSS}),
@@ -29,7 +29,7 @@ Drivetrain::Drivetrain() : ValorSubsystem(),
 
 void Drivetrain::setKF(){
     azimuthMotors[0]->Config_kF(0, SwerveConstants::KF);
-    std::cout << "set kf" << std::endl;
+    //std::cout << "set kf" << std::endl;
 }
 
 Drivetrain::~Drivetrain()
@@ -45,7 +45,7 @@ Drivetrain::~Drivetrain()
 
 void Drivetrain::configSwerveModule(int i)
 {
-    azimuthMotors.push_back(new WPI_TalonFX(DriveConstants::AZIMUTH_CANS[i]));
+    azimuthMotors.push_back(new WPI_TalonFX(DriveConstants::AZIMUTH_CANS[i], "baseCAN"));
     azimuthMotors[i]->ConfigFactoryDefault();
     azimuthMotors[i]->SetInverted(false);
     azimuthMotors[i]->ConfigSelectedFeedbackSensor(FeedbackDevice::IntegratedSensor, 0, 10);
@@ -60,9 +60,10 @@ void Drivetrain::configSwerveModule(int i)
     azimuthMotors[i]->SetNeutralMode(NeutralMode::Brake);
     //azimuthMotors[i]->ConfigSupplyCurrentLimit(SupplyCurrentLimitConfiguration(true, 60, 80, .75));
 
-    driveMotors.push_back(new WPI_TalonFX(DriveConstants::DRIVE_CANS[i]));
+    driveMotors.push_back(new WPI_TalonFX(DriveConstants::DRIVE_CANS[i], "baseCAN"));
     driveMotors[i]->ConfigFactoryDefault();
     driveMotors[i]->SetInverted(false);
+
     driveMotors[i]->ConfigSelectedFeedbackSensor(FeedbackDevice::IntegratedSensor, 0, 10);
     driveMotors[i]->SetNeutralMode(NeutralMode::Coast);
     driveMotors[i]->ConfigSupplyCurrentLimit(SupplyCurrentLimitConfiguration(true, 60, 80, .75));
@@ -97,11 +98,11 @@ void Drivetrain::init()
     // }
 
     resetState();
-    //pullSwerveModuleZeroReference();
-    std::cout <<"init drivetrain" << std::endl;
+    // pullSwerveModuleZeroReference();
+    // std::cout <<"init drivetrain" << std::endl;
 }
 
-void Drivetrain::setController(frc::XboxController *controller)
+void Drivetrain::setController(ValorGamepad *controller)
 {
     driverController = controller;
 }
@@ -133,22 +134,9 @@ void Drivetrain::assessInputs()
         return;
     }
 
-    // driver inputs
-    state.leftStickX = driverController->GetLeftX();
-    state.leftStickY = driverController->GetLeftY();
-    state.rightStickX = driverController->GetRightX();
-    state.rightStickY = driverController->GetRightY();
-
-    state.bButtonPressed = driverController->GetBButton();
-    state.aButtonPressed = driverController->GetAButton();
-    state.xButtonPressed = driverController->GetXButton();
-    state.yButtonPressed = driverController->GetYButton();
-
-    state.startButtonPressed = driverController->GetStartButtonPressed();
-
-    state.stickPressed = std::abs(state.leftStickY) > 0.05 || 
-    std::abs(state.leftStickX) > 0.05 ||
-    std::abs(state.rightStickX) > 0.05;
+    state.stickPressed = driverController->leftStickYActive() || 
+                         driverController->leftStickXActive() ||
+                         driverController->rightStickXActive();
 
     //state.dPadDownPressed = driverController->GetPOV(frc::GenericHID::)
 
@@ -157,9 +145,6 @@ void Drivetrain::assessInputs()
 
 void Drivetrain::analyzeDashboard()
 {
-    state.backButtonPressed = driverController->GetBackButtonPressed();
-
-
     table->PutNumber("Robot X", getPose_m().X().to<double>());
     table->PutNumber("Robot Y", getPose_m().Y().to<double>());
     table->PutNumber("Robot Theta", getPose_m().Rotation().Degrees().to<double>());
@@ -206,7 +191,7 @@ void Drivetrain::analyzeDashboard()
                     swerveModules[2]->getState(),
                     swerveModules[3]->getState());
 
-    if (state.backButtonPressed){
+    if (driverController->GetBackButtonPressed()) {
         resetGyro();
     }
 }
@@ -226,33 +211,32 @@ void Drivetrain::assignOutputs()
 {
     // Get the x speed. We are inverting this because Xbox controllers return
     // negative values when we push forward.
-    double xSpeed = std::abs(state.leftStickY) > OIConstants::kDeadbandY ? fabs(state.leftStickY) * -state.leftStickY : 0 ;
+    double xSpeed = driverController->leftStickY(2);
 
     // Get the y speed or sideways/strafe speed. We are inverting this because
     // we want a positive value when we pull to the left. Xbox controllers
     // return positive values when you pull to the right by default.
-    double ySpeed = std::abs(state.leftStickX) > OIConstants::kDeadbandX ? fabs(state.leftStickX) * -state.leftStickX : 0;
+    double ySpeed = driverController->leftStickX(2);
 
     // Get the rate of angular rotation. We are inverting this because we want a
     // positive value when we pull to the left (remember, CCW is positive in
     // mathematics). Xbox controllers return positive values when you pull to
     // the right by default.
-
-    double rot = std::abs(state.rightStickX) > OIConstants::kDeadbandX ? -1 * state.rightStickX * state.rightStickX * state.rightStickX : 0;
+    double rot = driverController->rightStickX(3);
 
     units::meters_per_second_t xSpeedMPS = units::meters_per_second_t{xSpeed * SwerveConstants::DRIVE_MAX_SPEED_MPS};
     units::meters_per_second_t ySpeedMPS = units::meters_per_second_t{ySpeed * SwerveConstants::DRIVE_MAX_SPEED_MPS};
     units::radians_per_second_t rotRPS = units::radians_per_second_t{rot * SwerveConstants::ROTATION_MAX_SPEED_RPS};
 
-    if(state.aButtonPressed){
+    if(driverController->GetAButton()){
         double magnitude = std::sqrt(std::pow(xSpeed, 2) + std::pow(ySpeed, 2));
         xSpeed /= magnitude;
         ySpeed /= magnitude;
         xSpeedMPS = units::meters_per_second_t{xSpeed};
         ySpeedMPS = units::meters_per_second_t{ySpeed};
         if(rot != 0){
             int sign = std::signbit(rot) == 0 ? 1 : -1;
-            rotRPS = units::radians_per_second_t{SwerveConstants::ROTATION_SLOW_SPEED_RPS};
+            rotRPS = units::radians_per_second_t{rot * SwerveConstants::ROTATION_SLOW_SPEED_RPS};
         }
     }
     // double heading = getPose_m().Rotation().Degrees().to<double>();
@@ -271,7 +255,7 @@ void Drivetrain::assignOutputs()
         rotRPS = units::radians_per_second_t{-limeTable->GetNumber("tx", 0.0) * DriveConstants::LIMELIGHT_KP * limeTable->GetNumber("tv", 0) * SwerveConstants::ROTATION_MAX_SPEED_RPS};
     }
 
-    if (state.startButtonPressed){
+    if (driverController->GetStartButtonPressed()) {
         // x0y0 = frc::Pose2d(8.514_m, 1.771_m, frc::Rotation2d(182.1_deg));
 
         // goZeroZero = frc::TrajectoryGenerator::GenerateTrajectory(