Skip to content

Commit 2df1f03

Browse files
prawny-boyprawny-boy
committed
Tested on robot
1 parent 7e9b5f9 commit 2df1f03

File tree

4 files changed

+81
-18
lines changed

4 files changed

+81
-18
lines changed

‎src/main/cpp/Intake.cpp‎

Lines changed: 6 additions & 3 deletions
Original file line numberDiff line numberDiff line change
@@ -31,7 +31,7 @@ void Intake::OnUpdate(units::second_t dt) {
3131
setState(IntakeState::kIdle);
3232
}
3333
_stringStateName = "Eject";
34-
_setVoltage = -5_V;
34+
_setVoltage = 7_V;
3535
} break;
3636
case IntakeState::kHold: {
3737
// _config.IntakeMotor.motorController->SetVoltage(0_V);
@@ -41,15 +41,15 @@ void Intake::OnUpdate(units::second_t dt) {
4141
case IntakeState::kIntake: {
4242
// _config.IntakeMotor.motorController->SetVoltage(5_V);
4343
_stringStateName = "Intake";
44-
_setVoltage = 5_V;
44+
_setVoltage = -7_V;
4545
} break;
4646
case IntakeState::kPass: {
4747
// _config.IntakeMotor.motorController->SetVoltage(5_V);
4848
// if (_config.shooterSensor->Get()) {
4949
// setState(IntakeState::kIdle);
5050
// _stringStateName = "Pass";
5151
// }
52-
_setVoltage = 5_V;
52+
_setVoltage = -7_V;
5353
} break;
5454
default:
5555
std::cout << "Error: Intake in INVALID STATE." << std::endl;
@@ -72,3 +72,6 @@ void Intake::setState(IntakeState state) {
7272
void Intake::setRaw(units::volt_t voltage) {
7373
_rawVoltage = voltage;
7474
}
75+
IntakeState Intake::getState() {
76+
return _state;
77+
}

‎src/main/cpp/IntakeBehaviour.cpp‎

Lines changed: 70 additions & 14 deletions
Original file line numberDiff line numberDiff line change
@@ -12,44 +12,100 @@ IntakeManualControl::IntakeManualControl(Intake* intake, frc::XboxController& co
1212

1313
void IntakeManualControl::OnTick(units::second_t dt) {
1414
if (_codriver.GetBButtonPressed()) {
15-
if (_rawControl == true) {
15+
if (_rawControl) {
1616
_rawControl = false;
17+
_intake->setState(IntakeState::kIdle);
1718
} else {
1819
_rawControl = true;
20+
_intake->setState(IntakeState::kRaw);
1921
}
2022
}
21-
23+
2224
if (_rawControl) {
2325
if (_codriver.GetRightTriggerAxis() > 0.1) {
2426
_intake->setRaw(_codriver.GetRightTriggerAxis() * 10_V);
2527
} else if (_codriver.GetLeftTriggerAxis() > 0.1) {
2628
_intake->setRaw(_codriver.GetLeftTriggerAxis() * -10_V);
27-
} else if (_rawVoltage != 0_V) {
28-
if (_intake->GetConfig().intakeSensor->Get() == 1) {
29-
_intake->setRaw(0_V);
30-
} else {
31-
_intake->setRaw(10_V);
32-
}
3329
} else {
3430
_intake->setRaw(0_V);
3531
}
3632
_intake->setState(IntakeState::kRaw);
3733

3834
} else {
39-
if (_intake->GetConfig().intakeSensor->Get() == 1) {
40-
_intake->setState(IntakeState::kHold);
41-
} else if (_intake->GetConfig().intakeSensor->Get() == 0) {
42-
if (_codriver.GetRightTriggerAxis() > 0.1) {
43-
_intake->setState(IntakeState::kIntake);
44-
} else {
35+
if (_codriver.GetRightTriggerAxis() > 0.1) {
36+
if (_intaking) {
37+
_intaking = false;
38+
_intake->setState(IntakeState::kIdle);
39+
} else {
40+
_intaking = true;
41+
}
42+
}
43+
44+
if (_codriver.GetLeftTriggerAxis() > 0.1) {
45+
if (_ejecting) {
46+
_ejecting = false;
47+
} else {
48+
_ejecting = true;
49+
}
50+
}
51+
52+
if (_intaking) {
53+
if (_intake->getState() == IntakeState::kHold || _intake->getState() == IntakeState::kPass) {
54+
if (_intake->GetConfig().intakeSensor->Get() == 1) {
4555
_intake->setState(IntakeState::kIdle);
56+
_intaking = false;
57+
} else {
58+
_intake->setState(IntakeState::kPass);
59+
}
60+
} else {
61+
if (_intake->GetConfig().intakeSensor->Get() == 0) {
62+
_intake->setState(IntakeState::kHold);
63+
_intaking = false;
64+
} else {
65+
_intake->setState(IntakeState::kIntake);
4666
}
4767
}
4868
}
69+
70+
if (_ejecting) {
71+
if (_intake->GetConfig().intakeSensor->Get() == 1) {
72+
_intake->setState(IntakeState::kIdle);
73+
_ejecting = false;
74+
} else {
75+
_intake->setState(IntakeState::kEject);
76+
}
77+
}
4978
}
79+
}
5080

5181
IntakeAutoControl::IntakeAutoControl(Intake* intake) : _intake(intake) {
5282
Controls(intake);
5383
}
5484

5585
void IntakeAutoControl::OnTick(units::second_t dt) {}
86+
87+
// } else {
88+
// if (_setVoltage != 0_V) { // If the intake wasn't running last tick,
89+
// if (_setVoltage < 0_V && _intake->GetConfig().intakeSensor->Get() == 0) { // If the intake was intaking and there is a note in the intake,
90+
// _intake->setState(IntakeState::kHold); // Hold that note.
91+
// } else if (_setVoltage > 0_V && _intake->GetConfig().intakeSensor->Get() == 1) { // Else if the intake was ejecting and there is noting in the intake,
92+
// _intake->setState(IntakeState::kIdle); // Set the intake to idle mode.
93+
// }
94+
95+
// } else {
96+
// if (_intake->GetConfig().intakeSensor->Get() == 0) { // If there is a note in the intake.
97+
// if (_codriver.GetRightTriggerAxis() > 0.1) { // If the right trigger is pressed,
98+
// _intake->setState(IntakeState::kPass); // Start passing the note into the shooter.
99+
// } else { // Otherwise,
100+
// _intake->setState(IntakeState::kHold); // Keep holding the note.
101+
// }
102+
// } else if (_intake->GetConfig().intakeSensor->Get() == 1) { // If there is nothing in the intake,
103+
// if (_codriver.GetRightTriggerAxis() > 0.1) { // If the right trigger is pressed,
104+
// _intake->setState(IntakeState::kIntake); // Start intaking.
105+
// } else { // Otherwise,
106+
// _intake->setState(IntakeState::kIdle); // Continue to go Idle.
107+
// }
108+
// } else if (_codriver.GetLeftTriggerAxis() > 0.1) { // If the left trigger is pressed,
109+
// _intake->setState(IntakeState::kEject); // Eject the note.
110+
// }
111+
// }

‎src/main/include/Intake.h‎

Lines changed: 1 addition & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -28,6 +28,7 @@ class Intake : public behaviour::HasBehaviour {
2828

2929
void setState(IntakeState state);
3030
void setRaw(units::volt_t voltage);
31+
IntakeState getState();
3132
IntakeConfig GetConfig();
3233

3334
private:

‎src/main/include/IntakeBehaviour.h‎

Lines changed: 4 additions & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -20,7 +20,10 @@ class IntakeManualControl : public behaviour::Behaviour {
2020
frc::XboxController& _codriver;
2121

2222
units::volt_t _rawVoltage;
23-
bool _rawControl;
23+
units::volt_t _setVoltage;
24+
bool _rawControl = true;
25+
bool _intaking = false;
26+
bool _ejecting = false;
2427
};
2528

2629
class IntakeAutoControl : public behaviour::Behaviour {

0 commit comments

Comments
 (0)