Skip to content

Commit 985b881

Browse files
prawny-boyprawny-boy
committed
Merged intake into auto code
2 parents 0c6bfbe + 699b531 commit 985b881

File tree

7 files changed

+242
-23
lines changed

7 files changed

+242
-23
lines changed

‎src/main/cpp/Behaviours/IntakeBehaviour.cpp

Lines changed: 43 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,43 @@
1+
// Copyright (c) 2023-2024 CurtinFRC
2+
// Open Source Software, you can modify it according to the terms
3+
// of the MIT License at the root of this project
4+
5+
#include "Behaviours/IntakeBehaviour.h"
6+
7+
IntakeManualControl::IntakeManualControl(Intake* intake, frc::XboxController* codriver)
8+
: _intake(intake), _codriver(codriver) {
9+
Controls(intake);
10+
}
11+
12+
void IntakeManualControl::OnTick(units::second_t dt) {
13+
if (_codriver->GetBButtonPressed()) {
14+
if (_rawControl == true) {
15+
_rawControl = false;
16+
} else {
17+
_rawControl = true;
18+
}
19+
}
20+
21+
if (_rawControl) {
22+
_intake->setState(IntakeState::kRaw);
23+
_intake->setRaw(_codriver->GetLeftY() * 10_V);
24+
std::cout << "Raw" << std::endl;
25+
} else {
26+
if (_codriver->GetYButtonPressed()) {
27+
_intake->setState(IntakeState::kIntake);
28+
}
29+
if (_codriver->GetAButtonPressed()) {
30+
_intake->setState(IntakeState::kPass);
31+
}
32+
}
33+
}
34+
35+
IntakeAutoControl::IntakeAutoControl(Intake* intake) : _intake(intake) {}
36+
37+
void IntakeAutoControl::OnTick(units::second_t dt) {
38+
if (_intake->GetConfig().intakeSensor->Get() == 1) {
39+
_intake->setState(IntakeState::kPass);
40+
} else if (_intake->GetConfig().magSensor->Get() == 0) {
41+
_intake->setState(IntakeState::kIdle);
42+
}
43+
}

‎src/main/cpp/Intake.cpp

Lines changed: 74 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,74 @@
1+
// Copyright (c) 2023-2024 CurtinFRC
2+
// Open Source Software, you can modify it according to the terms
3+
// of the MIT License at the root of this project
4+
5+
#include "Intake.h"
6+
7+
Intake::Intake(IntakeConfig config) : _config(config) {}
8+
9+
IntakeConfig Intake::GetConfig() {
10+
return _config;
11+
}
12+
13+
void Intake::OnUpdate(units::second_t dt) {
14+
switch (_state) {
15+
case IntakeState::kIdle: {
16+
// _config.IntakeMotor.motorController->SetVoltage(0_V);
17+
// if (_config.intakeSensor->Get()) {
18+
// setState(IntakeState::kHold);
19+
// }
20+
_stringStateName = "Idle";
21+
_setVoltage = 0_V;
22+
} break;
23+
case IntakeState::kRaw: {
24+
// _config.IntakeMotor.motorController->SetVoltage(_rawVoltage);
25+
_stringStateName = "Raw";
26+
_setVoltage = _rawVoltage;
27+
} break;
28+
case IntakeState::kEject: {
29+
// _config.IntakeMotor.motorController->SetVoltage(-5_V);
30+
// if (_config.intakeSensor->Get() == 0 && _config.magSensor->Get() == 0) {
31+
// setState(IntakeState::kIdle);
32+
// }
33+
_stringStateName = "Eject";
34+
_setVoltage = -5_V;
35+
} break;
36+
case IntakeState::kHold: {
37+
// _config.IntakeMotor.motorController->SetVoltage(0_V);
38+
_stringStateName = "Hold";
39+
_setVoltage = 0_V;
40+
} break;
41+
case IntakeState::kIntake: {
42+
// _config.IntakeMotor.motorController->SetVoltage(5_V);
43+
_stringStateName = "Intake";
44+
_setVoltage = 5_V;
45+
} break;
46+
case IntakeState::kPass: {
47+
// _config.IntakeMotor.motorController->SetVoltage(5_V);
48+
// if (_config.shooterSensor->Get()) {
49+
// setState(IntakeState::kIdle);
50+
// _stringStateName = "Pass";
51+
// }
52+
_setVoltage = 5_V;
53+
} break;
54+
default:
55+
std::cout << "Error: Intake in INVALID STATE." << std::endl;
56+
break;
57+
}
58+
_table->GetEntry("State: ").SetString(_stringStateName);
59+
_table->GetEntry("Motor Voltage: ").SetDouble(_setVoltage.value());
60+
// _table->GetEntry("Intake Sensor: ").SetBoolean(_config.intakeSensor->Get());
61+
// _table->GetEntry("Shooter Sensor: ").SetBoolean(_config.shooterSensor->Get());
62+
// _table->GetEntry("Magazine Sensor: ").SetBoolean(_config.magSensor->Get());
63+
64+
std::cout << _setVoltage.value() << std::endl;
65+
66+
_config.IntakeMotor.motorController->SetVoltage(_setVoltage);
67+
}
68+
69+
void Intake::setState(IntakeState state) {
70+
_state = state;
71+
}
72+
void Intake::setRaw(units::volt_t voltage) {
73+
_rawVoltage = voltage;
74+
}

