My main criticism would be on confusing naming, which is just a readability nit-pick, though that's just due to my coding standard (which nobody else is obligated to follow, of course).
(Ex: autonomous() implies runAutonomous() as opposed to getAutonomousThread())
---
Other things to think about
:
The system might be a bit strange to work with if you try to mix autonomous methods with teleoperated methods. Autonomous-ish methods were used in games like Breakaway and Logomotion, for line following, and goal alignment in Rebound Rumble.
We tried the following, and it worked pretty decently:
Shooter - Calculus Bazooka
Bridge Knockdown Mechanism: Del Operator
Tower - Transported ball collection mechanism to shooter.
Code:
public class SubsystemControlState
{
public boolean DriveTrainAutomated = false;
public boolean CalculusBazookaAutomated = false;
public boolean DelOperatorAutomated = false;
}
Code:
/**
* Runs the autonomous loops that are binded to certain buttons.
* The result of this method is an AutonomousLoopsState object, which tells
* the other methods what subsystems are available for them to manipulate.
*/
private SubsystemControlState SelectivelyRunAutonomousLoops()
{
SubsystemControlState subsystemControlStates = new SubsystemControlState();
//----------------------------------------------------------------------
//
// Del Operator
//
//----------------------------------------------------------------------
subsystemControlStates.DelOperatorAutomated = DelOperator.IsAutomated();
//----------------------------------------------------------------------
//
// Drive Train
//
//----------------------------------------------------------------------
// Move Towards Ball (L2)
//----------------------------------------------------------------------
if(Gamepad.IsDown(Gamepad.kButtonL2) && !subsystemControlStates.DriveTrainAutomated) //Move towards ball (hold)
{
m_moveToBallThreadEnabled = true;
subsystemControlStates.DriveTrainAutomated = true;
}else{
m_moveToBallThreadEnabled = false;
}
//----------------------------------------------------------------------
// Align to Goal (R2)
//----------------------------------------------------------------------
if(Gamepad.IsDown(Gamepad.kButtonR2) && !subsystemControlStates.DriveTrainAutomated) //Move towards goal (hold)
{
m_turnToGoalThreadEnabled = true;
subsystemControlStates.DriveTrainAutomated = true;
}else{
m_turnToGoalThreadEnabled = false;
}
//----------------------------------------------------------------------
//
// Calculus Bazooka
//
//----------------------------------------------------------------------
return subsystemControlStates;
}
Code:
public void teleopPeriodic()
{
Gamepad.UpdateState();
Gamepad2.UpdateState();
String result = "";
for(int i = 1; i < 13; i++)
{
result += Gamepad.IsDown(i) ? "1": "0";
}
//Logger.LogLine(result + " " + Gamepad.GetLeftY() + " " + Gamepad.GetRightY() + " TowerShooting: " + Tower.IsShooting() + " VBattery: " + m_ds.getBatteryVoltage());
//----------------------------------------------------------------------
//
// Run autonomous code if their triggers are pressed.
//
//----------------------------------------------------------------------
SubsystemControlState subsystemControlStates = SelectivelyRunAutonomousLoops();
//----------------------------------------------------------------------
//
//
// Run methods that do not conflict with automation of their subsystems.
//
//
//----------------------------------------------------------------------
//
// Drive Train
//
//----------------------------------------------------------------------
// Tank Drive when we aren't automated (Analog Sticks & Buttons L3, R3)
//----------------------------------------------------------------------
if(!subsystemControlStates.DriveTrainAutomated)
{
double leftSpeed = Gamepad.GetLeftY();
double rightSpeed = Gamepad.GetRightY();
//If an analog input is pressed, it counts as a button on the dualshock.
//While this isn't so useful for most methods (as it requires sort of
//moving the analog stick a bit, it's useful for special driving stuff.
//In our case, if the buttons are pressed, we square inputs.
//This allows for more precise control by the driver, especially when
//you think of things such as deadzone.
if(Gamepad.IsDown(Gamepad.kButtonL3))
leftSpeed = leftSpeed * leftSpeed;
if(Gamepad.IsDown(Gamepad.kButtonR3))
rightSpeed = rightSpeed * rightSpeed;
//We now invoke tank drive with the joystick's potentially squared inputs.
this.GetKinematixDriveTrain().TankDrive(leftSpeed, rightSpeed);
//We now enable PID if R1 is pressed. This is for going up the ramp.
//PID compensates for the higher amount of resistence to the wheels,
//and ensures that the robot doesn't jerk forwards when the ramp
//levels itself (which would lower the resistance to the wheels)
if(Gamepad.IsDown(Gamepad.kButtonR1))
{
//Logger.LogLine("PID Trigger");
this.GetKinematixDriveTrain().SetControlMode(ControlMode.kProportionalGain);
}
else
{
//Logger.LogLine("Direct Control Trigger");
this.GetKinematixDriveTrain().SetControlMode(ControlMode.kDirectControl);
}
}
//----------------------------------------------------------------------
//
// Calculus Bazooka
//
//----------------------------------------------------------------------
// Shoot if Button 1 is pressed, based on firing mode
//----------------------------------------------------------------------
if(!subsystemControlStates.CalculusBazookaAutomated)
{
// if(CalculusBazooka.GetFiringMode().equals(FiringMode.kAutomatic))
// {
// if(Gamepad.IsDown(Gamepad.kButton1) && !Gamepad.IsDown(Gamepad.kButton2))
// {
// //CalculusBazooka.
// Tower.SetControlMode(ControlMode.kAutonomousControl);
// Tower.BeginShooting();
// }
// }
// else if(CalculusBazooka.GetFiringMode().equals(FiringMode.kSingle))
// {
// if(Gamepad.IsJustDown(Gamepad.kButton1) && !Gamepad.IsDown(Gamepad.kButton2))
// {
// Tower.SetControlMode(ControlMode.kAutonomousControl);
// Tower.BeginShooting();
// }
// }
//Disable letting the primary driver control elevator. Too kludgy
// if(Gamepad.IsDown(Gamepad.kButton1) && Gamepad.IsDown(Gamepad.kButton2))
// {
// Tower.SetControlMode(ControlMode.kAutonomousControl);
// Tower.DirectReverseElevatorAndCollector();
// }else if(Gamepad.IsDown(Gamepad.kButton1))
// {
// Tower.SetControlMode(ControlMode.kAutonomousControl);
// Tower.BeginShooting();
// }else if(Gamepad.IsDown(Gamepad.kButton2))
// {
// Tower.SetControlMode(ControlMode.kAutonomousControl);
// Tower.ShootingIsDone();
// }
//4 3 5
if(Gamepad2.IsDown(Gamepad.kButton4))
{
Tower.SetControlMode(ControlMode.kAutonomousControl);
Tower.DirectStartElevatorAndCollector();
}else if(Gamepad2.IsDown(Gamepad.kButton3))
{
Tower.SetControlMode(ControlMode.kAutonomousControl);
Tower.ShootingIsDone();
}else if(Gamepad2.IsDown(Gamepad.kButton5))
{
Tower.SetControlMode(ControlMode.kAutonomousControl);
Tower.DirectReverseElevatorAndCollector();
}
}
LEDRings.SetWhiteLedRingEnabled(m_ds.getDigitalIn(7));
LEDRings.SetGreenLedRingEnabled(m_ds.getDigitalIn(8));
//If Button 9 is pressed, we swap the firing mode of the Calculus Bazooka.
if(Gamepad.IsJustDown(Gamepad.kButton9))
{
if(CalculusBazooka.GetFiringMode().equals(FiringMode.kAutomatic))
CalculusBazooka.SetFiringMode(FiringMode.kSingle);
else if(CalculusBazooka.GetFiringMode().equals(FiringMode.kSingle))
CalculusBazooka.SetFiringMode(FiringMode.kAutomatic);
}
//If Button 10 is pressed, we swap the subsystem mode of the Calculus Bazooka.
//if(Gamepad.IsJustDown(Gamepad.kButton10))
if(Gamepad2.IsJustDown(Gamepad.kButton1))
{
if(m_manipulatorSystemState.equals(ManipulatorSystemState.kCollecting))
{
m_manipulatorSystemState = ManipulatorSystemState.kShooting;
}else if(m_manipulatorSystemState.equals(ManipulatorSystemState.kShooting))
{
m_manipulatorSystemState = ManipulatorSystemState.kCollecting;
}
}
//----------------------------------------------------------------------
//
// Del Operator
//
//----------------------------------------------------------------------
if(!subsystemControlStates.DelOperatorAutomated)
{
// if(Gamepad.IsDown(Gamepad.kButtonL1) && !subsystemControlStates.DelOperatorAutomated)
// {
// //Tells the del operator to extend if it's in a retracted position
// DelOperator.ExtendIfReady();
// }
if(Gamepad2.IsDown(Gamepad.kButton6))// && !Gamepad.IsDown(Gamepad.kButton4))
{
DelOperator.DirectExtend();
}
else if(Gamepad2.IsDown(Gamepad.kButton7))// && Gamepad.IsDown(Gamepad.kButton4))
{
DelOperator.DirectRetract();
}
else
{
DelOperator.DirectStop();
}
}
}