Skip to content

Commit 771e888

Browse files
boxofroxboxofrox
authored
Merge pull request #84 from PhyXTGears-programming/competition/kokomo
Competition/kokomo
2 parents f18990b + 0c116a6 commit 771e888

13 files changed

+151
-32
lines changed

‎src/main/cpp/Robot.cpp

+105-9
Original file line numberDiff line numberDiff line change
@@ -2,6 +2,12 @@
22

33
#include "util/util.h"
44

5+
// Select hatch/piston controls
6+
//
7+
// 0 = Tippecanoe controls. No PID hatch arm. 2 hatch buttons.
8+
// 1 = Kokomo controls. PID hatch arm. 3 hatch buttons.
9+
#define HATCH_CONTROL_TYPE 1
10+
511
// Initialize Operator Interface
612
OI Robot::m_OI;
713
// Initialize Subsystems
@@ -108,17 +114,23 @@ void Robot::RobotInit() {
108114
}
109115

110116
void Robot::RobotPeriodic() {
111-
frc::SmartDashboard::PutNumber("flight throttle",
112-
m_OI.GetOperatorConsole().GetThrottle());
113-
frc::SmartDashboard::PutNumber("intake rotation",
114-
GetCargoIntake().GetIntakeRotation());
115-
frc::SmartDashboard::PutNumber("climb arm rotation",
116-
GetCreeperClimb().GetCurrentArmPosition());
117+
frc::SmartDashboard::PutNumber("flight throttle", m_OI.GetOperatorConsole().GetThrottle());
118+
frc::SmartDashboard::PutNumber("intake rotation", GetCargoIntake().GetIntakeRotation());
119+
frc::SmartDashboard::PutNumber("climb arm rotation", GetCreeperClimb().GetCurrentArmPosition());
117120
frc::SmartDashboard::PutNumber("climb stage", m_ClimbStep->GetSegment());
118-
const double psiPerVolt = 150.0 / 1.1;
119-
frc::SmartDashboard::PutNumber("air pressure", (m_AirPressureMeter.GetVoltage() - 0.3) * psiPerVolt);
120121
frc::SmartDashboard::PutBoolean("climb ready", GetCreeperClimb().IsArmAtPosition("arm-ready"));
121122
frc::SmartDashboard::PutBoolean("climb done", GetCreeperClimb().IsArmAtPosition("arm-climb"));
123+
frc::SmartDashboard::PutNumber("hatch anlge", GetHatchMechanism().GetArmPosition());
124+
125+
bool manualControl = abs(m_OI.GetOperatorConsole().GetThrottle()) >= 0.75;
126+
bool cargoControl = m_OI.GetOperatorConsole().GetThrottle() >= 0.75;
127+
frc::SmartDashboard::PutBoolean("manual control", manualControl);
128+
frc::SmartDashboard::PutString("manual mode", manualControl ? (cargoControl ? "Cargo" : "Creeper") : "None");
129+
130+
// 3.36 = 110
131+
const double psiPerVolt = 1.0 / 1.0;
132+
const double psiOffset = 0.0;
133+
frc::SmartDashboard::PutNumber("air pressure", (m_AirPressureMeter.GetVoltage() * psiPerVolt) + psiOffset);
122134

123135
{
124136
static bool lastCargoState = false;
@@ -175,6 +187,7 @@ void Robot::AutonomousInit() {
175187
// Some positions of hatch mechanism are compliant, so disable
176188
// PID/rotation instead of RunReset.
177189
GetHatchMechanism().StopRotation();
190+
GetHatchMechanism().HoldPID();
178191

179192
m_CanSandstormStepDrive = true;
180193

@@ -253,6 +266,13 @@ void Robot::CompetitionJoystickInput() {
253266
m_Bling.SetBling(m_Bling.CargoIntakePattern);
254267
}
255268

269+
#if HATCH_CONTROL_TYPE==0
270+
// Hatch controls for Compeition 2 Tippecanoe.
271+
// 2 vertical black buttons.
272+
//
273+
// Top button - RAISE + GRAB hatch.
274+
// Bottom button - LOWER + RELEASE hatch.
275+
256276
if (console.GetHatchGrabPressed()) {
257277
std::cout << "Comp Joy Input: Console: Hatch Grab Pressed" << std::endl;
258278
m_RaiseHatch
@@ -267,6 +287,35 @@ void Robot::CompetitionJoystickInput() {
267287
m_Bling.SetBling(m_Bling.HatchPattern);
268288
}
269289

290+
// Piston controls for Competition 2 Tippecanoe.
291+
// Top 2 of 3 vertical buttons along right edge of console.
292+
//
293+
// Top button - RETRACT piston.
294+
// Middle button - EXTEND piston.
295+
296+
if (console.GetHatchTopPositionPressed()) {
297+
std::cout << "Comp Joy Input: Console: Piston Extend Pressed" << std::endl;
298+
GetCreeperClimb().PistonExtend();
299+
} else if (console.GetHatchTopPositionReleased()) {
300+
std::cout << "Comp Joy Input: Console: Piston Extend Released" << std::endl;
301+
GetCreeperClimb().PistonHold();
302+
} else if (console.GetHatchMidPositionPressed()) {
303+
std::cout << "Comp Joy Input: Console: Piston Retract Pressed" << std::endl;
304+
GetCreeperClimb().PistonRetract();
305+
} else if (console.GetHatchMidPositionReleased()) {
306+
std::cout << "Comp Joy Input: Console: Piston Retract Released" << std::endl;
307+
GetCreeperClimb().PistonHold();
308+
}
309+
310+
#elif HATCH_CONTROL_TYPE==1
311+
312+
// Hatch controls for Competition 3 Kokomo.
313+
// 3 buttons on right edge of console.
314+
//
315+
// Top button - GRAB hatch + TOP position + HOLD pid.
316+
// Middle button - GRAB hatch + MID position + HOLD pid.
317+
// Lower button - RELEASE hatch + rotate DOWN + no pid.
318+
270319
if (console.GetHatchTopPositionPressed()) {
271320
std::cout << "Comp Joy Input: Console: Hatch Top Position Pressed" << std::endl;
272321
m_GrabHatchFromLoadingStation->Start();
@@ -283,6 +332,53 @@ void Robot::CompetitionJoystickInput() {
283332
m_Bling.SetBling(m_Bling.HatchPattern);
284333
}
285334

335+
// Hatch controls for Compeition 2 Tippecanoe.
336+
// 2 vertical black buttons.
337+
//
338+
// Top button - RAISE + GRAB hatch.
339+
// Bottom button - LOWER + RELEASE hatch.
340+
341+
if (console.GetHatchGrabPressed()) {
342+
std::cout << "Comp Joy Input: Console: Hatch Grab Pressed" << std::endl;
343+
m_RaiseHatch
344+
->Until([]() { return Robot::m_OI.GetOperatorConsole().GetHatchGrabReleased(); })
345+
->Start();
346+
m_Bling.SetBling(m_Bling.HatchPattern);
347+
} else if (console.GetHatchReleasePressed()) {
348+
std::cout << "Comp Joy Input: Console: Hatch Release Pressed" << std::endl;
349+
m_LowerHatch
350+
->Until([]() { return Robot::m_OI.GetOperatorConsole().GetHatchReleaseReleased(); })
351+
->Start();
352+
m_Bling.SetBling(m_Bling.HatchPattern);
353+
}
354+
355+
if (console.GetOpenHatchGripperPressed()) {
356+
GetHatchMechanism().ReleaseHatch();
357+
} else if (console.GetOpenHatchGripperReleased()) {
358+
GetHatchMechanism().GrabHatch();
359+
}
360+
361+
// Piston controls for Competition 3 Kokomo.
362+
// 2 vertical black buttons (originally for hatch)
363+
// Top button - RETRACT piston.
364+
// Bottom button - EXTEND piston.
365+
366+
if (console.GetCreeperExtendPistonPressed()) {
367+
std::cout << "Comp Joy Input: Console: Piston Extend Pressed" << std::endl;
368+
GetCreeperClimb().PistonExtend();
369+
} else if (console.GetCreeperExtendPistonReleased()) {
370+
std::cout << "Comp Joy Input: Console: Piston Extend Released" << std::endl;
371+
GetCreeperClimb().PistonHold();
372+
} else if (console.GetCreeperRetractPistonPressed()) {
373+
std::cout << "Comp Joy Input: Console: Piston Retract Pressed" << std::endl;
374+
GetCreeperClimb().PistonRetract();
375+
} else if (console.GetCreeperRetractPistonReleased()) {
376+
std::cout << "Comp Joy Input: Console: Piston Retract Released" << std::endl;
377+
GetCreeperClimb().PistonHold();
378+
}
379+
380+
#endif
381+
286382
if (console.GetCargoShootRocketTwoPressed()) {
287383
std::cout << "Comp Joy Input: Console: Rocket Shot (Level 2)" << std::endl;
288384
m_ShootCargoForLevelTwoRocket->Start();
@@ -292,7 +388,7 @@ void Robot::CompetitionJoystickInput() {
292388
if (console.GetCreeperReadyArmPressed()) {
293389
std::cout << "Comp Joy Input: Console: Creeper Ready Arm Pressed" << std::endl;
294390
m_ReadyCreeperArm->Start();
295-
m_Bling.SetBling(m_Bling.Climb);
391+
m_Bling.SetBling(m_Bling.ClimbReady);
296392
} else if (console.GetCreeperClimbEnabled()) {
297393
std::cout << "Comp Joy Input: Console: Climb Sequence Pressed" << std::endl;
298394
m_ClimbStep->Start();

‎src/main/cpp/commands/ReadyHatch.cpp

+1-1
Original file line numberDiff line numberDiff line change
@@ -38,7 +38,7 @@ void ReadyHatch::Initialize() {
3838

3939
// Start moving to mid position.
4040
hatch.RotateToMidPosition();
41-
hatch.ReleaseHatch();
41+
hatch.GrabHatch();
4242
}
4343

4444
void ReadyHatch::Execute() {}

‎src/main/cpp/commands/RotateCargoForCargoShip.cpp

+1-1
Original file line numberDiff line numberDiff line change
@@ -38,5 +38,5 @@ void RotateCargoForCargoShip::End() {
3838
}
3939

4040
void RotateCargoForCargoShip::Interrupted() {
41-
Robot::GetCargoIntake().StopRotation();
41+
Robot::GetCargoIntake().HoldRotation();
4242
}

‎src/main/cpp/commands/RotateCargoForLevelOneRocket.cpp

+1-1
Original file line numberDiff line numberDiff line change
@@ -38,5 +38,5 @@ void RotateCargoForLevelOneRocket::End() {
3838
}
3939

4040
void RotateCargoForLevelOneRocket::Interrupted() {
41-
Robot::GetCargoIntake().StopRotation();
41+
Robot::GetCargoIntake().HoldRotation();
4242
}

‎src/main/cpp/subsystems/CargoIntake.cpp

+5
Original file line numberDiff line numberDiff line change
@@ -146,6 +146,11 @@ void CargoIntake::StopRotation() {
146146
m_RotationPID.Disable();
147147
}
148148

149+
void CargoIntake::HoldRotation() {
150+
m_RotationPID.SetSetpoint(GetIntakeRotation());
151+
m_RotationPID.Enable();
152+
}
153+
149154
double CargoIntake::GetIntakeRotation() {
150155
return machineAngleToWorld(m_IntakeRotation.Get());
151156
}

‎src/main/cpp/subsystems/HatchMechanism.cpp

+6
Original file line numberDiff line numberDiff line change
@@ -20,6 +20,7 @@ HatchMechanism::HatchMechanism(wpi::json &jsonConfig) : Subsystem("HatchMechanis
2020
m_ArmMotor.SetInverted(true);
2121

2222
m_ArmPID.SetPID(p, i, d);
23+
HoldPID();
2324

2425
m_Config.MidPosition = jsonConfig["hatch"]["rotation"]["mid-position"];
2526
m_Config.TopPosition = jsonConfig["hatch"]["rotation"]["top-position"];
@@ -86,6 +87,11 @@ void HatchMechanism::SetRotateSpeed(double speed) {
8687
m_ArmMotor.Set(speed);
8788
}
8889

90+
void HatchMechanism::HoldPID() {
91+
m_ArmPID.SetSetpoint(m_ArmPosition.Get());
92+
m_ArmPID.Enable();
93+
}
94+
8995
void HatchMechanism::Disable() {
9096
SetRotateSpeed(0.0);
9197
m_ArmPID.Reset();

‎src/main/deploy/compbot-config.json

+5-5
Original file line numberDiff line numberDiff line change
@@ -26,7 +26,7 @@
2626
"D": 0.0
2727
},
2828
"rotation": {
29-
"zero-point": 0.0,
29+
"zero-point": -0.461,
3030
"angles": {
3131
"home": 0.81,
3232
"cargo-ship-shoot": 0.81,
@@ -60,23 +60,23 @@
6060
"zero-point": 30
6161
,
6262
"angles": {
63-
"home": 85,
63+
"home": 80,
6464
"arm-ready": 161,
6565
"arm-climb": 220
6666
}
6767
}
6868
},
6969
"hatch": {
7070
"PID": {
71-
"P": 1.0,
71+
"P": 0.75,
7272
"I": 0.0,
7373
"D": 0.0,
7474
"F": 0.0
7575
},
7676
"rotation": {
77-
"grip-threshold": 1.9,
77+
"grip-threshold": 1.5,
7878
"top-position": 0.55,
79-
"mid-position": 2.0
79+
"mid-position": 1.5
8080
}
8181
}
8282
}

‎src/main/include/Bling.h

+6-3
Original file line numberDiff line numberDiff line change
@@ -15,20 +15,22 @@ class Bling {
1515
IntakeRotateShip,
1616
IntakeRotateRocket,
1717
HatchPattern,
18+
ClimbReady,
1819
Climb,
1920
SizeValue
2021
};
2122

2223
private:
2324
const double m_PwmMap [SizeValue] = {
24-
[Default] = -0.99, // Rainbow
25+
[Default] = -0.91, // Rainbow Forest Palette
2526
[CargoIntakePattern] = 0.65, // Orange
2627
[CargoShootShipPattern] = -0.39, // Color Waves, Lava Palette
2728
[CargoShootRocketPattern] = -0.57, // Large Fire
2829
[IntakeRotateShip] = -0.91, // Rainbow - Forest
2930
[IntakeRotateRocket] = 0.75, // Dark Green
3031
[HatchPattern] = -0.07, // Strobe Gold
31-
[Climb] = -0.13, // Breathe Gray
32+
[ClimbReady] = -0.13, // Breathe Gray
33+
[Climb] = -0.99, // Rainbow
3234
};
3335

3436
const double m_DelayMap [SizeValue] = {
@@ -39,7 +41,8 @@ class Bling {
3941
[IntakeRotateShip] = 2.0,
4042
[IntakeRotateRocket] = 2.0,
4143
[HatchPattern] = 2.0,
42-
[Climb] = 5.0,
44+
[ClimbReady] = 3.0,
45+
[Climb] = 10.0,
4346
};
4447

4548
public:

‎src/main/include/OperatorHID.h

+9-1
Original file line numberDiff line numberDiff line change
@@ -35,6 +35,8 @@
3535
#define bHatchMidPosition 9
3636
#define bHatchLowPosition 8
3737

38+
// Using Joystick 9 and 11
39+
3840
// A combo class that encapsules our button board and flight stick.
3941
// This is only for our robot operator to use.
4042
class OperatorHID {
@@ -73,7 +75,13 @@ class OperatorHID {
7375
bool IsCreeperCrawlForwardDown() { return m_Board1.GetButton(bCreeperCrawlForward); }
7476
bool GetCreeperCrawlForwardReleased() { return m_Board1.GetButtonReleased(bCreeperCrawlForward); }
7577

76-
bool GetCreeperRetractPistonPressed() { return false; }
78+
bool GetCreeperRetractPistonPressed() { return m_FlightStick.GetRawButtonPressed(9); }
79+
bool GetCreeperRetractPistonReleased() { return m_FlightStick.GetRawButtonReleased(9); }
80+
bool GetCreeperExtendPistonPressed() { return m_FlightStick.GetRawButtonPressed(11); }
81+
bool GetCreeperExtendPistonReleased() { return m_FlightStick.GetRawButtonReleased(11); }
82+
83+
bool GetOpenHatchGripperPressed() { return m_Board1.GetButtonPressed(5); }
84+
bool GetOpenHatchGripperReleased() { return m_Board1.GetButtonReleased(5); }
7785

7886
double GetThrottle() { return m_FlightStick.GetThrottle(); }; // Throttle is the switch on the base of the stick
7987
double GetJoystickY() {

‎src/main/include/RobotMap.h

+7-9
Original file line numberDiff line numberDiff line change
@@ -128,7 +128,7 @@
128128
// Creeper Arm Climber constants
129129
// TalonSRX CAN ID, Relay pin, and Solenoid PCM IDs
130130
# define kCreeperArmDrive RELAY_0
131-
# define kCreeperArmAngle AIO_1
131+
# define kCreeperArmAngle AIO_3
132132
# define kCreeperArmRotate CAN_3
133133

134134
# define kCreeperSolenoidExtendLeft AIR_6
@@ -152,13 +152,11 @@
152152
# define kCargoRotationSensor AIO_0
153153

154154
// Hatch Mechanism
155-
# define kHatchArmMotor PWM_3
156-
157-
# define kNavxPin I2C::Port::kMXP
158-
159-
# define kAirPressureMeterPin AIO_3
160-
161-
# define kBlinkinPin PWM_9
162-
155+
# define kHatchArmMotor PWM_3
163156
# define kHatchArmPositionSensor AIO_2
157+
158+
// Miscellaneous
159+
# define kAirPressureMeterPin AIO_1
160+
# define kNavxPin I2C::Port::kMXP
161+
# define kBlinkinPin PWM_9
164162
#endif

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

+1-1
Original file line numberDiff line numberDiff line change
@@ -25,6 +25,6 @@ class ClimbStep : public frc::Command {
2525
Segment GetSegment() { return m_Segment; }
2626
private:
2727
bool m_HasPrerequisites;
28-
Delay m_Delay{2}; // Roll creeper delay.
28+
Delay m_Delay{1.5}; // Roll creeper delay.
2929
Segment m_Segment = Segment::Initialize;
3030
};

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

+1
Original file line numberDiff line numberDiff line change
@@ -47,6 +47,7 @@ class CargoIntake : public frc::Subsystem {
4747
void RotateToPosition(double position);
4848
void SetRotateSpeed(double speed);
4949
void StopRotation();
50+
void HoldRotation();
5051

5152
void GoHome();
5253
double GetIntakeRotation();

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

+3-1
Original file line numberDiff line numberDiff line change
@@ -35,9 +35,11 @@ class HatchMechanism : public frc::Subsystem {
3535

3636
bool IsArmRotationDone();
3737

38+
void HoldPID();
39+
3840
void Disable();
3941

40-
bool IsAboveGripThreshold() { return GetArmPosition() > m_Config.GripThreshold; }
42+
bool IsAboveGripThreshold() { return GetArmPosition() <= m_Config.GripThreshold; }
4143

4244
private:
4345
frc::Spark m_ArmMotor {kHatchArmMotor};

0 commit comments

Comments
 (0)