Code not Properly Downloading

[SM987]

Let's see, I'll try and post the code here below. Haven't tried the printf test but have entered debug mode with the robot on blocks and I believe it appeared to follow the code appropriately, but the robot did not respond accordingly. Pretty sure we don't have any uninitialized variables, but I just took this on today. I'll be taking a closer look at the problem today after school and post the results here. Probably will start with using a different computer/windriver reinstall. Thanks for the help.

BTW, this year the students and mentors have been collaborating with code via bitbucket.org. We highly recommend it for tracking changes/reaching mentors at work etc.

Here's the code:

#include "WPILib.h"

#include "Target.h"
#include "DashboardDataSender.h"
#include "Dashboard.h"
#include "Math.h"



const float ARM_MINIMUM_ROTATION = 7.0f; //7.0f;        // don't ever drive the arm lower than this!
const float ARM_MAXIMUM_ROTATION = 650.0f;
const int ARM_PICKUP_PRESET = 135;
const int ARM_HIGH_GOAL_PRESET = 430;
const float ARM_PROPORTIONAL_CONSTANT = 0.0075f;


const int AUTO_ROTATION_START = 345;
const int AUTO_ROTATION_START_FORK = 330;
const int AUTO_ROTATION_SCORE_HIGH = 350;
const int AUTO_ROTATION_LOWER = 50;

const int AUTO_FULL_SPEED_MOVE_ENCODER_TICKS = 2000;  //2500 is total ticks start to end
const float AUTO_CLAW_REJECT_SECONDS = 1.5f;
const float AUTO_BACKUP_TIME = 5.0f;
const float AUTO_BACKUP_SECONDS = 4.0f;
const int AUTO_BACKUP_TICKS = 2200;
const int AUTO_TURN_TICKS = 520;

const float CLAW_CAPTURE_SPEED = 0.6f;
const float CLAW_REJECT_SPEED = 0.3f;
const float CLAW_ROTATION_SPEED = 0.3f; 

const float AUTO_CLAW_REJECT_SPEED = .2f;
UINT32 Auto_End_Line;
UINT32 Auto_End_Line_Fork;

const float MINIBOT_AUTO_DEPLOY_VOLTAGE = 4.5f;
const float MINIBOT_AUTO_DEPLOY_SECONDS = 110.0f;

//#define MINIMUM_SCORE 0.01

double outputValue;
const UINT32 DETECTSLINE = 1;

const UINT32 LEFT_SENSOR_CHANNEL = 10;
const UINT32 CENTER_SENSOR_CHANNEL = 11;
const UINT32 RIGHT_SENSOR_CHANNEL = 12;

const float AUTO_SPEED_LOW = 0.8f;
const float AUTO_SPEED_FULL = 1.0f;

UINT32 LeftSensorReading;
int XL;



class SamplePIDOutput : public PIDOutput {
public:
        SamplePIDOutput(RobotDrive *base) {
                m_base = base;
        }

        void PIDWrite(float output) {
                m_base->ArcadeDrive(0.0, output);
                outputValue = -output;
        }
private:
        RobotDrive *m_base;
};


/**
 * Demo that demonstrates tracking with the robot.
 * In Teleop mode the robot will drive using a single joystick plugged into port 1.
 * When the trigger is pressed, the robot will turn to face the circular target.
 * This demo uses the AxisCamera class to grab images. It finds the target and computes
 * the angle to the target from the center of view in the camera image. It then computes
 * a goal correction angle and uses the gyro to turn to that heading. The gyro correction
 * is done using the PID class to drive the robot to the desired heading.
 * The coefficients for the PID loop will need to be tuned to match the CG and weight of
 * your robot as well as the wheel and driving surface types.
 * 
 * As an added bonus, the program also sends robot status information to the LabVIEW sample
 * dashboard as well as camera annotation to show the targets that the robot is currently
 * seeing.
 */
class RobotDemo : public SimpleRobot {

//      RobotDrive *base;
//      Joystick *stick; // only joystick

        DigitalInput *armUpperLimit;    // arm upper limit switch
        DigitalInput *armLowerLimit;    // arm lower limit switch
        AnalogChannel *armUpperLimit_ANALOG;
        AnalogChannel *armLowerLimit_ANALOG;
        