‎src/main/cpp/Robot.cpp

Lines changed: 20 additions & 14 deletions
Original file line numberDiff line numberDiff line change
@@ -4,18 +4,20 @@
44

55
#include "Robot.h"
66

7-
// include units
8-
#include <units/velocity.h>
7+
#include <frc/TimedRobot.h>
8+
#include <frc/Timer.h>
9+
#include <frc/controller/RamseteController.h>
10+
#include <frc/kinematics/DifferentialDriveKinematics.h>
11+
#include <networktables/DoubleTopic.h>
12+
#include <networktables/NetworkTable.h>
13+
#include <networktables/NetworkTableInstance.h>
914
#include <units/acceleration.h>
10-
#include <units/length.h>
1115
#include <units/angle.h>
16+
#include <units/length.h>
1217
#include <units/time.h>
18+
#include <units/velocity.h>
1319
#include <units/voltage.h>
1420

15-
#include <frc/kinematics/DifferentialDriveKinematics.h>
16-
#include <frc/controller/RamseteController.h>
17-
#include <frc/Timer.h>
18-
1921
static units::second_t lastPeriodic;
2022

2123
void Robot::RobotInit() {
@@ -32,6 +34,7 @@ void Robot::RobotPeriodic() {
3234
_swerveDrive->OnUpdate(dt);
3335
mag->OnUpdate(dt);
3436
climber->OnUpdate(dt);
37+
intake->OnUpdate(dt);
3538
}
3639

3740
void Robot::TeleopInit() {
@@ -72,21 +75,24 @@ void Robot::TeleopInit() {
7275
wom::BehaviourScheduler::GetInstance()->Register(mag);
7376
mag->SetDefaultBehaviour(
7477
[this]() { return wom::make<MagManualControl>(mag, &robotmap.controllers.coDriver); });
78+
79+
intake = new Intake(robotmap.intakeSystem.config);
80+
wom::BehaviourScheduler::GetInstance()->Register(intake);
81+
intake->SetDefaultBehaviour(
82+
[this]() { return wom::make<IntakeManualControl>(intake, &robotmap.controllers.coDriver); });
83+
7584
// m_driveSim = new wom::TempSimSwerveDrive(&simulation_timer, &m_field);
7685
// m_driveSim = wom::TempSimSwerveDrive();
7786
}
7887

7988
void Robot::AutonomousInit() {
80-
// m_driveSim->SetPath(m_path_chooser.GetSelected());
81-
8289
loop.Clear();
83-
sched->InterruptAll();
84-
// _swerveDrive->OnStart();
85-
}
86-
void Robot::AutonomousPeriodic() {
87-
// m_driveSim->OnUpdate();
90+
wom::BehaviourScheduler* scheduler = wom::BehaviourScheduler::GetInstance();
91+
scheduler->InterruptAll();
8892
}
8993

94+
void Robot::AutonomousPeriodic() {}
95+
9096
void Robot::TeleopPeriodic() {}
9197

9298
void Robot::DisabledInit() {}

‎src/main/include/Behaviours/IntakeBehaviour.h

Lines changed: 34 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,34 @@
1+
// Copyright (c) 2023-2024 CurtinFRC
2+
// Open Source Software, you can modify it according to the terms
3+
// of the MIT License at the root of this project
4+
5+
#pragma once
6+
7+
#include <frc/XboxController.h>
8+
9+
#include "Intake.h"
10+
#include "Wombat.h"
11+
12+
class IntakeManualControl : public behaviour::Behaviour {
13+
public:
14+
explicit IntakeManualControl(Intake* intake, frc::XboxController* codriver);
15+
16+
void OnTick(units::second_t dt) override;
17+
18+
private:
19+
Intake* _intake;
20+
frc::XboxController* _codriver;
21+
22+
units::volt_t _rawVoltage;
23+
bool _rawControl;
24+
};
25+
26+
class IntakeAutoControl : public behaviour::Behaviour {
27+
public:
28+
explicit IntakeAutoControl(Intake* intake);
29+
30+
void OnTick(units::second_t dt) override;
31+
32+
private:
33+
Intake* _intake;
34+
};

‎src/main/include/Intake.h

Lines changed: 41 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,41 @@
1+
// Copyright (c) 2023-2024 CurtinFRC
2+
// Open Source Software, you can modify it according to the terms
3+
// of the MIT License at the root of this project
4+
5+
#pragma once
6+
7+
#include <frc/DigitalInput.h>
8+
9+
#include <memory>
10+
#include <string>
11+
12+
#include "Wombat.h"
13+
14+
struct IntakeConfig {
15+
wom::Gearbox IntakeMotor;
16+
frc::DigitalInput* intakeSensor;
17+
frc::DigitalInput* magSensor;
18+
frc::DigitalInput* shooterSensor;
19+
};
20+
21+
enum class IntakeState { kIdle, kRaw, kHold, kEject, kIntake, kPass };
22+
23+
class Intake : public behaviour::HasBehaviour {
24+
public:
25+
explicit Intake(IntakeConfig config);
26+
27+
void OnUpdate(units::second_t dt);
28+
29+
void setState(IntakeState state);
30+
void setRaw(units::volt_t voltage);
31+
IntakeConfig GetConfig();
32+
33+
private:
34+
IntakeConfig _config;
35+
IntakeState _state = IntakeState::kIdle;
36+
37+
units::volt_t _rawVoltage = 0_V;
38+
std::string _stringStateName = "error";
39+
units::volt_t _setVoltage = 0_V;
40+
std::shared_ptr<nt::NetworkTable> _table = nt::NetworkTableInstance::GetDefault().GetTable("Intake");
41+
};

‎src/main/include/Robot.h

Lines changed: 3 additions & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -20,9 +20,10 @@
2020
#include <string>
2121

2222
#include "Behaviours/ClimberBehaviour.h"
23+
#include "Behaviours/IntakeBehaviour.h"
24+
#include "Behaviours/MagBehaviour.h"
2325
#include "RobotMap.h"
2426
#include "Wombat.h"
25-
#include "behaviours/MagBehaviour.h"
2627

2728
class Robot : public frc::TimedRobot {
2829
public:
@@ -53,4 +54,5 @@ class Robot : public frc::TimedRobot {
5354
wom::SwerveDrive* _swerveDrive;
5455
Mag* mag;
5556
Climber* climber;
57+
Intake* intake;
5658
};

‎src/main/include/RobotMap.h

Lines changed: 27 additions & 8 deletions
Original file line numberDiff line numberDiff line change
@@ -19,9 +19,10 @@
1919
#include <ctre/phoenix6/Pigeon2.hpp>
2020
#include <ctre/phoenix6/TalonFX.hpp>
2121

22-
#include "Mag.h"
2322
#include "Climber.h"
23+
#include "Mag.h"
2424
#include "Wombat.h"
25+
#include "Intake.h"
2526

2627
struct RobotMap {
2728
struct Controllers {
@@ -134,6 +135,18 @@ struct RobotMap {
134135
};
135136
SwerveBase swerveBase;
136137

138+
struct Climber {
139+
rev::CANSparkMax* climberMotor = new rev::CANSparkMax{99, rev::CANSparkMax::MotorType::kBrushless};
140+
wom::CANSparkMaxEncoder climberEncoder{climberMotor, 0.1_m};
141+
142+
wom::Gearbox climberGearbox{climberMotor, &climberEncoder, frc::DCMotor::NEO(1)};
143+
144+
ClimberConfig config{
145+
climberGearbox,
146+
};
147+
};
148+
Climber climberSystem;
149+
137150
struct Mag {
138151
rev::CANSparkMax* magMotor = new rev::CANSparkMax{99, rev::CANSparkMax::MotorType::kBrushless};
139152
// wom::VoltageController magMotorGroup = wom::VoltageController::Group(magMotor);
@@ -153,15 +166,21 @@ struct RobotMap {
153166
};
154167
Mag magSystem;
155168

156-
struct Climber {
157-
rev::CANSparkMax* climberMotor = new rev::CANSparkMax{99, rev::CANSparkMax::MotorType::kBrushless};
158-
wom::CANSparkMaxEncoder climberEncoder{climberMotor, 0.1_m};
169+
struct IntakeSystem {
170+
rev::CANSparkMax intakeMotor{2, rev::CANSparkMax::MotorType::kBrushed};
171+
// wom::CANSparkMaxEncoder intakeEncoder{&intakeMotor, 0.1_m};
172+
// frc::DigitalInput intakeSensor{0};
173+
// frc::DigitalInput magSensor{0};
174+
// frc::DigitalInput shooterSensor{0};
159175

160-
wom::Gearbox climberGearbox{climberMotor, &climberEncoder, frc::DCMotor::NEO(1)};
176+
wom::Gearbox IntakeGearbox{&intakeMotor, nullptr, frc::DCMotor::CIM(1)};
161177

162-
ClimberConfig config{
163-
climberGearbox,
178+
IntakeConfig config {
179+
IntakeGearbox,
180+
// &intakeSensor,
181+
// &magSensor,
182+
// &shooterSensor
164183
};
165184
};
166-
Climber climberSystem;
185+
IntakeSystem intakeSystem;
167186
};

0 commit comments

Comments
 (0)