FIRST Robotics Code 2011

 

 

 

 

 

 

class RobotDemo : public SimpleRobot

        RobotDrive *myRobot; // robot drive system

        Victor *leftDrive;

        Victor *rightDrive;

        Jaguar *armJags;

        Jaguar *rightArmJag;

        Jaguar *topClawJag;

        Jaguar *bottomClawJag;

        Servo *leftDeployServo;

        Servo *rightDeployServo;

       

        //GetRawAxis returns from -1 to 1, -1 is top

        Joystick *xboxStickManip;

        Joystick *leftStick;

        Joystick *rightStick;

       

        Compressor *compressor1;

       

        Solenoid *shiftHigh;

        Solenoid *shiftLow;

        Solenoid *minibotDeployer;

        Solenoid *armExtend;

        Solenoid *armRetract;

        Solenoid *minibotHold;

       

        DigitalInput *lightLeft; //MAKE SURE THE LIGHT SENSOR RIBBON IS WIRED CORRECTLY

        DigitalInput *lightMid; //PORTS 1,2,3 MUST BE OCCUPIED

        DigitalInput *lightRight;

        DigitalInput *autoSwitch;

       

        DigitalOutput *redLEDs;

        DigitalOutput *blueLEDs;

       

        //DigitalInput *minibotDeploymentBarSwitch; // Activates when the switch is depressed, deployment bar is touching pole

        DigitalInput *highLimitSwitch;

        DigitalInput *lowLimitSwitch;

       

        Gyro *gyro1;

       

        //Ultrasonic *ultrasonic1;

       

        Encoder *rightEncoder;

       

        Timer *robotTimer;

       

        AnalogChannel *pot1;

       

        //presets for arm angle, calibrated by pot values

        #define ARM_ANGLE_MIN (1000-560)

        #define ARM_ANGLE_MAX (1000-430)

 

        #define ARM_ANGLE_FLOOR (1000-550)

        #define ARM_ANGLE_HOME_LOW (1000-560)

        #define ARM_ANGLE_FEED (1000-500)

        #define ARM_ANGLE_MID (1000-470)

        #define ARM_ANGLE_MID_DROP (1000-502)

        #define AUTO_RELEASE_THRESHOLD (1000-450)//the value where the auto release chooses between high release and mid release

        #define ARM_ANGLE_HIGH (1000-428)

        #define ARM_ANGLE_HIGH_DROP (1000-460)//drops the arm while spinning durning auto

 

       

        bool firstCycle;

        int previousError;

        int LEDState;

       

        RobotDemo(void)

        {

                /* DIGITAL IO PORTS

               

                leftDrive = new Victor(1);

                rightDrive = new Victor(2);

                myRobot = new RobotDrive(leftDrive, rightDrive);

                armJags = new Jaguar(3);

                topClawJag = new Jaguar(5);

                bottomClawJag = new Jaguar(6);

                leftDeployServo = new Servo(7);

                rightDeployServo = new Servo(8);

                leftDeployServo->Set(0.3); //to reset, must restart robot. Disable won't reset.

                rightDeployServo->Set(0.7);

               

                AxisCamera::GetInstance();

               

                xboxStickManip = new Joystick(1);

                leftStick = new Joystick(2);

                rightStick = new Joystick(3);

               

                compressor1 = new Compressor(12,1);

                compressor1->Start();

               

                shiftHigh = new Solenoid(1);

                shiftLow = new Solenoid(2);

                minibotDeployer = new Solenoid(3);

                armExtend = new Solenoid (4);

                armRetract = new Solenoid (5);

                minibotHold = new Solenoid (6);

                minibotHold->Set(true);

                minibotDeployer->Set(false);

               

                lightLeft = new DigitalInput(1);

                lightMid = new DigitalInput(2);

                lightRight = new DigitalInput(3);

               

                redLEDs = new DigitalOutput(6);

                blueLEDs = new DigitalOutput(7);

               

                //minibotDeploymentBarSwitch = new DigitalInput(10);

                highLimitSwitch = new DigitalInput(11);

                autoSwitch = new DigitalInput(13);//0 is to the side, 1 is towards robot

                lowLimitSwitch = new DigitalInput(14);

               

                gyro1 = new Gyro(1);

               

                //ultrasonic1 = new Ultrasonic(4, 5, Ultrasonic::kInches); //Digital ping out on 4, echo in on 5

                //ultrasonic1->SetAutomaticMode(true);

 

                rightEncoder = new Encoder(9, 8, true, Encoder::k4X);

                rightEncoder->Start();

               

                                                                                                                //set how many inches traveled per tick

                rightEncoder->SetDistancePerPulse(0.0349);              //this number should be the circumference of the

                                                                                                                //wheel divided by 360

               

                robotTimer = new Timer();

               

                SmartDashboard::init();

               

                pot1 = new AnalogChannel(2);

               

                firstCycle = true;

                previousError = 0; //do not apply d during first cycle through position arm

                LEDState = 2;

               

                GetWatchdog().SetExpiration(0.1);

        }

 

        ~RobotDemo(void)

        {

                delete myRobot;

                delete armJags;

                delete topClawJag;

                delete bottomClawJag;

                delete leftDeployServo;

                delete rightDeployServo;

               

                delete xboxStickManip;

                delete leftStick;

                delete rightStick;

               

                delete compressor1;

               

                delete shiftHigh;

                delete shiftLow;

                delete minibotDeployer;

                delete armExtend;

                delete armRetract;

                delete minibotHold;

               

                delete autoSwitch;

               

                delete gyro1;

               

                //delete ultrasonic1;

               

                delete rightEncoder;

               

                delete robotTimer;

               

                delete pot1;

        }

 

        void Autonomous(void)

        {

                GetWatchdog().SetEnabled(false);

               

                double timeElapsed = 0.0;

               

                robotTimer->Reset();

                robotTimer->Start();

               

                rightEncoder->Reset();

               

                gyro1->Reset();

               

 

                ExtendArm(false);

               

                if (autoSwitch->Get()==1)//set to 1, gyro drive

                {

                        while(IsAutonomous())

                        {

                                timeElapsed = robotTimer->Get();

 

                                Shift(0);

                               

                                if(((int)(timeElapsed))%2==0)

                                        SetLEDs(2);

                                else

                                        SetLEDs(1);

                                                               

                                if(timeElapsed < 8.0)

                                {      

                                        PositionArm(ARM_ANGLE_HIGH);

                                        ExtendArm(true);

                                        GyroDrive(0.0, 0.5, 214);

                                }

                                else if (timeElapsed > 8.0 && timeElapsed <8.5)

                                {

                                        PositionArm(ARM_ANGLE_HIGH);

                                        myRobot->Drive(0.0, 0.0);

                                }

                               

                                if(timeElapsed > 8.0 && timeElapsed < 11.0)

                                {

                                        PositionArm(ARM_ANGLE_HIGH_DROP);

                                        SpinClaw(-1, 0.7);//tilt tube forward

                                }

                                else if(timeElapsed > 11.0 && timeElapsed < 11.5)

                                {      

                                        PositionArm(ARM_ANGLE_HIGH_DROP);

                                        SpinClaw(0.0, 0.0);

                                }

                               

                                if(timeElapsed > 9.0)

                                        ExtendArm(false);

                               

                                if(timeElapsed > 11.0 && timeElapsed < 13.0)

                                {

                                        PositionArm(ARM_ANGLE_MID);

                                        myRobot->Drive(-0.5, 0);

                                }

                                else if (timeElapsed > 12.0)

                                {

                                        PositionArm(ARM_ANGLE_MID);

                                        myRobot->Drive(0.0, 0.0);

                                }

 

                        }

                }      

                else

                {

                        while(IsAutonomous())

                        {

                                timeElapsed = robotTimer->Get();

 

                                Shift(0);

                                                       

                                if(((int)(timeElapsed))%2==0)

                                        SetLEDs(2);

                                else

                                        SetLEDs(1);

                                                                                       

                                if(timeElapsed < 8.0)

                                {      

                                        PositionArm(ARM_ANGLE_HIGH);

                                        ExtendArm(true);

                                        LineFollow(false, 0.5, 214);

                                }

                                else if (timeElapsed > 8.0 && timeElapsed <8.5)

                                {

                                        PositionArm(ARM_ANGLE_HIGH);

                                        myRobot->Drive(0.0, 0.0);

                                }

                                                       

                                if(timeElapsed > 8.0 && timeElapsed < 11.0)

                                {

                                        PositionArm(ARM_ANGLE_HIGH_DROP);

                                        SpinClaw(-1, 0.7);//tilt tube forward

                                }

                                else if(timeElapsed > 11.0 && timeElapsed < 11.5)

                                {      

                                        PositionArm(ARM_ANGLE_HIGH_DROP);

                                        SpinClaw(0.0, 0.0);

                                }

                                                       

                                if(timeElapsed > 9.0)

                                        ExtendArm(false);

                                       

                                if(timeElapsed > 11.0 && timeElapsed < 13.0)

                                {

                                        PositionArm(ARM_ANGLE_MID);

                                        myRobot->Drive(-0.5, 0);

                                }

                                else if (timeElapsed > 12.0)

                                {

                                        PositionArm(ARM_ANGLE_MID);

                                        myRobot->Drive(0.0, 0.0);

                                }

 

                        }      

                }

        }

                /* Dual tube mode

 

 

               

        void GyroDrive(float desiredDriveAngle, float speed, int desiredDistance)//todo:fix for teleop

        {

                double encoderInchesTraveled = fabs(rightEncoder->GetDistance());//absolute value distance

                float myAngle = gyro1->GetAngle();

                //normalizes angle from gyro to [-180,180) with zero as straight ahead

                while(myAngle >= 180)

                {

                        GetWatchdog().Feed();

                        myAngle = myAngle - 360;

                }

                while(myAngle < -180)

                {

                        GetWatchdog().Feed();

                        myAngle = myAngle + 360;

                }

               

                float my_speed = 0.0;

                float turn = 0.0;

               

                if(speed > 0)

                        turn = -((myAngle + desiredDriveAngle) / 30.0); //proportionally adjust turn. As the robot gets more off 0, the greater the turn will be

                else

                        turn = (myAngle + desiredDriveAngle) / 30.0; //proportionally adjust turn. As the robot gets more off 0, the greater the turn will be

                       

                if (encoderInchesTraveled < desiredDistance)

                        my_speed = speed;

                else

                        my_speed = 0.0;

               

                myRobot->Drive(my_speed, turn);

        }

 

        void LineFollow(bool lookForLine, float speed, int desiredDistance)

        {

                //flips 0 to 1, so 0 means the sensor does not detect a line

                int lightLeftValue = (int)(!((bool)lightLeft->Get()));

                int lightMidValue = (int)(!((bool)lightMid->Get()));

                int lightRightValue = (int)(!((bool)lightRight->Get()));

               

                //double ultrasonicInchesFromWall = ultrasonic1->GetRangeInches();

                double encoderInchesTraveled = rightEncoder->GetDistance();

 

                float my_speed = 0.0;

                float turn = 0.0;

               

                if (encoderInchesTraveled<desiredDistance)

                        my_speed = speed;

                else

                        my_speed = 0.0;

               

                if((lightLeftValue == 1) && (lightMidValue == 1) && (lightRightValue == 1)) // sensors detect everything, maybe at end?

                {

                        my_speed = (0.0);

                        turn = (0.0);

                }

                else if((lightLeftValue == 0) && (lightMidValue == 0) && (lightRightValue == 0)) // sensors detect nothing, maybe passed end?

                {

                        if(!lookForLine)//if not looking for line, set speed to 0 if sensors detect nothing

                        {

                                my_speed = (0.0);

                                turn = (0.0);

                        }

                }

                else if ((lightLeftValue == 0) && (lightMidValue == 1) && (lightRightValue == 0)) // ideal condition

                        turn = 0.0;

                else if ((lightLeftValue == 1) && (lightMidValue == 0) && (lightRightValue == 0))// far left, so turn right

                        turn = -0.8;

                else if ((lightLeftValue == 0) && (lightMidValue == 0) && (lightRightValue == 1))// far right, so turn left

                        turn = 0.8;

                else if ((lightLeftValue == 1) && (lightMidValue == 1) && (lightRightValue == 0))// near left, so turn right

                        turn = -0.8;

                else if ((lightLeftValue == 0) && (lightMidValue == 1) && (lightRightValue == 1))// near right, so turn left

                        turn = 0.8;

                else if((lightLeftValue == 1) && (lightMidValue == 0) && (lightRightValue == 1)) // should happen at branch, but unreliable

                        turn = -0.8; // in a perfect world, turns right at branch

                else turn = 0.0; //should never reach this

               

                       

                myRobot->Drive(my_speed, turn);

                               

               

        }

               

        void Shift(bool gear)

        {

                if (gear)

                {

                        shiftHigh->Set(true);

                        shiftLow->Set(false);

                }

                else

                {

                        shiftHigh->Set(false);

                        shiftLow->Set(true);

                }

        }

       

        void PositionArm(INT16 desiredArmAngle)

        {

               

                double proportionalControl = 0.0;

                double derivativeControl = 0.0;

                double combinedControl = 0.0;

               

                int currentAngle = 1000-(pot1->GetValue());

                int currentError = desiredArmAngle - currentAngle;

                if(firstCycle)

                {

                        previousError = currentError;

                        firstCycle = false;

                }

                int deltaError = currentError - previousError;

                previousError = currentError;

               

                proportionalControl = currentError * 0.02;

                derivativeControl = deltaError * 0.01; //todo: fix this.

                combinedControl = proportionalControl + derivativeControl;

               

                MoveArm(combinedControl);

               

        }

        void MoveArm(float speed) //negative speed is lift up arm

        {

                /*

                float my_speed = speed;

               

                if(speed > 0.7)

                        my_speed = 0.7;

                if(speed < -0.7)

                        my_speed = -0.7;

               

                //Prevent the arm from overshooting, mainly for manual arm control, which does not slow down on its own

                //if((1000-(pot1->GetValue()) > ARM_ANGLE_MID+20) && (speed > 0.3))

                        //my_speed = 0.3;

               

                if(highLimitSwitch->Get() && (speed > 0))

                        my_speed = 0;

                if(lowLimitSwitch->Get() && (speed < 0))

                        my_speed = 0;

               

                armJags->Set(my_speed);

               

        }

        void ExtendArm(bool extend)

        {

                if (extend)

                {

                        armExtend->Set(true);

                        armRetract->Set(false);

                }

                else

                {

                        armExtend->Set(false);

                        armRetract->Set(true);

                }

        }

        void SpinClaw(float topSpeed, float bottomSpeed)//squared for more sensitive low end

        {

                if (topSpeed > 0)

                        topClawJag->Set(topSpeed*topSpeed);

                else

                        topClawJag->Set(-topSpeed*topSpeed);

                if (bottomSpeed > 0)

                        bottomClawJag->Set(bottomSpeed*bottomSpeed);

                else

                        bottomClawJag->Set(-bottomSpeed*bottomSpeed);

        }

        //robot turns to desired position with a deadband of 2 degrees in each direction

        void GyroTurn (float desiredTurnAngle, float turnSpeed)

        {

                float myAngle = gyro1->GetAngle();

                //normalizes angle from gyro to [-180,180) with zero as straight ahead

                while(myAngle >= 180)

                {

                        GetWatchdog().Feed();

                        myAngle = myAngle - 360;

                }

                while(myAngle < -180)

                {

                        GetWatchdog().Feed();

                        myAngle = myAngle + 360;

                }

                if(myAngle < (desiredTurnAngle - 2))// if robot is too far left, turn right a bit

                {

                        myRobot->TankDrive(turnSpeed, -turnSpeed); //(right,left)

                }

                if(myAngle > (desiredTurnAngle + 2))// if robot is too far right, turn left a bit

                {

                        myRobot->TankDrive(-turnSpeed, turnSpeed); //(right,left)

                }

        }

 

        void SetLEDs(int state)//0 is off, 1 is red, 2 is blue, 3 is both on.

        {//hmm red and blue may be reversed

                if(state==0)

                {

                        redLEDs->Set(0);

                        blueLEDs->Set(0);

                }

                else if(state==1)

                {

                        redLEDs->Set(1);

                        blueLEDs->Set(0);

                }

                else if(state==2)

                {

                        redLEDs->Set(0);

                        blueLEDs->Set(1);

                }

                else

                {

                        redLEDs->Set(1);

                        blueLEDs->Set(1);

                }

        }

 

       

        void OperatorControl(void)

        {

               

                robotTimer->Stop();

                rightEncoder->Reset();

                myRobot->TankDrive(0.0, 0.0);

               

                bool minibotStage1Complete = false;// stage 1 (drop) has to be done before stage 2 (push)

               

                Shift(0);

                ExtendArm(false);

                MoveArm(0.0);

               

                bool previousLEDToggle = false;

                bool currentLEDToggle = false;

               

                bool previousShiftToggle = false;

                bool currentShiftToggle = false;

               

                bool previousArmExtendToggle = false;

                bool currentArmExtendToggle = false;

 

               

                LEDState = 1;

                bool ShiftState = false;

                bool ArmExtendState = false;

               

                GetWatchdog().SetEnabled(true);

               

                while (IsOperatorControl())

                {

                        GetWatchdog().Feed();

                       

                        float leftStickYAxis = leftStick->GetY();                                                       //Tank Drive

                        float rightStickYAxis = rightStick->GetY();                                                     //Tank Drive

                        bool rightStickButton1 = rightStick->GetRawButton(1);                           //Shifts Robot

                        bool rightStickButton10 = rightStick->GetRawButton(10);                         //Button 10+11 = Activate deployment servos

                        bool rightStickButton11 = rightStick->GetRawButton(11);

                        bool leftStickButton9 = leftStick->GetRawButton(9);                                     //Backup Deploy Minibot

                        bool rightStickButton8 = rightStick->GetRawButton(8);

                       

                        float manipRightYAxis = xboxStickManip->GetRawAxis(5);                          //Manual Arm Up/Down

                        float manipTriggerAxis = xboxStickManip->GetRawAxis(3);                         //Rotate Tube Up/Down

                        float manipLeftYAxis = xboxStickManip->GetRawAxis(2);                           //Cycle LEDs

                        bool manipLeftBumper = xboxStickManip->GetRawButton(5);                         //Acquire Tube

                        bool manipRightBumper = xboxStickManip->GetRawButton(6);                        //Spit Out Tube

                        bool manipYButton = xboxStickManip->GetRawButton(4);                            //High Preset

                        bool manipBButton = xboxStickManip->GetRawButton(2);                            //Mid Preset

                        bool manipAButton = xboxStickManip->GetRawButton(1);                            //Floor Preset

                        bool manipXButton = xboxStickManip->GetRawButton(3);                            //Home/Low Preset

                        bool manipLeftClick = xboxStickManip->GetRawButton(9);                          //Feed preset = L click

                        bool manipStartButton = xboxStickManip->GetRawButton(8);                        //Start + Select deploys minibot

                        bool manipSelectButton = xboxStickManip->GetRawButton(7);

                        bool manipRightClick = xboxStickManip->GetRawButton(10);                        //Manual Arm Extend

 

                       

                        //SmartDashboard::Log(ultrasonic1->GetRangeInches(), "Ultrasonic1 inches");

                       

                        //SmartDashboard::Log(rightEncoder->GetDistance(), "Encoder Inches Traveled");

                       

                        //SmartDashboard::Log(pot1->GetValue(), "pot1 Value");//must not be neg, read true value to calibrate

                       

                        //SmartDashboard::Log(gyro1->GetAngle(), "gyro1 Angle");

                       

                        //int lightLeftValue = (int)(!((bool)lightLeft->Get()));

                        //int lightMidValue = (int)(!((bool)lightMid->Get()));

                        //int lightRightValue = (int)(!((bool)lightRight->Get()));

                        //SmartDashboard::Log(lightLeftValue, "Left Light");

                        //SmartDashboard::Log(lightMidValue, "Middle Light");

                        //SmartDashboard::Log(lightRightValue, "Right Light");

                       

                        //SmartDashboard::Log(leftDeployServo->Get(), "Left Servo Position");

                        //SmartDashboard::Log(rightDeployServo->Get(), "Right Servo Position");

 

                        //int autoVal = autoSwitch->Get();

                        //SmartDashboard::Log(autoVal, "Autoswitch");

                       

                        /****************DRIVE CODE STARTS HERE***************/

                        myRobot->TankDrive(-leftStickYAxis, -rightStickYAxis);

                       

                       

                        //MINIBOT DEPLOY STAGE 1

                        if (rightStickButton10 && rightStickButton11)

                        {

                               

                                leftDeployServo->Set(0.7); //to reset, must restart robot. Disable won't reset.

                                rightDeployServo->Set(0.3);

                                minibotStage1Complete = true;

                        }

                       

                        //MINIBOT DEPLOY STAGE 2 BACKUP

                        if(leftStickButton9 && rightStickButton8 && minibotStage1Complete)

                        {

                                minibotHold->Set(false);

                                minibotDeployer->Set(true);

                        }

                       

                        Shift(ShiftState); //shifts back to previous gear when line follow/gyro routine ends

 

                        //Shift code, keeps track of shift button state and actual gear state

                        if(rightStickButton1)

                                currentShiftToggle = true;

                        else

                                currentShiftToggle = false;

                       

                        if(currentShiftToggle && (previousShiftToggle != currentShiftToggle))

                                ShiftState=!ShiftState;

                       

                        previousShiftToggle = currentShiftToggle;

                        Shift(ShiftState);

                       

                        /****************MANIPULATOR CODE STARTS HERE***************/

                        //CYCLE LEDS

                        if(manipLeftYAxis < -0.6 || manipLeftYAxis > 0.6)

                                currentLEDToggle = true;

                        else

                                currentLEDToggle = false;

                        //LEDState will only advance if thumbstick moves from 0 to an extreme, not from an extreme to 0,

                        //nor when the thumbstick is held. This prevents rapid LED cycling.

                        if(currentLEDToggle && (previousLEDToggle != currentLEDToggle))

                        {

                                if (LEDState > 3)

                                        LEDState = 0;

                                else

                                        LEDState++;

                        }

                        previousLEDToggle = currentLEDToggle;

                       

                        //if(minibotDeploymentBarSwitch->Get()==1)

                                //SetLEDs(2);

                        //else

                                SetLEDs(LEDState);

                               

                       

                        // ARM ANGLE PRESETS

                        if(manipXButton)

                        {

                                if(1000-(pot1->GetValue()) > AUTO_RELEASE_THRESHOLD) //high release

                                {

                                        ArmExtendState = false;

                                        PositionArm(ARM_ANGLE_HIGH_DROP);

                                        SpinClaw(-1.0, 0.7);

                                }

                                else //mid release

                                {

                                        ArmExtendState = false;

                                        PositionArm(ARM_ANGLE_MID_DROP);

                                        SpinClaw(-1.0, 0.7);   

                                }

                        }

                        else if(manipAButton)

                        {

                                ArmExtendState = true;

                                PositionArm(ARM_ANGLE_FLOOR);

                        }

                        else if(manipYButton)

                        {

                                ArmExtendState = true;

                                PositionArm(ARM_ANGLE_HIGH);

                        }

                        else if(manipBButton)

                        {

                                ArmExtendState = false;

                                PositionArm(ARM_ANGLE_MID);

                        }

                        else if(manipLeftClick)

                        {

                                ArmExtendState = false;

                                PositionArm(ARM_ANGLE_FEED);

                        }

                        else if(manipRightYAxis>=0.1 || manipRightYAxis<=-0.1)

                                MoveArm(-manipRightYAxis); //manual arm control

                        else

                                MoveArm(0.0);

                       

                        ExtendArm(ArmExtendState);

                       

                        //Arm extend code, keeps track of extend button state and actual extend state

                        if(manipRightClick)

                                currentArmExtendToggle = true;

                        else

                                currentArmExtendToggle = false;

                       

                        if(currentArmExtendToggle && (previousArmExtendToggle != currentArmExtendToggle))

                                ArmExtendState=!ArmExtendState;

                       

                        previousArmExtendToggle = currentArmExtendToggle;

                        ExtendArm(ArmExtendState);

                       

                        //MINIBOT DEPLOY STAGE 2

                        if(manipStartButton && manipSelectButton && minibotStage1Complete)

                        {

                                minibotHold->Set(false);

                                minibotDeployer->Set(true);

                        }

                       

                        // MANUAL CLAW BEGINS HERE

                        if (manipXButton)//if the release preset is activated, skip the manual claw section

                                ;//do nothing

                        else if(manipLeftBumper) //Left Bumper aquires tube

                                SpinClaw(0.85, -0.85);

                        else if(manipRightBumper)

                                SpinClaw(-0.85, 0.85);

                        else SpinClaw(-manipTriggerAxis, -manipTriggerAxis); // Left Trigger rotates tube up, Right rotates down

                       

                       

                }

 

        }

 

 

START_ROBOT_CLASS(RobotDemo);