        Joystick *rightStick;                   // joystick 1 (arcade stick or right tank stick)
        Joystick *leftStick;                    // joystick 2 (tank left stick)
        Joystick *armStick;                             // joystick 3 to control arm
        DriverStation *ds;                              // driver station object
        
        Jaguar *LeftDrive;
        Jaguar *RightDrive;
        Jaguar *ArmRotation;
        Jaguar *ArmExtension;
        Jaguar *ClawTop;
        Jaguar *ClawBottom;

        Encoder *LeftDriveEnc;
        Encoder *RightDriveEnc;
        Encoder *ArmRotationEnc;
        Encoder *ArmExtensionEnc;
        
        Compressor *compressor;
        Solenoid *ShifterHigh;
        Solenoid *ShifterLow;
        Solenoid *EndGameIn;
        Solenoid *EndGameOut;
        Solenoid *DeploymentOut;
        Solenoid *DeploymentIn;
        
        DashboardDataSender *dds;
        PIDOutput *pidOutput;
        Gyro *gyro;
        
        DigitalInput *LeftLineSensor;
        DigitalInput *CenterLineSensor;
        DigitalInput *RightLineSensor;
        
        DigitalInput *ArmSensor;
        float LastExtensionCommand;
        
        AnalogChannel *Minibot_Deploy_Light_Sensor;
        //Timer Minibot_Deploy_Timer;   
        bool Minibot_Deploy_Enabled;
        
        // Arm Position (angle) control
        float CurrentArmPosition;
        float TargetArmPosition;
        float ArmPosition_error;
        float ArmPositionCommand;
        float ArmPositionConstant;
        float CurrentArmStick;
        float PreviousArmStick;
        float ArmPosMaxCmd;     
        
        int Previous_Sensor_Detect;
        int AutoSeq;
        Timer AutoStateTimer;
        int AutoState;
        
        int AutoLeftEncoderStart;
        int AutoRightEncoderStart;
        
        int left_drive_count;
        int right_drive_count;
        
        enum                                                    // Driver Station jumpers to control program operation
        { ARCADE_MODE = 1,                              // Tank/Arcade jumper is on DS Input 1 (Jumper present is arcade)
          ENABLE_AUTONOMOUS = 2,                // Autonomous/Teleop jumper is on DS Input 2 (Jumper present is autonomous)
        } jumpers;                                  
        

public:
        RobotDemo(void) 
        {
/*              // initialize all variables
                base = new RobotDrive(1, 2);
                base->SetInvertedMotor(RobotDrive::kRearLeftMotor, true);
                base->SetInvertedMotor(RobotDrive::kRearRightMotor, true);
                stick = new Joystick(1);
                pidOutput = new SamplePIDOutput(base);
                base->SetExpiration(0.5);
*/
                gyro = new Gyro(1);
                dds = new DashboardDataSender();
        
                
                ds = DriverStation::GetInstance();
                rightStick = new Joystick(1);                   // create the joysticks
                leftStick = new Joystick(2);
                armStick = new Joystick(3);
                armUpperLimit = new DigitalInput(8);    // create the limit switch inputs
                armLowerLimit = new DigitalInput(9);
                
                armUpperLimit_ANALOG = new AnalogChannel(1);
                armLowerLimit_ANALOG = new AnalogChannel(2);

                Minibot_Deploy_Light_Sensor = new AnalogChannel(3);
                
                
                LeftDrive = new Jaguar(1);
                RightDrive = new Jaguar(2);
                ArmRotation = new Jaguar(3);
                ArmExtension = new Jaguar(4);
                ClawTop = new Jaguar(5);
                ClawBottom = new Jaguar(6);
                
                LeftDriveEnc = new Encoder(2,3);
                RightDriveEnc = new Encoder(4,5);
                ArmRotationEnc = new Encoder(6,7);
                ArmExtensionEnc = new Encoder(13,14);
                
                compressor = new Compressor(1,1);       //Pressure Switch, Compressor Relay 
                compressor->Start();

                ShifterHigh = new Solenoid(8,1);
                ShifterLow = new Solenoid(8,2);
                EndGameIn = new Solenoid(8,4);
                EndGameOut = new Solenoid(8,5);
                DeploymentOut = new Solenoid(8,7);
                DeploymentIn = new Solenoid(8,8);
                
                
                LeftLineSensor = new DigitalInput(LEFT_SENSOR_CHANNEL);
                CenterLineSensor = new DigitalInput(CENTER_SENSOR_CHANNEL);
                RightLineSensor = new DigitalInput(RIGHT_SENSOR_CHANNEL);
                
                ArmSensor = new DigitalInput(13);
                
                //Update the motors at least every 100ms.
//              myRobot.SetExpiration(0.1);
                GetWatchdog().SetExpiration(100); //0.5); //100);

                // Arm Rotation Control Variables
                CurrentArmPosition = 0.0f;
                TargetArmPosition = 0.0f;
                ArmPosition_error = 0.0f;
                ArmPositionCommand = 0.0f;
                ArmPositionConstant = ARM_PROPORTIONAL_CONSTANT;                // this controls how fast the arm reacts
                CurrentArmStick = 0.0f;
                PreviousArmStick = 0.0f;
                ArmPosMaxCmd = 1.0f;    

                ArmRotation->Set(0.0);          
        }

        
                //ArmExtensionEnc->Get(),
        void Dashboard_Update(void)
        {
                dds->sendIOPortData
                (       
                        LeftDriveEnc->Get(),
                        RightDriveEnc->Get(),
                        LeftLineSensor->Get(),
                        CenterLineSensor->Get(),
                        RightLineSensor->Get(),
                        ArmRotationEnc->Get(),
                        TargetArmPosition,
                        ArmPosition_error,
                        ArmPositionCommand,
                        ds->GetAnalogIn(1),   //slider
                        armUpperLimit->Get(),  
                        armLowerLimit->Get(),
                        armUpperLimit_ANALOG->GetVoltage(),
                        armLowerLimit_ANALOG->GetVoltage(),
                        Minibot_Deploy_Light_Sensor->GetVoltage(),
                        AutoSeq,
                        AutoState
                );
        }

