Chief Delphi - Robot Base Code

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();

            }

        }

    }