diff --git a/.github/workflows/build-check.yml b/.github/workflows/build-check.yml deleted file mode 100644 index a395fe0..0000000 --- a/.github/workflows/build-check.yml +++ /dev/null @@ -1,19 +0,0 @@ -name: Build Checks - -on: - push: - branches: [ "*" ] - pull_request: - branches: [ "master" ] - -jobs: - Ubuntu-Build: - runs-on: ubuntu-latest - - steps: - - uses: actions/checkout@v3 - - uses: spacey-sooty/Actions-For-GradleRIO@v0.0.27 - - name: Install - run: ./gradlew installRoboRioToolchain - - name: Build - run: ./gradlew build \ No newline at end of file diff --git a/.idea/codeStyles/Project.xml b/.idea/codeStyles/Project.xml new file mode 100644 index 0000000..f603881 --- /dev/null +++ b/.idea/codeStyles/Project.xml @@ -0,0 +1,7 @@ + + + + + + \ No newline at end of file diff --git a/.idea/codeStyles/codeStyleConfig.xml b/.idea/codeStyles/codeStyleConfig.xml new file mode 100644 index 0000000..79ee123 --- /dev/null +++ b/.idea/codeStyles/codeStyleConfig.xml @@ -0,0 +1,5 @@ + + + + \ No newline at end of file diff --git a/src/main/cpp/Gripper.cpp b/src/main/cpp/Gripper.cpp index 74834b5..a1c5013 100644 --- a/src/main/cpp/Gripper.cpp +++ b/src/main/cpp/Gripper.cpp @@ -10,6 +10,10 @@ void Gripper::OnUpdate(units::second_t dt) { voltage = 1_V; break; + case GripperState::kStop: + voltage = 0_V; + break; + case GripperState::kIntaking: voltage = 8_V; break; @@ -39,6 +43,10 @@ void Gripper::SetOutaking(double speed) { _state = GripperState::kOutaking; } +void Gripper::SetStop() { + _state = GripperState::kStop; +} + void Gripper::SetHolding() { _state = GripperState::kHolding; } @@ -57,5 +65,8 @@ std::string Gripper::GetState() { case GripperState::kOutaking: return "Outaking"; break; + case GripperState::kStop: + return "Stop"; + break; } } \ No newline at end of file diff --git a/src/main/cpp/Robot.cpp b/src/main/cpp/Robot.cpp index 47949cd..c00f1e5 100644 --- a/src/main/cpp/Robot.cpp +++ b/src/main/cpp/Robot.cpp @@ -29,15 +29,20 @@ void Robot::RobotInit() { lastPeriodic = wom::now(); //sets the offsets of the swerve cancoders to retune these values set the offsets to 0 and record the values - map.swerveBase.moduleConfigs[0].turnMotor.encoder->SetEncoderOffset(4.11207_rad); - map.swerveBase.moduleConfigs[1].turnMotor.encoder->SetEncoderOffset(1.32638_rad); - map.swerveBase.moduleConfigs[2].turnMotor.encoder->SetEncoderOffset(1.67817_rad); - map.swerveBase.moduleConfigs[3].turnMotor.encoder->SetEncoderOffset(0.60899_rad); - - // map.swerveBase.moduleConfigs[0].turnMotor.encoder->SetEncoderOffset(0_rad); - // map.swerveBase.moduleConfigs[1].turnMotor.encoder->SetEncoderOffset(0_rad); - // map.swerveBase.moduleConfigs[2].turnMotor.encoder->SetEncoderOffset(0_rad); - // map.swerveBase.moduleConfigs[3].turnMotor.encoder->SetEncoderOffset(0_rad); + map.swerveBase.moduleConfigs[0].turnMotor.encoder->SetEncoderOffset(2.54181_rad); + map.swerveBase.moduleConfigs[1].turnMotor.encoder->SetEncoderOffset(6.03621_rad); + map.swerveBase.moduleConfigs[2].turnMotor.encoder->SetEncoderOffset(0.09050_rad); + map.swerveBase.moduleConfigs[3].turnMotor.encoder->SetEncoderOffset(5.30451_rad); + + // back left: 0.090504866485253 + // back right: 5.304505564508547 + // front left: 2.541806165526507 + // front right: 6.036214400329998 + +// map.swerveBase.moduleConfigs[0].turnMotor.encoder->SetEncoderOffset(0_rad); +// map.swerveBase.moduleConfigs[1].turnMotor.encoder->SetEncoderOffset(0_rad); +// map.swerveBase.moduleConfigs[2].turnMotor.encoder->SetEncoderOffset(0_rad); +// map.swerveBase.moduleConfigs[3].turnMotor.encoder->SetEncoderOffset(0_rad); //the start of selecting which auto mode, was never successfully implemented m_chooser.SetDefaultOption(kLowPlace, kLowPlace); diff --git a/src/main/cpp/behaviour/ArmavatorBehaviour.cpp b/src/main/cpp/behaviour/ArmavatorBehaviour.cpp index 37464d2..2674ee5 100644 --- a/src/main/cpp/behaviour/ArmavatorBehaviour.cpp +++ b/src/main/cpp/behaviour/ArmavatorBehaviour.cpp @@ -176,35 +176,7 @@ void ArmavatorManualBehaviour::OnTick(units::second_t dt) { frc::SmartDashboard::PutNumber("ArmVelocitySetpoint", av.angleSpeed.value()); - if (_codriver.GetPOV() == 0) { - //picking up cone down - - SetPosition(0_deg, 0.7_m, "1", 0.5, 0.2); - } else if (_codriver.GetPOV() == 90) { - //picking up cone up - - SetPosition(37.4_deg, 0.01_m, "2", 0.5, 0.2); - } else if (_codriver.GetPOV() == 180) { - //picking up cone down to collect - - SetPosition(-6_deg, 0.7_m, "3", 0.3, 0.1); - } else if (_codriver.GetPOV() == 270) { - //low hold - - SetPosition(60_deg, 0.1_m, "4", 0.35, 0.07); - } else if (_codriver.GetXButton()) { - //front mid place - - SetPosition(30_deg, 0.15_m, "5", 0.35, 0.07); - } else if (_codriver.GetYButton()) { - // 152_deg 0.1814_m back high place - - SetPosition(152_deg, 0.1814_m, "6", 0.35, 0.07); - } else if (_codriver.GetBButton()) { - // high hold - - SetPosition(161_deg, 0.0_m, "7", 0.35, 0.07); - } + CheckSetpoints(); std::cout << "Mode Velocity" << std::endl; } @@ -309,38 +281,7 @@ void ArmavatorManualBehaviour::OnTick(units::second_t dt) { std::cout << "set position height: " << _manualSetpoint.height.value() << std::endl; std::cout << "set position angle: " << _manualSetpoint.angle.value() << std::endl; - if (_codriver.GetPOV() == 0) { - //picking up cone down - - SetPosition(0_deg, 0.7_m, "1", 0.5, 0.2); - } else if (_codriver.GetPOV() == 90) { - //picking up cone up - - SetPosition(37.4_deg, 0.01_m, "2", 0.5, 0.2); - } else if (_codriver.GetPOV() == 180) { - //picking up cone down to collect - - SetPosition(-6_deg, 0.7_m, "3", 0.3, 0.1); - } else if (_codriver.GetPOV() == 270) { - //low hold - - SetPosition(60_deg, 0.1_m, "4", 0.35, 0.07); - } else if (_codriver.GetXButton()) { - //front mid place - - SetPosition(30_deg, 0.15_m, "5", 0.35, 0.07); - } else if (_codriver.GetYButton()) { - // 152_deg 0.1814_m back high place - - SetPosition(152_deg, 0.1814_m, "6", 0.35, 0.07); - } else if (_codriver.GetBButton()) { - // high hold - - SetPosition(161_deg, 0.0_m, "7", 0.35, 0.07); - - std::cout << "Mode Position" << std::endl; - - } + CheckSetpoints(); break; } @@ -484,11 +425,58 @@ void ArmavatorManualBehaviour::OnTick(units::second_t dt) { // } } +void ArmavatorManualBehaviour::CheckSetpoints() { + + if (_codriver.GetPOV() == 0) { + //picking up cone down + + SetPosition(0_deg, 0.7_m, "1", 0.5, 0.2); + } else if (_codriver.GetPOV() == 90) { + //picking up cone up + + SetPosition(37.4_deg, 0.01_m, "2", 0.5, 0.2); + } else if (_codriver.GetPOV() == 180) { + //picking up cone down to collect + + SetPosition(-6_deg, 0.7_m, "3", 0.3, 0.1); + } else if (_codriver.GetPOV() == 270) { + //low hold + + SetPosition(60_deg, 0.1_m, "4", 0.35, 0.07); + } else if (_codriver.GetXButton()) { + //front mid place + + SetPosition(30_deg, 0.15_m, "5", 0.35, 0.07); + } else if (_codriver.GetYButton()) { + // 152_deg 0.1814_m back high place + + SetPosition(152_deg, 0.1814_m, "6", 0.35, 0.07); + } else if (_codriver.GetBButton()) { + // high hold + + SetPosition(161_deg, 0.0_m, "7", 0.35, 0.07); + + std::cout << "Mode Position" << std::endl; + } + + return; +} + void ArmavatorManualBehaviour::SetPosition(units::degree_t angle, units::meter_t height, std::string name, double elevatorSpeed, double armSpeed) { - _setpointValue.height = height; - _setpointValue.angle = angle; - _armavator->SetPosition(_setpointValue); - _manualSetpoint = {_armavator->GetCurrentPosition().height, _armavator->GetCurrentPosition().angle}; - std::cout << "GO TO armavator POS " << name << std::endl; - _armavator->SetSpeedValues(elevatorSpeed, armSpeed); + if (_armManualModes == ArmavatorManualModesEnum::kPosition) { + _manualSetpoint.height = height; + _manualSetpoint.angle = angle; + // _armavator->SetPosition(_manualSetpoint); + // _manualSetpoint = {_armavator->GetCurrentPosition().height, _armavator->GetCurrentPosition().angle}; + std::cout << "GO TO armavator POS " << name << std::endl; + _armavator->SetSpeedValues(elevatorSpeed, armSpeed); + } + else if (_armManualModes == ArmavatorManualModesEnum::kVelocity) { + ArmavatorVelocity av; + av.angleSpeed = angle * (180_deg / 1_s); + av.elevatorSpeed = height * (2_m / 1_s); + _armavator->SetVelocity(av); + frc::SmartDashboard::PutNumber("ArmVelocitySetpoint", av.angleSpeed.value()); + } } + diff --git a/src/main/cpp/behaviour/GripperBehaviour.cpp b/src/main/cpp/behaviour/GripperBehaviour.cpp index 77af29f..38f7408 100644 --- a/src/main/cpp/behaviour/GripperBehaviour.cpp +++ b/src/main/cpp/behaviour/GripperBehaviour.cpp @@ -25,10 +25,20 @@ GripperBehaviour::GripperBehaviour(Gripper *gripper, frc::XboxController &codriv void GripperBehaviour::OnTick(units::second_t dt) { //setting the gripper mode if (_codriver.GetLeftTriggerAxis() > 0.2) { - gripper->SetOutaking(_codriver.GetLeftTriggerAxis()); + double outtakeAmount = _codriver.GetLeftTriggerAxis() * _codriver.GetLeftTriggerAxis() * _codriver.GetLeftTriggerAxis(); + gripper->SetOutaking(outtakeAmount); } else if (_codriver.GetRightTriggerAxis() > 0.2) { gripper->SetIntaking(); } else { - gripper->SetIdle(); + if (idleHold) { + gripper->SetIdle(); + } + else { + gripper->SetStop(); + } + } + + if (_codriver.GetBackButtonPressed()) { + idleHold = !idleHold; } } \ No newline at end of file diff --git a/src/main/cpp/behaviour/SwervebaseBehaviour.cpp b/src/main/cpp/behaviour/SwervebaseBehaviour.cpp index 4f74f9c..c7639be 100644 --- a/src/main/cpp/behaviour/SwervebaseBehaviour.cpp +++ b/src/main/cpp/behaviour/SwervebaseBehaviour.cpp @@ -74,10 +74,16 @@ void ManualDrivebase::OnTick(units::second_t deltaTime) { CalculateRequestedAngle(turnX, turnY, currentAngle); _swerveDriveTable->GetEntry("RotateMatch").SetDouble(_requestedAngle.value()); - _swerveDrivebase->RotateMatchJoystick(_requestedAngle, wom::FieldRelativeSpeeds{ - xVelocity * maxMovementMagnitude, - yVelocity * maxMovementMagnitude, - r_x * maxRotationMagnitude +// _swerveDrivebase->RotateMatchJoystick(_requestedAngle, wom::FieldRelativeSpeeds{ +// xVelocity * maxMovementMagnitude, +// yVelocity * maxMovementMagnitude, +// r_x * maxRotationMagnitude +// }); + + _swerveDrivebase->SetFieldRelativeVelocity(wom::FieldRelativeSpeeds{ + xVelocity * maxMovementMagnitude, + yVelocity * maxMovementMagnitude, + r_x * maxRotationMagnitude }); } diff --git a/src/main/include/Gripper.h b/src/main/include/Gripper.h index ae58539..4c68f76 100644 --- a/src/main/include/Gripper.h +++ b/src/main/include/Gripper.h @@ -17,7 +17,8 @@ enum class GripperState { kIdle, kIntaking, kOutaking, - kHolding + kHolding, + kStop }; class Gripper : public behaviour::HasBehaviour { @@ -30,6 +31,7 @@ class Gripper : public behaviour::HasBehaviour { void SetIntaking(); void SetOutaking(double speed); void SetHolding(); + void SetStop(); std::string GetState(); diff --git a/src/main/include/behaviour/ArmavatorBehaviour.h b/src/main/include/behaviour/ArmavatorBehaviour.h index f4d8500..c5b8cbd 100644 --- a/src/main/include/behaviour/ArmavatorBehaviour.h +++ b/src/main/include/behaviour/ArmavatorBehaviour.h @@ -107,4 +107,5 @@ class ArmavatorManualBehaviour : public behaviour::Behaviour { ArmavatorManualModeEnum _armManualModes = ArmavatorManualModeEnum::kRaw; void SetPosition(units::degree_t angle, units::meter_t height, std::string name, double elevatorSpeed, double armSpeed); + void CheckSetpoints(); }; \ No newline at end of file diff --git a/src/main/include/behaviour/GripperBehaviour.h b/src/main/include/behaviour/GripperBehaviour.h index 690487a..866aa66 100644 --- a/src/main/include/behaviour/GripperBehaviour.h +++ b/src/main/include/behaviour/GripperBehaviour.h @@ -25,4 +25,6 @@ class GripperBehaviour : public behaviour::Behaviour { frc::XboxController &_codriver; bool holdingObject = false; + + bool idleHold = false; }; \ No newline at end of file