        void ArmExtensionPref()
        {
                if(ds->GetDigitalIn(6) == 1)
                {
                        ArmExtension->Set(armStick->GetY());
                }
                else
                {
                        ArmExtension->Set(armStick->GetTop());
                }
        }
        void ArmRotationPref()
        {
                if(ds->GetDigitalIn(6) == 1)
                {
                        ArmRotation->Set(armStick->GetX());                     
                }
                else
                {
                        ArmRotation->Set(armStick->GetY());                     
                }
        }
        
        void UpdateArm()
        {
                CurrentArmPosition = (float)ArmRotationEnc->Get();
                if(TargetArmPosition > 0)
                {
                        ArmPosition_error = TargetArmPosition - CurrentArmPosition;
                        ArmPositionCommand = ArmPosition_error * ArmPositionConstant; // +.1f;
                        PreviousArmStick = CurrentArmStick;
                        CurrentArmStick = armStick->GetX();
                        
                        // Clamp the command given to the arm motor
                        if(ArmPositionCommand > ArmPosMaxCmd)
                        {
                                ArmPositionCommand = ArmPosMaxCmd;
                        }
                        if(ArmPositionCommand < -ArmPosMaxCmd)
                        {
                                ArmPositionCommand = -ArmPosMaxCmd;
                        }
                        
                        if(fabs(PreviousArmStick) > 0.1f)
                        {
                                if(fabs(CurrentArmStick) < 0.1f)
                                {
                                        // User just stopped using joystick, tell arm controller
                                        // to hold at current position
                                        TargetArmPosition = CurrentArmPosition;
                                }
                                else
                                {
                                        // User is still using the joystick, so set the target
                                        // to zero so the arm control code doesn't do anything
                                        TargetArmPosition = 0;
                                }
                        }
                }
                else
                {
                        ArmPositionCommand = armStick->GetX();
                }
                
                // Ok, the command has been computed, now protect the ends of
                // the arm's motion (don't let the user drive past either limit)
                // If the manual override button is pressed, skip this clamping step
                if ((CurrentArmPosition <= ARM_MINIMUM_ROTATION) && (armStick->GetRawButton(7) == 0))
                {
                        // we're at the lower limit, only allow zero or positive commands
                        ArmPositionCommand = max(0.0f,ArmPositionCommand);
                }
                else if ((CurrentArmPosition >= ARM_MAXIMUM_ROTATION) && (armStick->GetRawButton(7) == 0))
                {
                        ArmPositionCommand = min(ArmPositionCommand,0.0f);      
                }       
                
                ArmRotation->Set(ArmPositionCommand);
                //ArmRotation->Set(0);
        }

