diff --git a/.gitattributes b/.gitattributes index 416557ac..df788002 100644 --- a/.gitattributes +++ b/.gitattributes @@ -1,5 +1,6 @@ * text=auto *.sh text eol=lf +*.bat text eol=crlf *.gradle text eol=lf *.java text eol=lf *.json text eol=lf diff --git a/src/main/cpp/Intake.cpp b/src/main/cpp/Intake.cpp index 75839209..f63be144 100644 --- a/src/main/cpp/Intake.cpp +++ b/src/main/cpp/Intake.cpp @@ -13,43 +13,51 @@ IntakeConfig Intake::GetConfig() { void Intake::OnUpdate(units::second_t dt) { switch (_state) { case IntakeState::kIdle: { - // _config.IntakeMotor.motorController->SetVoltage(0_V); - // if (_config.intakeSensor->Get()) { - // setState(IntakeState::kHold); - // } + if (_config.intakeSensor->Get() == false) { + setState(IntakeState::kHold); + } _stringStateName = "Idle"; _setVoltage = 0_V; } break; + case IntakeState::kRaw: { - // _config.IntakeMotor.motorController->SetVoltage(_rawVoltage); _stringStateName = "Raw"; _setVoltage = _rawVoltage; } break; + case IntakeState::kEject: { - // _config.IntakeMotor.motorController->SetVoltage(-5_V); - // if (_config.intakeSensor->Get() == 0 && _config.magSensor->Get() == 0) { - // setState(IntakeState::kIdle); - // } _stringStateName = "Eject"; - _setVoltage = -5_V; + _setVoltage = 7_V; + if (_config.intakeSensor->Get() == true) { + setState(IntakeState::kIdle); + _ejecting = false; + } } break; + case IntakeState::kHold: { - // _config.IntakeMotor.motorController->SetVoltage(0_V); _stringStateName = "Hold"; _setVoltage = 0_V; + // if (_config.intakeSensor->Get() == false) { + // setState(IntakeState::kHold); + // } } break; + case IntakeState::kIntake: { - // _config.IntakeMotor.motorController->SetVoltage(5_V); _stringStateName = "Intake"; - _setVoltage = 5_V; + _setVoltage = -7_V; + if (_config.intakeSensor->Get() == false) { + setState(IntakeState::kHold); + _intaking = false; + } } break; + case IntakeState::kPass: { - // _config.IntakeMotor.motorController->SetVoltage(5_V); - // if (_config.shooterSensor->Get()) { - // setState(IntakeState::kIdle); - // _stringStateName = "Pass"; - // } - _setVoltage = 5_V; + _stringStateName = "Pass"; + _setVoltage = -7_V; + if (_config.intakeSensor->Get() == true) { + setState(IntakeState::kIdle); + _passing = false; + } } break; default: std::cout << "Error: Intake in INVALID STATE." << std::endl; @@ -57,6 +65,8 @@ void Intake::OnUpdate(units::second_t dt) { } _table->GetEntry("State: ").SetString(_stringStateName); _table->GetEntry("Motor Voltage: ").SetDouble(_setVoltage.value()); + _table->GetEntry("Intake Sensor: ").SetBoolean(_config.intakeSensor->Get()); + // _table->GetEntry("Shooter Sensor: ").SetBoolean(_config.shooterSensor->Get()); // _table->GetEntry("Intake Sensor: ").SetBoolean(_config.intakeSensor->Get()); // _table->GetEntry("Shooter Sensor: ").SetBoolean(_config.shooterSensor->Get()); // _table->GetEntry("Magazine Sensor: ").SetBoolean(_config.magSensor->Get()); @@ -72,3 +82,6 @@ void Intake::setState(IntakeState state) { void Intake::setRaw(units::volt_t voltage) { _rawVoltage = voltage; } +IntakeState Intake::getState() { + return _state; +} diff --git a/src/main/cpp/IntakeBehaviour.cpp b/src/main/cpp/IntakeBehaviour.cpp index 90867045..59558bba 100644 --- a/src/main/cpp/IntakeBehaviour.cpp +++ b/src/main/cpp/IntakeBehaviour.cpp @@ -12,44 +12,83 @@ IntakeManualControl::IntakeManualControl(Intake* intake, frc::XboxController& co } void IntakeManualControl::OnTick(units::second_t dt) { - if (_codriver.GetBButtonPressed()) { - if (_rawControl == true) { + if (_codriver.GetBButtonReleased()) { + if (_rawControl) { _rawControl = false; + _intaking = false; + _ejecting = false; + _intake->setState(IntakeState::kIdle); } else { _rawControl = true; + _intaking = false; + _ejecting = false; + _intake->setState(IntakeState::kRaw); } } if (_rawControl) { - _intake->setState(IntakeState::kRaw); - if (_codriver.GetLeftBumper()) { - _intake->setRaw(10_V); - } else if (_codriver.GetRightBumper()) { - _intake->setRaw(-10_V); + if (_codriver.GetRightTriggerAxis() > 0.1) { + _intake->setRaw(_codriver.GetRightTriggerAxis() * 10_V); + } else if (_codriver.GetLeftTriggerAxis() > 0.1) { + _intake->setRaw(_codriver.GetLeftTriggerAxis() * -10_V); } else { _intake->setRaw(0_V); } + _intake->setState(IntakeState::kRaw); + + } else { + if (_codriver.GetRightTriggerAxis() > 0.1) { + if (_intaking) { + _intaking = false; + _intake->setState(IntakeState::kIdle); + } else { + _intaking = true; + _ejecting = false; + } + } + + if (_codriver.GetLeftTriggerAxis() > 0.1) { + if (_ejecting) { + _ejecting = false; + _intake->setState(IntakeState::kIdle); + } else { + _ejecting = true; + _intaking = false; + } + } + + if (_codriver.GetAButtonPressed()) { + if (_passing) { + _passing = false; + _intake->setState(IntakeState::kIdle); + } else { + _passing = true; + _intaking = false; + } + } - // _intake->setRaw(_codriver.GetLeftBumper() * 10_V); - // _intake->setRaw(_codriver.GetRightBumper() * -10_V); - std::cout << "Raw" << std::endl; + if (_intaking) { + if (_intake->getState() == IntakeState::kIdle) { + _intake->setState(IntakeState::kIntake); + } + } + + if (_passing) { + if (_intake->getState() == IntakeState::kHold) { + _intake->setState(IntakeState::kPass); + } + } + + if (_ejecting) { + if (_intake->getState() == IntakeState::kIdle || _intake->getState() == IntakeState::kHold) { + _intake->setState(IntakeState::kEject); + } + } } - // } else { - // if (_codriver.GetYButtonPressed()) { - // _intake->setState(IntakeState::kIntake); - // } - // if (_codriver.GetAButtonPressed()) { - // _intake->setState(IntakeState::kPass); - // } - // } } -// IntakeAutoControl::IntakeAutoControl(Intake* intake) : _intake(intake) {} +IntakeAutoControl::IntakeAutoControl(Intake* intake) : _intake(intake) { + Controls(intake); +} -// void IntakeAutoControl::OnTick(units::second_t dt) { -// if (_intake->GetConfig().intakeSensor->Get() == 1) { -// _intake->setState(IntakeState::kPass); -// } else if (_intake->GetConfig().magSensor->Get() == 0) { -// _intake->setState(IntakeState::kIdle); -// } -// } +void IntakeAutoControl::OnTick(units::second_t dt) {} diff --git a/src/main/cpp/Robot.cpp b/src/main/cpp/Robot.cpp index 2278fa1a..473873ba 100644 --- a/src/main/cpp/Robot.cpp +++ b/src/main/cpp/Robot.cpp @@ -46,10 +46,6 @@ void Robot::RobotInit() { // robotmap.swerveBase.gyro->Reset(); - // _swerveDrive = new wom::SwerveDrive(robotmap.swerveBase.config, frc::Pose2d()); - // wom::BehaviourScheduler::GetInstance()->Register(_swerveDrive); - // _swerveDrive->SetDefaultBehaviour( - // [this]() { return wom::make(_swerveDrive, &robotmap.controllers.driver); }); _swerveDrive = new wom::SwerveDrive(robotmap.swerveBase.config, frc::Pose2d()); wom::BehaviourScheduler::GetInstance()->Register(_swerveDrive); _swerveDrive->SetDefaultBehaviour( @@ -93,9 +89,7 @@ void Robot::RobotPeriodic() { shooter->OnUpdate(dt); sched->Tick(); - robotmap.swerveTable.swerveDriveTable - ->GetEntry("frontLeftEncoder") - + robotmap.swerveTable.swerveDriveTable->GetEntry("frontLeftEncoder") .SetDouble(robotmap.swerveBase.moduleConfigs[0].turnMotor.encoder->GetEncoderPosition().value()); robotmap.swerveTable.swerveDriveTable->GetEntry("frontRightEncoder") .SetDouble(robotmap.swerveBase.moduleConfigs[1].turnMotor.encoder->GetEncoderPosition().value()); @@ -106,6 +100,7 @@ void Robot::RobotPeriodic() { _swerveDrive->OnUpdate(dt); alphaArm->OnUpdate(dt); + shooter->OnStart(); intake->OnUpdate(dt); } @@ -118,7 +113,6 @@ void Robot::AutonomousPeriodic() {} void Robot::TeleopInit() { loop.Clear(); wom::BehaviourScheduler* sched = wom::BehaviourScheduler::GetInstance(); - shooter->OnStart(); sched->InterruptAll(); // frontLeft->SetVoltage(4_V); diff --git a/src/main/include/Intake.h b/src/main/include/Intake.h index a84b41b1..8d9c80b5 100644 --- a/src/main/include/Intake.h +++ b/src/main/include/Intake.h @@ -28,6 +28,7 @@ class Intake : public behaviour::HasBehaviour { void setState(IntakeState state); void setRaw(units::volt_t voltage); + IntakeState getState(); IntakeConfig GetConfig(); private: @@ -37,5 +38,9 @@ class Intake : public behaviour::HasBehaviour { units::volt_t _rawVoltage = 0_V; std::string _stringStateName = "error"; units::volt_t _setVoltage = 0_V; + bool _intaking; + bool _ejecting; + bool _passing; + std::shared_ptr _table = nt::NetworkTableInstance::GetDefault().GetTable("Intake"); }; diff --git a/src/main/include/IntakeBehaviour.h b/src/main/include/IntakeBehaviour.h index c84c6d32..931273fa 100644 --- a/src/main/include/IntakeBehaviour.h +++ b/src/main/include/IntakeBehaviour.h @@ -20,7 +20,11 @@ class IntakeManualControl : public behaviour::Behaviour { frc::XboxController& _codriver; units::volt_t _rawVoltage; - bool _rawControl; + units::volt_t _setVoltage; + bool _rawControl = true; + bool _intaking = false; + bool _ejecting = false; + bool _passing = false; }; class IntakeAutoControl : public behaviour::Behaviour { diff --git a/src/main/include/RobotMap.h b/src/main/include/RobotMap.h index 60c3ed19..0cadce34 100644 --- a/src/main/include/RobotMap.h +++ b/src/main/include/RobotMap.h @@ -27,10 +27,22 @@ struct RobotMap { struct Controllers { frc::XboxController driver = frc::XboxController(0); frc::XboxController codriver = frc::XboxController(1); - frc::XboxController testController = frc::XboxController(2); }; Controllers controllers; + struct IntakeSystem { + rev::CANSparkMax intakeMotor{2, rev::CANSparkMax::MotorType::kBrushed}; + // wom::CANSparkMaxEncoder intakeEncoder{&intakeMotor, 0.1_m}; + frc::DigitalInput intakeSensor{4}; + // frc::DigitalInput magSensor{0}; + // frc::DigitalInput shooterSensor{0}; + + wom::Gearbox IntakeGearbox{&intakeMotor, nullptr, frc::DCMotor::CIM(1)}; + + IntakeConfig config{IntakeGearbox, &intakeSensor /*, &magSensor, &shooterSensor*/}; + }; + IntakeSystem intakeSystem; + struct Shooter { rev::CANSparkMax shooterMotor{11, rev::CANSparkMax::MotorType::kBrushless}; // Port 11 // frc::DigitalInput shooterSensor{2}; @@ -52,19 +64,6 @@ struct RobotMap { }; Shooter shooterSystem; - struct IntakeSystem { - rev::CANSparkMax intakeMotor{2, rev::CANSparkMax::MotorType::kBrushed}; - // wom::CANSparkMaxEncoder intakeEncoder{&intakeMotor, 0.1_m}; - // frc::DigitalInput intakeSensor{0}; - // frc::DigitalInput magSensor{0}; - // frc::DigitalInput shooterSensor{0}; - - wom::Gearbox IntakeGearbox{&intakeMotor, nullptr, frc::DCMotor::CIM(1)}; - - IntakeConfig config{IntakeGearbox /*, &intakeSensor, &magSensor, &shooterSensor*/}; - }; - IntakeSystem intakeSystem; - struct SwerveBase { ctre::phoenix6::hardware::CANcoder frontLeftCancoder{18, "Drivebase"}; ctre::phoenix6::hardware::CANcoder frontRightCancoder{19, "Drivebase"};