diff --git a/src/main/cpp/Intake.cpp b/src/main/cpp/Intake.cpp index c8450b40..ba879268 100644 --- a/src/main/cpp/Intake.cpp +++ b/src/main/cpp/Intake.cpp @@ -16,29 +16,127 @@ void Intake::OnStart() { void Intake::OnUpdate(units::second_t dt) { + // switch (_state) { + + // case IntakeState::kAdjust: + // { + // // if (_config.intakeSensor->Get() == false && _config.passSensor->Get() == false) { + // // _setVoltage = 4_V; + // // } else + // if (_config.intakeSensor->Get() == false && _config.passSensor->Get() == true) { + // SetState(IntakeState::kHold); + // } else if (_config.intakeSensor->Get() == true && _config.passSensor->Get() == true) { + // SetState(IntakeState::kIdle); + // } + // } + // case IntakeState::kIdle: + // { + // if (_config.intakeSensor->Get() == false && _config.passSensor->Get() == true) { + // SetState(IntakeState::kHold); + // // } else if (_config.intakeSensor->Get() == false && _config.passSensor->Get() == false) { + // // SetState(IntakeState::kAdjust); + // } + // _stringStateName = "Idle"; + // _pid.Reset(); + // _setVoltage = 0_V; + // _recordNote = false; + // } + // break; + + // case IntakeState::kRaw: + // { + // _stringStateName = "Raw"; + // _pid.Reset(); + // _setVoltage = _rawVoltage; + // } + // break; + + // case IntakeState::kEject: + // { + // if (_config.intakeSensor->Get() == true && _config.passSensor->Get() == true) { + // SetState(IntakeState::kIdle); + // // } else if (_config.intakeSensor->Get() == false && _config.passSensor->Get() == false) { + // // SetState(IntakeState::kAdjust); + // } + // _stringStateName = "Eject"; + // _setVoltage = 4_V; + // _pid.Reset(); + // } + // break; + + // case IntakeState::kHold: + // { + // if (_config.intakeSensor->Get() == true && _config.passSensor->Get() == true) { + // SetState(IntakeState::kIdle); + // } + // // } else if (_config.intakeSensor->Get() == false && _config.passSensor->Get() == false) { + // // SetState(IntakeState::kAdjust); + // // } + // _pid.SetSetpoint(0); + // units::volt_t pidCalculate = + // // units::volt_t{_pid.Calculate(_config.IntakeGearbox.encoder->GetEncoderAngularVelocity().value())}; + // // units::volt_t pidCalculate = + // units::volt_t{_pidPosition.Calculate(_config.IntakeGearbox.encoder->GetEncoderPosition().value())}; + // _setVoltage = pidCalculate; + // _stringStateName = "Hold"; + // } + // break; + + // case IntakeState::kIntake: + // { + // if (_config.intakeSensor->Get() == false && _config.passSensor->Get() == true) { + // SetState(IntakeState::kHold); + // // } else if (_config.intakeSensor->Get() == false && _config.passSensor->Get() == false) { + // // SetState(IntakeState::kAdjust); + // } + // _stringStateName = "Intake"; + // _setVoltage = -4_V; + // } + // break; + + // case IntakeState::kPass: + // { + // if (_config.intakeSensor->Get() == true && _config.passSensor->Get() == true) { + // SetState(IntakeState::kIdle); + // } + + // if (!_recordNote) { + // _noteShot ++; + // _recordNote = true; + // } + + // _stringStateName = "Pass"; + // _setVoltage = -4_V; + // } + // break; + // } + switch (_state) { case IntakeState::kAdjust: { - if (_config.intakeSensor->Get() == true && _config.passSensor->Get() == true) { - _setVoltage = -4_V; - } else if (_config.intakeSensor->Get() == true && _config.passSensor->Get() == false) { + // // if (_config.intakeSensor->Get() == false && _config.passSensor->Get() == false) { + // // _setVoltage = 4_V; + // // } else + if (_config.intakeSensor->Get() == false && _config.passSensor->Get() == true) { SetState(IntakeState::kHold); - } else if (_config.intakeSensor->Get() == false && _config.passSensor->Get() == false) { + } else if (_config.intakeSensor->Get() == true && _config.passSensor->Get() == true) { SetState(IntakeState::kIdle); } } + break; + case IntakeState::kIdle: { - if (_config.intakeSensor->Get() == true && _config.passSensor->Get() == false) { + if (_config.intakeSensor->Get() == false && _config.passSensor->Get() == true) { SetState(IntakeState::kHold); - } else if (_config.intakeSensor->Get() == true && _config.passSensor->Get() == true) { + } else if (_config.intakeSensor->Get() == false && _config.passSensor->Get() == false) { SetState(IntakeState::kAdjust); } - _stringStateName = "Idle"; - _pid.Reset(); - _setVoltage = 0_V; - _recordNote = false; + // _stringStateName = "Idle"; + // _pid.Reset(); + // _setVoltage = 0_V; + // _recordNote = false; } break; @@ -52,62 +150,66 @@ void Intake::OnUpdate(units::second_t dt) { case IntakeState::kEject: { - if (_config.intakeSensor->Get() == false && _config.passSensor->Get() == false) { + if (_config.intakeSensor->Get() == true && _config.passSensor->Get() == true) { SetState(IntakeState::kIdle); - } else if (_config.intakeSensor->Get() == true && _config.passSensor->Get() == true) { + } else if (_config.intakeSensor->Get() == false && _config.passSensor->Get() == false) { SetState(IntakeState::kAdjust); } - _stringStateName = "Eject"; - _setVoltage = -4_V; - _pid.Reset(); + // _stringStateName = "Eject"; + // _setVoltage = 4_V; + // _pid.Reset(); } break; case IntakeState::kHold: { - if (_config.intakeSensor->Get() == false && _config.passSensor->Get() == false) { - SetState(IntakeState::kIdle); - } else if (_config.intakeSensor->Get() == true && _config.passSensor->Get() == true) { + if (_config.intakeSensor->Get() == true && _config.passSensor->Get() == true) { + _state = IntakeState::kIdle; + } else if (_config.intakeSensor->Get() == false && _config.passSensor->Get() == false) { SetState(IntakeState::kAdjust); } - _pid.SetSetpoint(0); - units::volt_t pidCalculate = - // units::volt_t{_pid.Calculate(_config.IntakeGearbox.encoder->GetEncoderAngularVelocity().value())}; + // // } else if (_config.intakeSensor->Get() == false && _config.passSensor->Get() == false) { + // // SetState(IntakeState::kAdjust); + // // } + // _pid.SetSetpoint(0); // units::volt_t pidCalculate = - units::volt_t{_pidPosition.Calculate(_config.IntakeGearbox.encoder->GetEncoderPosition().value())}; - _setVoltage = pidCalculate; - _stringStateName = "Hold"; + // // units::volt_t{_pid.Calculate(_config.IntakeGearbox.encoder->GetEncoderAngularVelocity().value())}; + // // units::volt_t pidCalculate = + // units::volt_t{_pidPosition.Calculate(_config.IntakeGearbox.encoder->GetEncoderPosition().value())}; + // _setVoltage = pidCalculate; + // _stringStateName = "Hold"; } break; case IntakeState::kIntake: { - if (_config.intakeSensor->Get() == true && _config.passSensor->Get() == false) { - SetState(IntakeState::kHold); - } else if (_config.intakeSensor->Get() == true && _config.passSensor->Get() == true) { - SetState(IntakeState::kAdjust); + if (_config.intakeSensor->Get() == false && _config.passSensor->Get() == true) { + _state = IntakeState::kHold; + } else if (_config.intakeSensor->Get() == false && _config.passSensor->Get() == false) { + _state = IntakeState::kAdjust; } - _stringStateName = "Intake"; - _setVoltage = 4_V; + // _stringStateName = "Intake"; + // _setVoltage = -4_V; } break; case IntakeState::kPass: { - if (_config.intakeSensor->Get() == false && _config.passSensor->Get() == false) { + if (_config.intakeSensor->Get() == true && _config.passSensor->Get() == true) { SetState(IntakeState::kIdle); } - if (!_recordNote) { - _noteShot ++; - _recordNote = true; - } + // if (!_recordNote) { + // _noteShot ++; + // _recordNote = true; + // } - _stringStateName = "Pass"; - _setVoltage = 4_V; + // _stringStateName = "Pass"; + // _setVoltage = -4_V; } break; } + _table->GetEntry("State: ").SetString(_stringStateName); _table->GetEntry("Motor Voltage: ").SetDouble(_setVoltage.value()); _table->GetEntry("Intake Sensor: ").SetBoolean(_config.intakeSensor->Get()); diff --git a/src/main/cpp/IntakeBehaviour.cpp b/src/main/cpp/IntakeBehaviour.cpp index aa17077d..435b3ee4 100644 --- a/src/main/cpp/IntakeBehaviour.cpp +++ b/src/main/cpp/IntakeBehaviour.cpp @@ -12,7 +12,15 @@ IntakeManualControl::IntakeManualControl(Intake* intake, frc::XboxController& co void IntakeManualControl::OnTick(units::second_t dt) { + if (_codriver.GetStartButtonReleased()) { + std::cout <<"Start2 button is: "<< std::endl; + } + if (_codriver.GetBackButtonReleased()) { + std::cout <<"Back button is: "<< std::endl; + } + + if (_codriver.GetStartButtonReleased()) { if (_rawControl) { _rawControl = false; _intake->SetState(IntakeState::kIdle); diff --git a/src/main/include/IntakeBehaviour.h b/src/main/include/IntakeBehaviour.h index 642d20b2..b8101460 100644 --- a/src/main/include/IntakeBehaviour.h +++ b/src/main/include/IntakeBehaviour.h @@ -19,6 +19,10 @@ class IntakeManualControl : public behaviour::Behaviour { Intake* _intake; frc::XboxController& _codriver; bool _rawControl = false; + + std::string _stringStateName = "error"; + + std::shared_ptr _table = nt::NetworkTableInstance::GetDefault().GetTable("Intake"); }; class AutoIntake : public behaviour::Behaviour { diff --git a/src/main/include/RobotMap.h b/src/main/include/RobotMap.h index d4953b92..2a55e3d7 100644 --- a/src/main/include/RobotMap.h +++ b/src/main/include/RobotMap.h @@ -50,7 +50,7 @@ struct RobotMap { rev::CANSparkMax intakeMotor{35, rev::CANSparkMax::MotorType::kBrushless}; wom::CANSparkMaxEncoder intakeEncoder{&intakeMotor, 0.1_m}; frc::DigitalInput intakeSensor{5}; - frc::DigitalInput passSensor{99}; + frc::DigitalInput passSensor{8}; // frc::DigitalInput magSensor{0}; // frc::DigitalInput shooterSensor{0};