        void SetArm(int Position)
        {
                TargetArmPosition = Position;
        }

        
        void UpdateArmExtension(float command)
        {
                bool UpperLimitReached = (armUpperLimit_ANALOG->GetVoltage() < 3.0f);
                bool LowerLimitReached = (armLowerLimit_ANALOG->GetVoltage() > 3.0f);
                if(armStick->GetRawButton(7) == 1)
                {
                        UpperLimitReached = false;
                }
                float ExtensionCmd = command;
                if (UpperLimitReached)
                {
                        if (ExtensionCmd > 0.0f) ExtensionCmd = 0.0f;
                }
                if (LowerLimitReached)
                {
                        if (ExtensionCmd < 0.0f) ExtensionCmd = 0.0f;
                }
                ArmExtension->Set(ExtensionCmd);
                //ArmExtension->Set(0);

        }
        void UpdateDriving()
        {
                LeftDrive->Set(leftStick->GetY());
                RightDrive->Set(-rightStick->GetY());
        }
        
        void StartEncoders()
        {
                LeftDriveEnc->Reset();
                RightDriveEnc->Reset();
                //Enc->Reset();
                ArmRotationEnc->Reset();
                LeftDriveEnc->Start();
                RightDriveEnc->Start();
                ArmExtensionEnc->Start();
                ArmRotationEnc->Start();
        }


        void Follow_Line(bool Fork, int Left_Right,float speed_scale) //Left = 0; Right = 1; Straight = 2;
        {
                if((LeftLineSensor->Get()==1)&&(CenterLineSensor->Get()==1)&&(RightLineSensor->Get()==1))
                {
                        LeftDrive->Set(0.0f);
                        RightDrive->Set(0.0f);
                        Auto_End_Line = 1;
                }
                if((LeftLineSensor->Get() ==  1) && (CenterLineSensor->Get() == 1))
                {
                        Auto_End_Line_Fork = 1;
                }
                else if((LeftLineSensor->Get() ==  1) && (RightLineSensor->Get() == 1))
                {
                        Auto_End_Line_Fork = 1;
                }
                else if((CenterLineSensor->Get() ==  1) && (RightLineSensor->Get() == 1))
                {
                        Auto_End_Line_Fork = 1;
                }
                if(Auto_End_Line == 0)
                {
                        if(LeftLineSensor->Get() == 1)
                        {
                                Previous_Sensor_Detect = 1;
                                //LeftDrive->Set(-AUTO_SPEED_LOW);               //<- That is forward
                                //RightDrive->Set(AUTO_SPEED_FULL);
                        }
                        if(RightLineSensor->Get() == 1)
                        {
                                Previous_Sensor_Detect = 2;
                                //LeftDrive->Set(-AUTO_SPEED_FULL);
                                //RightDrive->Set(AUTO_SPEED_LOW);
                        }
                        if(CenterLineSensor->Get() == 1)
                        {
                                Previous_Sensor_Detect = 3;
                                //LeftDrive->Set(-AUTO_SPEED_FULL);
                                //RightDrive->Set(AUTO_SPEED_FULL);
                        }
                        if(Previous_Sensor_Detect == 1)
                        {
                                LeftDrive->Set(-AUTO_SPEED_LOW*speed_scale);              
                                RightDrive->Set(AUTO_SPEED_FULL*speed_scale);
                        }
                        if(Previous_Sensor_Detect == 2)
                        {
                                LeftDrive->Set(-AUTO_SPEED_FULL*speed_scale);
                                RightDrive->Set(AUTO_SPEED_LOW*speed_scale);
                        }
                        if(Previous_Sensor_Detect == 3)
                        {
                                LeftDrive->Set(-AUTO_SPEED_FULL*speed_scale);
                                RightDrive->Set(AUTO_SPEED_FULL*speed_scale);
                        }       
                }
                else if(Auto_End_Line == 1)
                {
                        LeftDrive->Set(0.0f);
                        RightDrive->Set(0.0f);
                }
        }

