Skip to content

Commit

Permalink
idk
Browse files Browse the repository at this point in the history
  • Loading branch information
@kill-shots
kill-shots committed Mar 4, 2024
1 parent 4a40db1 commit 4f1769d
Show file tree
Hide file tree
Showing 4 changed files with 152 additions and 38 deletions.
176 changes: 139 additions & 37 deletions src/main/cpp/Intake.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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;

Expand All @@ -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());
Expand Down
8 changes: 8 additions & 0 deletions src/main/cpp/IntakeBehaviour.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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);
Expand Down
4 changes: 4 additions & 0 deletions src/main/include/IntakeBehaviour.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -19,6 +19,10 @@ class IntakeManualControl : public behaviour::Behaviour {
Intake* _intake;
frc::XboxController& _codriver;
bool _rawControl = false;

std::string _stringStateName = "error";

std::shared_ptr<nt::NetworkTable> _table = nt::NetworkTableInstance::GetDefault().GetTable("Intake");
};

class AutoIntake : public behaviour::Behaviour {
Expand Down
2 changes: 1 addition & 1 deletion src/main/include/RobotMap.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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};

Expand Down

0 comments on commit 4f1769d

Please sign in to comment.