Skip to content

Commit 0224043

Browse files
PhyXTGears-1720PhyXTGears-1720
committed
Finish climb command and fix drive
Tested, FULLY WORKING - Robby S, Logan H-D
1 parent c3a6676 commit 0224043

File tree

9 files changed

+102
-36
lines changed

9 files changed

+102
-36
lines changed

‎.vscode/settings.json

+4-1
Original file line numberDiff line numberDiff line change
@@ -56,7 +56,10 @@
5656
"type_traits": "cpp",
5757
"tuple": "cpp",
5858
"typeinfo": "cpp",
59-
"utility": "cpp"
59+
"utility": "cpp",
60+
"hash_map": "cpp",
61+
"hash_set": "cpp",
62+
"unordered_set": "cpp"
6063
},
6164
"C_Cpp.default.configurationProvider": "vscode-wpilib"
6265
}

‎src/main/cpp/Robot.cpp

+20-1
Original file line numberDiff line numberDiff line change
@@ -69,7 +69,9 @@ void Robot::AutonomousInit() {}
6969

7070
void Robot::AutonomousPeriodic() {}
7171

72-
void Robot::TeleopInit() {}
72+
void Robot::TeleopInit() {
73+
GetDriveTrain().RunReset();
74+
}
7375

7476
void Robot::TeleopPeriodic() {
7577
if (m_OI.GetDriverJoystick().GetXButtonPressed()) {
@@ -82,7 +84,24 @@ void Robot::TeleopPeriodic() {
8284
Robot::GetCreeperClimb().SetSolenoidAscend(false);
8385
} else if (m_OI.GetDriverJoystick().GetYButtonPressed()) {
8486
m_ReadyCreeperArm->Start();
87+
} else if (m_OI.GetDriverJoystick().GetBackButtonPressed()) {
88+
Robot::GetCreeperClimb().RotateArmToPosition("home");
89+
}
90+
91+
if (m_OI.GetDriverJoystick().GetStartButton()) {
92+
Robot::GetCreeperClimb().SetArmWheels(true); // wheels need to be SLOWED!!
93+
std::cout << "start" << std::endl;
94+
} else if (m_OI.GetDriverJoystick().GetStartButtonReleased()) {
95+
Robot::GetCreeperClimb().SetArmWheels(false);
8596
}
97+
98+
// if (m_OI.GetDriverJoystick().GetBumperPressed(frc::XboxController::kLeftHand)) {
99+
// m_ClimbStep--;
100+
// } else if (m_OI.GetDriverJoystick().GetBumperPressed(frc::XboxController::kLeftHand)) {
101+
// m_ClimbStep++;
102+
// }
103+
104+
frc::SmartDashboard::PutNumber("climb stage", m_ClimbStep->m_Segment);
86105
}
87106

88107
void Robot::TestPeriodic() {}

‎src/main/cpp/commands/ClimbStep.cpp

+40-25
Original file line numberDiff line numberDiff line change
@@ -36,6 +36,9 @@ void ClimbStep::Initialize() {
3636
// Prevent drive team from activating ClimbStep before ReadyCreeperArm
3737
// runs. Otherwise, the climb arm may swing into the cargo-intake.
3838
m_HasPrerequisites = Robot::GetCreeperClimb().IsArmAtPosition("arm-ready");
39+
m_Segment = Segment::Initialize;
40+
41+
Robot::GetCreeperClimb().SetOutputRange(-0.7, 0.7);
3942
}
4043

4144
void ClimbStep::Execute() {
@@ -45,55 +48,67 @@ void ClimbStep::Execute() {
4548
}
4649

4750
switch (m_Segment) {
48-
case 0:
51+
case Segment::Initialize: // Initialization, runs once
4952
Robot::GetCreeperClimb().RotateArmToPosition("arm-climb");
5053

5154
Robot::GetCreeperClimb().SetSolenoidDescend(false);
5255
Robot::GetCreeperClimb().SetSolenoidAscend(true);
5356

54-
// if (Robot::GetCreeperClimb().GetSolenoidSwitch() == true) {
55-
// Robot::GetCreeperClimb().SetSolenoidDescend(true);
56-
// Robot::GetCreeperClimb().SetSolenoidAscend(false);
57-
// }
57+
m_Segment = Segment::CheckSwitch;
5858
break;
59-
case 1:
60-
/*
61-
Robot::GetCreeperClimb().SetArmWheels(true); // wheels need to be SLOWED!!
62-
Robot::GetDriveTrain().Drive(0.2, 0.2); // move robot wheels slow
63-
*/
59+
case Segment::CheckSwitch:
60+
if (Robot::GetCreeperClimb().GetSolenoidSwitch()) {
61+
Robot::GetCreeperClimb().SetSolenoidDescend(true);
62+
Robot::GetCreeperClimb().SetSolenoidAscend(true);
63+
64+
m_Segment = Segment::CheckArm;
65+
}
6466
break;
65-
case 2:
66-
/*
67-
Robot::GetCreeperClimb().SetSolenoidDescend(false);
68-
Robot::GetCreeperClimb().SetSolenoidAscend(true);
69-
*/
67+
case Segment::CheckArm:
68+
if (Robot::GetCreeperClimb().IsArmAtPosition("arm-climb")) {
69+
m_Segment = Segment::RollCreeper;
70+
}
71+
case Segment::RollCreeper:
72+
Robot::GetCreeperClimb().SetArmWheels(true);
73+
m_Delay.Start();
74+
75+
m_Segment = Segment::StopCreeper;
76+
break;
77+
case Segment::StopCreeper:
78+
if (m_Delay.IsDone()) {
79+
m_Delay.Stop();
80+
Robot::GetCreeperClimb().SetArmWheels(false);
81+
m_Segment = Segment::RaiseSolenoids;
82+
}
7083
break;
71-
case 3:
72-
/*
73-
Robot::GetDriveTrain().Drive(0.2, 0.2); // move robot wheels slow
74-
*/
84+
case Segment::RaiseSolenoids:
85+
Robot::GetCreeperClimb().RotateArmToPosition("home");
86+
Robot::GetCreeperClimb().SetSolenoidDescend(true);
87+
Robot::GetCreeperClimb().SetSolenoidAscend(false);
88+
m_Segment = Segment::End;
89+
break;
90+
case Segment::End:
7591
break;
7692
default:
7793
cout << "Unknown segment in ClimbStep! (segment:" << m_Segment << ")" << endl;
7894
}
7995

80-
if (Robot::m_OI.GetOperatorJoystick().GetAButtonPressed()) {
81-
m_Segment++; // next segment every A button press
82-
} else if (Robot::m_OI.GetOperatorJoystick().GetBButtonPressed()) {
83-
m_Segment--; // let operator go back a segment in case of a mis-press
84-
}
96+
cout << "segment: " << m_Segment << endl;
97+
Robot::GetDriveTrain().ArcadeDrive(0, 0);
8598
}
8699

87100
bool ClimbStep::IsFinished() {
88-
return ! m_HasPrerequisites;
101+
return !m_HasPrerequisites || m_Segment == Segment::End;
89102
}
90103

91104
void ClimbStep::End() {
92105
// Make sure rollers are stopped.
93106
Robot::GetCreeperClimb().StopArmWheels();
107+
Robot::GetCreeperClimb().SetOutputRange(-1, 1);
94108
}
95109

96110
void ClimbStep::Interrupted() {
97111
// Make sure rollers are stopped.
98112
Robot::GetCreeperClimb().StopArmWheels();
113+
Robot::GetCreeperClimb().SetOutputRange(-1, 1);
99114
}

‎src/main/cpp/subsystems/CreeperClimb.cpp

+6-2
Original file line numberDiff line numberDiff line change
@@ -32,7 +32,7 @@ CreeperClimb::CreeperClimb(wpi::json &jsonConfig) : Subsystem("CreeperClimb") {
3232
m_ArmRotate.SetInverted(true);
3333

3434
AddChild("Climb Arm PID", m_RotationPID);
35-
AddChild("Climb Arm Angle", m_ArmAngle);
35+
AddChild("Climb Arm Angle", m_ArmPosition);
3636
AddChild("Climb Arm Motor", m_ArmRotate);
3737
AddChild("Climb Arm Wheels", m_ArmDrive);
3838
AddChild("Climb Sol Ascend", m_SolAscend);
@@ -59,7 +59,7 @@ double CreeperClimb::GetCurrentArmPosition() {
5959
void CreeperClimb::SetArmRotateSpeed(double spd) { m_ArmRotate.Set(spd); }
6060

6161
void CreeperClimb::SetArmWheels(bool on) {
62-
m_ArmDrive.Set(on ? frc::Relay::kForward : frc::Relay::kOff);
62+
m_ArmDrive.Set(on ? frc::Relay::kOn : frc::Relay::kOff);
6363
}
6464

6565
void CreeperClimb::StopArmWheels() {
@@ -95,6 +95,10 @@ void CreeperClimb::StopArmRotation() {
9595
m_RotationPID.Disable();
9696
}
9797

98+
void CreeperClimb::SetOutputRange(double minimumOutput, double maximumOutput) {
99+
m_RotationPID.SetOutputRange(minimumOutput, maximumOutput);
100+
}
101+
98102

99103
// HELPER FUNCTIONS
100104

‎src/main/cpp/subsystems/DriveTrain.cpp

+8-1
Original file line numberDiff line numberDiff line change
@@ -55,7 +55,7 @@ void DriveTrain::Drive(frc::XboxController& driver) {
5555
double hidX = -driver.GetX(frc::XboxController::kRightHand);
5656
double hidY = driver.GetY(frc::XboxController::kLeftHand);
5757

58-
ArcadeDrive(hidY, hidX, true);
58+
ArcadeDrive(hidY, hidX);
5959
}
6060

6161
void DriveTrain::ArcadeDrive(double xSpeed, double zRotation, bool squareInputs) {
@@ -137,3 +137,10 @@ void DriveTrain::InitSendable(frc::SendableBuilder& builder) {
137137
[=](double value) { return m_RightPID.SetSetpoint(value); }
138138
);
139139
}
140+
141+
void DriveTrain::RunReset() {
142+
m_RightPID.Reset();
143+
m_LeftPID.Reset();
144+
m_RightPID.Enable();
145+
m_LeftPID.Enable();
146+
}

‎src/main/deploy/config.json

+5-4
Original file line numberDiff line numberDiff line change
@@ -20,15 +20,16 @@
2020
"drive": {},
2121
"climb": {
2222
"PID": {
23-
"P": 0.060,
24-
"I": 0.0000000,
25-
"D": 0.0000000
23+
"P": 0.05,
24+
"I": 0.0,
25+
"D": 0.0
2626
},
2727
"rotation": {
2828
"zero-point": 0.0,
2929
"angles": {
30+
"home": 82,
3031
"arm-ready": 145,
31-
"arm-climb": 200
32+
"arm-climb": 197
3233
}
3334
}
3435
}

‎src/main/include/commands/ClimbStep.h

+14-1
Original file line numberDiff line numberDiff line change
@@ -2,6 +2,18 @@
22

33
#include <frc/commands/Command.h>
44

5+
#include "util/Delay.h"
6+
7+
enum Segment {
8+
Initialize,
9+
CheckSwitch,
10+
CheckArm,
11+
RollCreeper,
12+
StopCreeper,
13+
RaiseSolenoids,
14+
End
15+
};
16+
517
class ClimbStep : public frc::Command {
618
public:
719
ClimbStep();
@@ -10,7 +22,8 @@ class ClimbStep : public frc::Command {
1022
bool IsFinished() override;
1123
void End() override;
1224
void Interrupted() override;
25+
Segment m_Segment = Segment::Initialize;
1326
private:
14-
int m_Segment = 0;
1527
bool m_HasPrerequisites;
28+
Delay m_Delay{2.5};
1629
};

‎src/main/include/subsystems/CreeperClimb.h

+2
Original file line numberDiff line numberDiff line change
@@ -36,6 +36,8 @@ class CreeperClimb : public frc::Subsystem {
3636
// get solenoid reed switch state
3737
bool GetSolenoidSwitch();
3838

39+
void SetOutputRange(double minimumOutput, double maximumOutput);
40+
3941
frc::PIDController& GetArmPID() { return m_RotationPID; }
4042

4143
private:

‎src/main/include/subsystems/DriveTrain.h

+3-1
Original file line numberDiff line numberDiff line change
@@ -32,12 +32,14 @@ class DriveTrain : public frc::Subsystem, public frc::RobotDriveBase {
3232
void InitDefaultCommand() override;
3333
void Drive(double left, double right);
3434
void Drive(frc::XboxController& driver);
35-
void ArcadeDrive(double xSpeed, double zRotation, bool squareInputs);
35+
void ArcadeDrive(double xSpeed, double zRotation, bool squareInputs = false);
3636

3737
void StopMotor() override;
3838
void GetDescription(wpi::raw_ostream& desc) const override;
3939
void InitSendable(frc::SendableBuilder& builder) override;
4040

41+
void RunReset();
42+
4143
private:
4244
// Motors
4345
MOTORTYPE m_MotorRightFront {kRightFrontMotor};

0 commit comments

Comments
 (0)