        void Read_Auto_Switch(void)
        {
                // Check the auto sequence number only in disabled mode!  During auto, all
                // DS inputs are zeroed.
                int seq = 0;
                bool Dec1 = !ds->GetDigitalIn(1); 
                bool Dec2 = !ds->GetDigitalIn(3);
                bool Dec4 = !ds->GetDigitalIn(5);
                bool Dec8 = !ds->GetDigitalIn(7);
                
                if (Dec1 == true)
                {
                        seq += 1;
                }
                if (Dec2 == true)
                {
                        seq += 2;
                }
                if (Dec4 == true)
                {
                        seq += 4;
                }
                if (Dec8 == true)
                {
                        seq += 8;
                }
                if (seq != 0) AutoSeq = seq;
        }
        virtual void Disabled() 
        {
                Read_Auto_Switch();
        }
        
        void Auto_Goto_State(int new_state)
        {
                AutoState = new_state;
                AutoStateTimer.Reset();
                AutoLeftEncoderStart = LeftDriveEnc->Get();
                AutoRightEncoderStart = RightDriveEnc->Get();
        }
        
        void AUTO_one()
        {
                if(AutoState == 0)
                {
                    float speed_scale = 1.0f;
                    SetArm(AUTO_ROTATION_START);
                        UpdateArmExtension(-.5);
                        
                        // watch encoders and slow down when we get close!
                        // in low gear, about 2500 ticks from start to end
                        if (left_drive_count > AUTO_FULL_SPEED_MOVE_ENCODER_TICKS)      
                        {
                                speed_scale = 0.6f;
                        }
                        Follow_Line(false,3,speed_scale);       
                        if(Auto_End_Line == 1)
                        {
                                Auto_Goto_State(1);
                        }
                }
                else if (AutoState == 1)
                {
                        SetArm(AUTO_ROTATION_SCORE_HIGH); 
                        UpdateArmExtension(-.5);

                        if(abs((int)ArmPosition_error) < 60.0f)  //HACK!
                        {
                                Auto_Goto_State(2);
                        }
                }
                else if (AutoState == 2)
                {
                        // release tube
                        if (AutoStateTimer.Get() < AUTO_CLAW_REJECT_SECONDS)
                        {
                                ClawTop->Set(-AUTO_CLAW_REJECT_SPEED);
                                ClawBottom->Set(-AUTO_CLAW_REJECT_SPEED);
                                UpdateArmExtension(0.5);
                        }
                        else 
                        {
                                Auto_Goto_State(3);

                                ClawTop->Set(0);
                                ClawBottom->Set(0);
                        }
                }
                else if (AutoState == 3)
                {
                        // back up for a few seconds
                        if (AutoStateTimer.Get() < AUTO_BACKUP_SECONDS) // abs(left_drive_count) > 200)
                        {
                                LeftDrive->Set(1.0f);               //<- That is forward
                                RightDrive->Set(-1.0f);
                                if (AutoStateTimer.Get() > 2.0f)
                                {
                                        SetArm(AUTO_ROTATION_LOWER);  // lower arm after 2 sec
                                }
                        } 
                        else
                        {
                                LeftDrive->Set(0);
                                RightDrive->Set(0);
                                
                                Auto_Goto_State(4);

                        }
                }
                else if (AutoState == 4)
                {
                        
                        if (abs(left_drive_count) < AUTO_TURN_TICKS)
                        {
                                LeftDrive->Set(-AUTO_SPEED_LOW);               //<- That is forward
                                RightDrive->Set(-AUTO_SPEED_LOW);
                        } 
                        else
                        {
                                LeftDrive->Set(0);
                                RightDrive->Set(0);
                        Auto_Goto_State(5);
                        }
                }
        }
        void AUTO_two_fork(int rightleft) //o = right 1 = left
        {
                if(AutoState == 0)
                {
                    float speed_scale = 1.0f;
                        UpdateArmExtension(-.5);
                        SetArm(AUTO_ROTATION_START_FORK);
                        // watch encoders and slow down when we get close!
                        // in low gear, about 2500 ticks from start to end
                        //if (left_drive_count > AUTO_FULL_SPEED_MOVE_ENCODER_TICKS)    
                        //{
                        //      speed_scale = 0.5f;
                        //}
                        Follow_Line(false,3,speed_scale);       
                        Auto_End_Line = 0;
                        if(left_drive_count > 1640)
                        {
                                
                                Auto_Goto_State(1);
                                
                                LeftDrive->Set(0.0f);
                                RightDrive->Set(0.0f);
                        }
                }
                else if (AutoState == 1)
                {
                        if((CenterLineSensor->Get() == 1) && (RightLineSensor->Get() == 0) && (LeftLineSensor->Get() == 0))
                        {
                                Auto_Goto_State(2);
                        }
                        else
                        {
                                if(rightleft == 0)
                                {
                                        LeftDrive->Set(-0.35f);
                                        RightDrive->Set(-0.2f);
                                }
                                else
                                {
                                        LeftDrive->Set(0.2f);
                                        RightDrive->Set(.35f);
                                }
                        }
                }
                else if (AutoState == 2)
                {
                        // release tube
                        if (left_drive_count < 250)
                        {
                                Follow_Line(0,0,.5f);
                                                                
                        }
                        else 
                        {
                                Auto_Goto_State(3);

                                LeftDrive->Set(0.0f);
                                RightDrive->Set(0.0f);
                        }
                }
                else if (AutoState == 3)
                {
                        if(rightleft == 0)
                        {
                                if (left_drive_count > -40)
                                {
                                        LeftDrive->Set(.3f);
                                        RightDrive->Set(.3f);
                                }
                                else 
                                {
                                        Auto_Goto_State(4);
                                        LeftDrive->Set(0.0f);
                                        RightDrive->Set(0.0f);
                                }
                        }
                        else if(rightleft == 1)
                        {
                                if (right_drive_count > -40)
                                {
                                        LeftDrive->Set(-.3f);
                                        RightDrive->Set(-.3f);                                  
                                }
                                else 
                                {
                                        Auto_Goto_State(4);
                                        LeftDrive->Set(0.0f);
                                        RightDrive->Set(0.0f);
                                }
                        }
                }
                else if (AutoState == 4)
                {
                        if(left_drive_count < 240)
                        {
                                LeftDrive->Set(-.3f);
                                RightDrive->Set(.3f);
                        }
                        else
                        {
                                Auto_Goto_State(5);
                                LeftDrive->Set(0.0f);
                                RightDrive->Set(0.0f);
                        }
                }
                else if(AutoState == 5)
                {
                        if (AutoStateTimer.Get() < AUTO_CLAW_REJECT_SECONDS)
                        {
                                ClawTop->Set(-AUTO_CLAW_REJECT_SPEED);
                                ClawBottom->Set(-AUTO_CLAW_REJECT_SPEED);
                                UpdateArmExtension(0.5);
                        }
                        else
                        {
                                Auto_Goto_State(6);
                        }
                }
        }
        
