Feat/more parameterization

src/main/cpp/Robot.cpp

@@ -9,7 +9,8 @@
 #include <cameraserver/CameraServer.h>
 
 void Robot::RobotInit() {
-    frc::CameraServer::StartAutomaticCapture().SetResolution(480, 240);
+    double res = RobotParams::GetParam("camera.res", 240);
+    frc::CameraServer::StartAutomaticCapture().SetResolution(2* res, res);
 
     m_container.GetArm()->SetControlMode(Auto);
 }

src/main/cpp/RobotContainer.cpp

@@ -327,19 +327,29 @@ bool RobotContainer::InitCommands() {
     score_preload_balance_auto_ = std::make_shared<ScorePreloadedBalanceAuto>(swerve_drive_, arm_, intake_);
 
     // swerve commands
-    swerve_teleop_command_ = std::make_shared<TeleOpSwerveCommand>(swerve_drive_, gamepad1_, 0.8, 0.5, false); // speed mod, open loop
-    slow_swerve_teleop_command_ = std::make_shared<TeleOpSwerveCommand>(swerve_drive_, gamepad1_, 0.5, 1, true); // brake mode
-    fast_swerve_teleop_command_ = std::make_shared<TeleOpSwerveCommand>(swerve_drive_, gamepad1_, 1.5, 0.1, false); // BOOOOOOOOOST
+    double speed_mod = RobotParams::GetParam("swerve.speed_mod", 0.8);
+    double open_loop = RobotParams::GetParam("swerve.swerve_open_loop", 0.5);
+    double braking_speed_mod = RobotParams::GetParam("swerve.braking_speed_mod", 0.5);
+    double braking_open_loop = RobotParams::GetParam("swerve.swerve_braking_open_loop", 1);
+    double fast_speed_mod = RobotParams::GetParam("swerve.fast_speed_mod", 1.5);
+    double fast_open_loop = RobotParams::GetParam("swerve.swerve_fast_open_loop", 0.1);
+
+    swerve_teleop_command_ = std::make_shared<TeleOpSwerveCommand>(swerve_drive_, gamepad1_, speed_mod, open_loop, false); // speed mod, open loop
+    slow_swerve_teleop_command_ = std::make_shared<TeleOpSwerveCommand>(swerve_drive_, gamepad1_, braking_speed_mod, braking_open_loop, true); // brake mode
+    fast_swerve_teleop_command_ = std::make_shared<TeleOpSwerveCommand>(swerve_drive_, gamepad1_, fast_speed_mod, fast_open_loop, false); // BOOOOOOOOOST
 
     reset_gyro_command_ = std::make_shared<ResetGyroCommand>(swerve_drive_);
     OKC_CHECK(swerve_teleop_command_ != nullptr);
 
     // test arm commands
-    extendArmCommand = std::make_shared<IncrementArmExtendCommand>(arm_, 0.5); 
-    retractArmCommand = std::make_shared<IncrementArmExtendCommand>(arm_, -0.5);
+    double extend = RobotParams::GetParam("arm.extend", 0.5);
+    double position_increment = RobotParams::GetParam("arm.position_increment", 0.5);
+
+    extendArmCommand = std::make_shared<IncrementArmExtendCommand>(arm_, extend); 
+    retractArmCommand = std::make_shared<IncrementArmExtendCommand>(arm_, -extend);
 
-    raiseArmCommand = std::make_shared<IncrementArmPresetPositionCommand>(arm_, 0.5);
-    lowerArmCommand = std::make_shared<IncrementArmPresetPositionCommand>(arm_, -0.5);
+    raiseArmCommand = std::make_shared<IncrementArmPresetPositionCommand>(arm_, position_increment);
+    lowerArmCommand = std::make_shared<IncrementArmPresetPositionCommand>(arm_, -position_increment);
 
     // arm commands
     arm_carry_command_ = std::make_shared<ArmSetStateCommand>(arm_, TeamOKC::ArmState(1, 0));
@@ -351,9 +361,12 @@ bool RobotContainer::InitCommands() {
     arm_human_player_command_ = std::make_shared<ArmFieldOrientedCommand>(arm_, swerve_drive_, TeamOKC::ArmState(negative_human_player_extension_, negative_human_player_rotation_), TeamOKC::ArmState(human_player_extension_, human_player_rotation_));
 
     // intake commands
-    intake_command = std::make_shared<IntakeCommand>(intake_, 0.7);
-    other_intake_command = std::make_shared<IntakeCommand>(intake_, -0.7);
-    stop_intake_command = std::make_shared<IntakeCommand>(intake_, -0.03);
+    double intake_power = RobotParams::GetParam("intake.intake_power", 0.7);
+    double idle_intake_power = RobotParams::GetParam("intake.idle_power", -0.03);
+
+    intake_command = std::make_shared<IntakeCommand>(intake_, intake_power);
+    other_intake_command = std::make_shared<IntakeCommand>(intake_, -intake_power);
+    stop_intake_command = std::make_shared<IntakeCommand>(intake_, idle_intake_power);
 
     return true;
 }

src/main/cpp/SwerveModule.cpp

@@ -19,8 +19,10 @@ bool SwerveModule::Init(Location loc) {
     steer_pid_ = std::make_shared<frc::PIDController>(steer_kP, steer_kI, steer_kD);
 
     OKC_CHECK(this->steer_pid_ != nullptr);
-    steer_pid_->EnableContinuousInput(-180, 180);
-    steer_pid_->SetTolerance(2, 2); // tolerate 2 degrees of deviation, which shouldn't be a lot I don't think
+    double steer_pid_degrees = RobotParams::GetParam("swerve.steer_pid.degrees", 180);
+    double steer_pid_degrees_tolerance = RobotParams::GetParam("swerve.steer_pid.tolerance", 2);
+    steer_pid_->EnableContinuousInput(-steer_pid_degrees, steer_pid_degrees);
+    steer_pid_->SetTolerance(steer_pid_degrees_tolerance, steer_pid_degrees_tolerance); // tolerate 2 degrees of deviation, which shouldn't be a lot I don't think
 
     // units and conversions and numbers and stuff
     L2_GEAR_RATIO_ = RobotParams::GetParam("swerve.l2_gear_ratio", 6.75);

src/main/cpp/autos/ScorePreloadedNoDriveAuto.cpp

@@ -22,6 +22,6 @@ ScorePreloadedNoDriveAuto::ScorePreloadedNoDriveAuto(std::shared_ptr<Arm> arm, s
         frc2::WaitCommand(units::second_t(1)), // wait for cube to be dropped
         IntakeCommand(intake, 0), // stop the intake
         ArmSetStateCommand(arm, TeamOKC::ArmState(0.5, 0)), // bring the arm back in the robot
-        frc2::WaitCommand(units::second_t(4)) // wait a second so the arm is mostly in
+        frc2::WaitCommand(units::second_t(4)) // wait a second so the arm is mostly in0
     );
 }

src/main/cpp/subsystems/Arm.cpp

@@ -11,7 +11,10 @@ bool Arm::Init() {
     double arm_kD = RobotParams::GetParam("arm.lift_pid.kD", 0.0);
     double arm_kF = RobotParams::GetParam("arm.lift_pid.kF", 0.05);
     arm_pid_ = std::make_shared<frc::PIDController>(arm_kP, arm_kI, arm_kD);
-    arm_pid_->SetTolerance(2.5, 3.0);
+
+    double arm_pos_tol = RobotParams::GetParam("arm.arm_pos_tolerance", 2.5);
+    double arm_vel_tol = RobotParams::GetParam("arm.arm_vel_tolerance", 3.0);
+    arm_pid_->SetTolerance(arm_pos_tol, arm_vel_tol);
     arm_kF_ = arm_kF;
 
     double extend_kP = RobotParams::GetParam("arm.extend_pid.kP", 0.005);
@@ -35,6 +38,7 @@ bool Arm::Init() {
     // pull limits from the parameters file
     lift_limit_ = RobotParams::GetParam("arm.lift_limit", 100.0);
     extend_limit_ = RobotParams::GetParam("arm.extend_limit", 100.0);
+    double calibration_power_ = RobotParams::GetParam("arm.extend_power", 69);
 
     // initialize with default state
     state_ = TeamOKC::ArmState(0, 0);
@@ -208,7 +212,7 @@ bool Arm::AutoControl() {
             std::cout << "CALIBRATION COMPLETE" << std::endl;
         } else {
             // otherwise, we haven't hit it yet, so set the motor to a small negative power until we do
-            this->interface_->arm_extend_power = -0.1;
+            this->interface_->arm_extend_power = calibration_power_;
         }
 
         return true;
@@ -226,7 +230,7 @@ bool Arm::AutoControl() {
         this->inches_pid_->SetSetpoint(0.5);
 
         // if we have brought the extension in
-        if (abs(1.0 - state_.extension) < 1.0) {
+        if (inches_pid_->AtSetpoint()) {
             // then move the arm
             this->arm_pid_->SetSetpoint(this->desired_state_.rotation);
             this->interface_->arm_lift_power = this->arm_pid_->Calculate(this->interface_->arm_duty_cycle_encoder);
@@ -237,7 +241,7 @@ bool Arm::AutoControl() {
             this->interface_->arm_extend_power = this->inches_pid_->Calculate(this->interface_->arm_extend_encoder);
 
             // if we've reached our desired rotation
-            if (abs(desired_state_.rotation - state_.rotation) < 2) {
+            if (arm_pid_->AtSetpoint()) {
                 // move to the next stage
                 this->control_state_ = EXTENDING;
             }
@@ -290,9 +294,6 @@ void Arm::Periodic() {
         case Auto:
             VOKC_CALL(this->AutoControl());
             break;
-        case Manual:
-            VOKC_CALL(this->ManualControl());
-            break;
         default:
             VOKC_CHECK_MSG(false, "Unhandled enum");
     }

src/main/cpp/subsystems/SwerveDrive.cpp

@@ -8,6 +8,12 @@ bool SwerveDrive::Init() {
     // Initialize Shuffleboard from parameters.
     OKC_CALL(InitShuffleboard());
 
+    turn_deadband_ = RobotParams::GetParam("swerve.turn_deadband", 0.3);
+    strafe_deadband_ = RobotParams::GetParam("swerve.strafe_deadband", 0.15);
+    drive_deadband_ = RobotParams::GetParam("swerve.drive_deadband", 0.1);
+    invert_threshold_ = RobotParams::GetParam("swerve.invert_threshold", 100);
+    center_deadband_ = RobotParams::GetParam("swerve.center_deadband", 0.05);
+
     left_front_setpoint_log_ = wpi::log::DoubleLogEntry(TeamOKC::log, "/swerve/setpoint");
     left_front_output_log_ = wpi::log::DoubleLogEntry(TeamOKC::log, "/swerve/output");
     left_front_steer_enc_log_ = wpi::log::DoubleLogEntry(TeamOKC::log, "/swerve/steer_enc");
@@ -158,17 +164,17 @@ bool SwerveDrive::VectorTeleOpDrive(const double &drive, const double &strafe, c
     double turn_ = turn;
 
     // if turn is very small
-    if (abs(turn) < 0.3) {
+    if (abs(turn) < turn_deadband_) {
         turn_ = 0.0; // then zero it
     // } else if (final_turn)
     }
 
     // if strafe is very small
-    if (abs(strafe) < 0.15) {
+    if (abs(strafe) < strafe_deadband_) {
         strafe_ = 0.0; // then zero it
     }
 
-    if (abs(drive) < 0.1) {
+    if (abs(drive) < drive_deadband_) {
         drive_ = 0.0;
     }
 
@@ -225,22 +231,22 @@ bool SwerveDrive::VectorTeleOpDrive(const double &drive, const double &strafe, c
      * but it would be better to simply invert the direction that the drive motor/wheel is spinning, like normal
      * differential drivetrains do. So you should never need to steer more than 90 degrees.
     */
-    if (abs(left_front_angle - left_front_turn) > 100) {
+    if (abs(left_front_angle - left_front_turn) > invert_threshold_) {
         left_front_turn -= 180;
         left_front_speed *= -1;
     }
 
-    if (abs(left_back_angle - left_back_turn) > 100) {
+    if (abs(left_back_angle - left_back_turn) > invert_threshold_) {
         left_back_turn -= 180;
         left_back_speed *= -1;
     }
 
-    if (abs(right_front_angle - right_front_turn) > 100) {
+    if (abs(right_front_angle - right_front_turn) > invert_threshold_) {
         right_front_turn -= 180;
         right_front_speed *= -1;
     }
 
-    if (abs(right_back_angle - right_back_turn) > 100) {
+    if (abs(right_back_angle - right_back_turn) > invert_threshold_) {
         right_back_turn -= 180;
         right_back_speed *= -1;
     }
@@ -264,14 +270,14 @@ bool SwerveDrive::VectorTeleOpDrive(const double &drive, const double &strafe, c
     // really nice convoluted deadband
     // this is to stop the swerve modules from immediately trying to center themselves instead of
     // coasting until receiving another instruction so we don't tip
-    if (abs(drive_) > 0.05 || abs(strafe_) > 0.05 || abs(turn_) > 0.3) {
+    if (abs(drive_) > center_deadband_ || abs(strafe_) > center_deadband_ || abs(turn_) > center_deadband_) {
         OKC_CALL(this->left_front_module_->SetAngle(left_front_turn));
         OKC_CALL(this->left_back_module_->SetAngle(left_back_turn));
         OKC_CALL(this->right_front_module_->SetAngle(right_front_turn));
         OKC_CALL(this->right_back_module_->SetAngle(right_back_turn));
     }
 
-    if (abs(drive_) < 0.05 && abs(strafe_) < 0.05 && abs(turn_) < 0.3) {
+    if (abs(drive_) < center_deadband_ && abs(strafe_) < center_deadband_ && abs(turn_) < center_deadband_) {
         this->interface_->left_front_drive_motor_output = 0.0;
         this->interface_->left_back_drive_motor_output = 0.0;
         this->interface_->right_front_drive_motor_output = 0.0;

src/main/deploy/parameters.toml

@@ -25,6 +25,17 @@ drive_open_loop = 1
 steer_max_output = 1
 steer_open_loop = 0.0
 slew_limit = 2
+turn_deadband_ = 0.3
+strafe_deadband_ = 0.15
+drive_deadband_ = 0.1
+invert_threshold_ = 100.0
+center_deadband_ = 0.05
+speed_mod = 0.8
+swerve_open_loop = 0.5
+braking_speed_mod = 0.5
+braking_open_loop = 1
+fast_speed_mod = 1.5
+fast_swerve_open_loop = 0.1
 
 [swerve.dist_pid]
 kP = 0.5
@@ -41,6 +52,8 @@ kD = 0.0
 kP = 0.005
 kI = 0.0
 kD = 0.000005
+degrees = 180
+tolerance = 2
 
 [swerve.heading_pid]
 kP = 0.04
@@ -70,6 +83,11 @@ extend_limit = 95
 max_output = 0.7
 arm_open_loop = 10
 extend_open_loop = 10
+arm_pos_tolerance =  2.5
+arm_vel_tolerance =  3.0
+extend = 0.5
+position_increment
+extend_power = 69
 
 # right in front of the robot to pick stuff up
 [arm.pickup]
@@ -135,8 +153,13 @@ kD = 0
 
 [intake]
 tilt_offset = -55.0
+intake_power = 0.7
+idle_power = -0.03
 
 [intake.wrist_pid]
 kP = 0.005
 kI = 0.0
-kD = 0.0
+kD = 0.0
+
+[camera]
+res = 240

src/main/include/subsystems/Arm.h

@@ -74,6 +74,7 @@ class Arm : public frc2::SubsystemBase {
 
     double lift_power_;
     double extend_power_;
+    double calibration_power_;
 
     // logs
     wpi::log::DoubleLogEntry arm_lift_output_log_;

src/main/include/subsystems/SwerveDrive.h

@@ -112,6 +112,12 @@ class SwerveDrive : public frc2::SubsystemBase {
 
     double control_decay = 0.1;
 
+    double turn_deadband_;
+    double strafe_deadband_;
+    double drive_deadband_;
+    double invert_threshold_;
+    double center_deadband_;
+
     // auton
     bool probably_balanced_ = false;
     float last_pitch_ = 0.0;