        void Auto_Hack(void)
        {
                LeftDrive->Set(-AUTO_SPEED_FULL);
                RightDrive->Set(AUTO_SPEED_FULL);
                //Follow_Line(false,3,1.0f);    

                Dashboard_Update();
                //UpdateArm();
        }
        
        void Autonomous(void)
        {
                GetWatchdog().SetEnabled(false);        
                StartEncoders();
                
                // make sure we're in low gear
                //ShifterHigh->Set(true);
                //ShifterLow->Set(false);
                
                Previous_Sensor_Detect = 3;
                TargetArmPosition = 0; // clear out any arm command
                AutoLeftEncoderStart = LeftDriveEnc->Get();
                AutoRightEncoderStart = RightDriveEnc->Get();
                
                AutoStateTimer.Reset();
                AutoStateTimer.Start();
        
                if (ds->GetDigitalIn(ENABLE_AUTONOMOUS) == 1)   // only run the autonomous program if jumper is in place
                {
                        AutoState = 0;
                        Auto_End_Line = 0; 
                        Auto_End_Line_Fork = 0;
                        
                        
                        while(IsAutonomous() == true)
                        {
                                left_drive_count = LeftDriveEnc->Get() - AutoLeftEncoderStart;
                                right_drive_count = RightDriveEnc->Get() - AutoRightEncoderStart;

                                Dashboard_Update();
                                UpdateArm();
                                
                                //Auto_Hack();
                                //AUTO_one();
                                
                                if(AutoSeq == 1)
                                {
                                        AUTO_one();
                                }
                                else if(AutoSeq == 2)
                                {
                                        AUTO_two_fork(0);
                                }
                                else if(AutoSeq == 3)
                                {
                                        AUTO_two_fork(1);
                                }
                                
                                GetWatchdog().Feed();           
                        }
                }
                UpdateArmExtension(0.0);
                GetWatchdog().SetEnabled(true);
                
                // high gear after auto
                //ShifterHigh->Set(false);
                //ShifterLow->Set(true);
        }
        
        /**
         * Main test program in operator control period.
         * Aquires images from the camera and determines if they match FRC targets.
         */
        void OperatorControl(void) {
                /**
                 * Set up the PID controller with some parameters that should be pretty
                 * close for most kitbot robots.
                 */

                // Create and set up a camera instance. first wait for the camera to start
                // if the robot was just powered on. This gives the camera time to boot.
                
                StartEncoders();
                TargetArmPosition = 0; // clear out any arm command
                //Minibot_Deploy_Timer.Reset();
                //Minibot_Deploy_Timer.Start();
                                
                printf("Getting camera instance\n");
                AxisCamera &camera = AxisCamera::GetInstance();
                printf("Setting camera parameters\n");
                camera.WriteResolution(AxisCamera::kResolution_320x240);
                camera.WriteCompression(20);
                camera.WriteBrightness(0);


                // set sensitivity for the 2010 kit gyro
                gyro->SetSensitivity(0.007);

                // set MotorSafety expiration
                GetWatchdog().SetExpiration(1.0);

                
                // keep track of the previous joystick trigger value
//              bool lastTrigger = false;

                // loop getting images from the camera and finding targets
                printf("Starting operator control loop\n");
                Timer timer;
                timer.Start();
                while (IsOperatorControl())
                {

                        GetWatchdog().Feed();

                        Auto_End_Line =0; 
                        Dashboard_Update();
                        LeftSensorReading = LeftLineSensor->Get();
                        UpdateDriving();
                        UpdateArm();
//                      UpdateArmExtension();

                        static bool ShifterToggle = false;
                        static int LastTrigger = 0;
                        int Trigger = rightStick->GetTrigger();

                        if ((Trigger == 1) && (LastTrigger == 0))
                        { 
                                ShifterToggle = !ShifterToggle;
                        }
                        LastTrigger = Trigger;

                        if(ShifterToggle)
                        {
                                ShifterHigh->Set(true);
                                ShifterLow->Set(false);
                        }
                        else
                        {
                                ShifterHigh->Set(false);
                                ShifterLow->Set(true);
                        }
                        
                        if(rightStick->GetRawButton(2) == 1)
                        {
                                EndGameOut->Set(false);
//                              EndGameOut->Set(true);
//                              EndGameIn->Set(false);
                        }
                        else
                        {
                                EndGameOut->Set(true);
//                              EndGameOut->Set(false);
//                              EndGameIn->Set(true);
                        }
                        
                        static bool DeploymentToggle = false;
                        static int LastButton = 0;
                        int Button = rightStick->GetRawButton(3);

                        if ((Button == 1) && (LastButton == 0))
                        { 
                                DeploymentToggle = !DeploymentToggle;
                        }
                        LastButton = Button;

                        // Auto Deploy for MiniBot
//                      bool Auto_Deploy = ds->GetAnalogIn(1);
/*                      if (
                                        (Minibot_Deploy_Timer.Get() > MINIBOT_AUTO_DEPLOY_SECONDS) 
                                        && (Minibot_Deploy_Light_Sensor->GetVoltage() > MINIBOT_AUTO_DEPLOY_VOLTAGE)
                                        && (Auto_Deploy == true)
                                )
                        {
                                DeploymentToggle = true;
                        }
*/                      
                        if(DeploymentToggle)
                        {
                                DeploymentOut->Set(true);
                                DeploymentIn->Set(false);
                        }
                        else
                        {
                                DeploymentOut->Set(false);
                                DeploymentIn->Set(true);
                        }
                        
                        UpdateArmExtension(armStick->GetY());
                                
                        // Preset 1
                        if(armStick->GetRawButton(12)==1)
                        {
                                SetArm(ARM_PICKUP_PRESET);      // pickup height
                        }
                        // Preset 2
                        if(armStick->GetRawButton(11)==1)
                        {
                                SetArm(ARM_HIGH_GOAL_PRESET);   // high goal
                        }
                        
                        if (armStick->GetTrigger() == 1)
                        {
                                ClawTop->Set(-CLAW_REJECT_SPEED);
                                ClawBottom->Set(-CLAW_REJECT_SPEED);
                                
                                if (armUpperLimit_ANALOG->GetVoltage() < 3.0f)
                                {
                                        ArmExtension->Set(0.0f);
                                }
                                else 
                                {
                                        ArmExtension->Set(1.0f);
                                }
                                
                        }
                        else if (armStick->GetRawButton(2) == 1)
                        {
                                ClawTop->Set(CLAW_CAPTURE_SPEED);
                                ClawBottom->Set(CLAW_CAPTURE_SPEED);
                        }                       
                        else if (armStick->GetRawButton(3) == 1)
                        {
                                ClawTop->Set(-CLAW_ROTATION_SPEED);
                                ClawBottom->Set(CLAW_ROTATION_SPEED);
                        }
                        else if (armStick->GetRawButton(5) == 1)
                        {
                                ClawTop->Set(CLAW_ROTATION_SPEED);
                                ClawBottom->Set(-CLAW_ROTATION_SPEED);
                        }
                        else
                        {
                                ClawTop->Set(0.0);
                                ClawBottom->Set(0.0);
                        }

//                      ClawTop->Set(armStick->GetTop());
                        
/*                      bool trigger;
                        // if trigger is pulled, the robot will run with standard arcade drive
                        // otherwise the robot will home towards the target.
                        if (trigger = stick->GetTrigger()) {
                                if (trigger != lastTrigger)                     // check if trigger changed
                                        turnController.Enable();
                                // if there's a fresh and we're at the previous target heading then
                                // get a camera image and process it
                                if (camera.IsFreshImage())
                                {
                                        timer.Reset();
                                        // get the gyro heading that goes with this image
                                        double gyroAngle = gyro->PIDGet();

                                        // get the camera image
                                        HSLImage *image = camera.GetImage();

                                        // find FRC targets in the image
                                        vector<Target> targets = Target::FindCircularTargets(image);
                                        delete image;
                                        if (targets.size() == 0 || targets[0].m_score < MINIMUM_SCORE)
                                        {
                                                // no targets found. Make sure the first one in the list is 0,0
                                                // since the dashboard program annotates the first target in green
                                                // and the others in magenta. With no qualified targets, they'll all
                                                // be magenta.
                                                Target nullTarget;
                                                nullTarget.m_majorRadius = 0.0;
                                                nullTarget.m_minorRadius = 0.0;
                                                nullTarget.m_score = 0.0;
                                                if (targets.size() == 0)
                                                        targets.push_back(nullTarget);
                                                else
                                                        targets.insert(targets.begin(), nullTarget);
                                                dds->sendVisionData(0.0, gyro->GetAngle(), 0.0, 0.0, targets);
                                                if (targets.size() == 0)
                                                        printf("No target found\n\n");
                                                else
                                                        printf("No valid targets found, best score: %f ", targets[0].m_score);
                                        }
                                        else {
                                                // We have some targets.
                                                // set the new PID heading setpoint to the first target in the list
                                                double horizontalAngle = targets[0].GetHorizontalAngle();
                                                double setPoint = gyroAngle + horizontalAngle;

                                                turnController.SetSetpoint(setPoint);
                                                
                                                // send dashbaord data for target tracking
                                                dds->sendVisionData(0.0, gyro->GetAngle(), 0.0, targets[0].m_xPos / targets[0].m_xMax, targets);
                                                printf("Target found %f ", targets[0].m_score);
//                                              targets[0].Print();
                                        }
                                        printf("Time: %f\n", 1.0 / timer.Get());
                                }
                        } else {
                                // if the trigger is not pressed, then do Arcade driving with joystick 1
                                if (trigger != lastTrigger)
                                        turnController.Disable();
                                base->ArcadeDrive(stick);
                        }
                        lastTrigger = trigger;
                        
                        // send the dashbaord data associated with the I/O ports
                        dds->sendIOPortData();
*/              }
        }
};

START_ROBOT_CLASS(RobotDemo);