[krudeboy51]

#include "WPILib.h"
#include "DriverStation.h"
//#include "Vision/HSLImage.h"
//#include "Vision/AxisCamera.h"

/*
* This is a demo program showing the use of the RobotBase class.
* The SimpleRobot class is the base of a robot application that will automatically call your
* Autonomous and OperatorControl methods at the right time as controlled by the switches on
* the driver station or the field controls.
*/
///////////////////////////////////////////////////////////////////////////////////////////////////////
/// CLASS ///

class RobotDemo : public SimpleRobot
{
Victor *VictorPair1, *VictorPair2;
RobotDrive *myRobot; // robot drive system
Joystick *RightStick, *LeftStick, *ThirdStick; // only joystick
DriverStation *ds, *dsio; // driver station
Servo *Left_GearServo, *Right_GearServo;
Victor *Dribbler, *CamKicker, *Winch;
DigitalInput *limitSwitchB, *AutoSwitch1, *AutoSwitch2, *AutoSwitch3, *Swltch;
Relay *ArmRelease;
// AxisCamera *camera;

//////////////////////////////////////////////////////////////////////////////////////////////////////
/// PUBLIC ///

public:

RobotDemo(void)
{
VictorPair1 = new Victor(1);
VictorPair2 = new Victor(2);
ds = DriverStation::GetInstance();
myRobot = new RobotDrive(VictorPair1, VictorPair2); // create robot drive base


RightStick = new Joystick(1);// create the joysticks
LeftStick = new Joystick(2);
ThirdStick = new Joystick(3);


AutoSwitch1 = new DigitalInput(1);
AutoSwitch2 = new DigitalInput(2);
AutoSwitch3 = new DigitalInput(3);
limitSwitchB = new DigitalInput(7);



Left_GearServo = new Servo(3);
Right_GearServo = new Servo(4);


Dribbler = new Victor(5);
CamKicker = new Victor(6);
Winch = new Victor(7);


ArmRelease = new Relay(1);



GetWatchdog().SetExpiration(100);
}

///////////////////////////////////////////////////////////////////////////////////////////////////////
/// AUTONOMOUS FIRST ZONE ///

void Autonomous(void)
{
GetWatchdog().SetEnabled(false);

{
{
Right_GearServo->Set(127);
Left_GearServo->Set(127);
}


if (AutoSwitch1->Get()== 1)
{
{
myRobot->Drive(-0.5, 0.0);
Dribbler->Set(1.0);
Wait(1.5);
myRobot->Drive(0.0, 0.0);
CamKicker->Set(-1.0);
Wait(1.0);
CamKicker->Set(0.0);
Dribbler->Set(0.0);
while(limitSwitchB->Get()== 0)
{
CamKicker->Set(-1.0);
}
}
if (limitSwitchB->Get()== 1)
{
CamKicker->Set(0.0);
}
};
////////////////////////////////////////////////////////////////////////////////////////
//////SECOND ZONE/////

if (AutoSwitch2->Get()==1)
{
{
myRobot->Drive(-0.30, 0.0);
Dribbler->Set(1.0);
Wait(2.6);
myRobot->Drive(0.0, 0.0);
CamKicker->Set(-1.0);
Wait(1.0);
CamKicker->Set(0.0);
Dribbler->Set(0.0);
while(limitSwitchB->Get()== 0)
{
CamKicker->Set(-1.0);
}
}
if (limitSwitchB->Get()== 1)
{
CamKicker->Set(0.0);
}
{
myRobot->Drive(-0.30, 0.0);
Dribbler->Set(1.0);
Wait(1.2);
myRobot->Drive(0.0, 0.0);
CamKicker->Set(-1.0);
Wait(1.0);
CamKicker->Set(0.0);
Dribbler->Set(0.0);
while(limitSwitchB->Get()== 0)
{
CamKicker->Set(-1.0);
}
}
if (limitSwitchB->Get()== 1)
{
CamKicker->Set(0.0);
}

};
////////////////////////////////////////////////////////////////////////////////
//////THIRD ZONE//////
if (AutoSwitch3->Get()== 1)
{
{
myRobot->Drive(-0.30, 0.0);
Dribbler->Set(1.0);
Wait(1.3);
myRobot->Drive(0.0, 0.0);
CamKicker->Set(-1.0);
Wait(1.0);
CamKicker->Set(0.0);
Dribbler->Set(0.0);
while(limitSwitchB->Get()== 0)
{
CamKicker->Set(-1.0);
}
}
if (limitSwitchB->Get()== 1)
{
CamKicker->Set(0.0);
}
{
myRobot->Drive(-0.30, 0.0);
Dribbler->Set(1.0);
Wait(1.2);
myRobot->Drive(0.0, 0.0);
CamKicker->Set(-1.0);
Wait(1.0);
CamKicker->Set(0.0);
Dribbler->Set(0.0);
while(limitSwitchB->Get()== 0)
{
CamKicker->Set(-1.0);
}
}
if (limitSwitchB->Get()== 1)
{
CamKicker->Set(0.0);
}
{
myRobot->Drive(-0.30, 0.0);
Dribbler->Set(1.0);
Wait(1.3);
myRobot->Drive(0.0, 0.0);
CamKicker->Set(-1.0);
Wait(1.0);
CamKicker->Set(0.0);
Dribbler->Set(0.0);
while(limitSwitchB->Get()== 0)
{
CamKicker->Set(-1.0);
}
}
if (limitSwitchB->Get()== 1)
{
CamKicker->Set(0.0);
}
{
myRobot->Drive(1.0, 0.0);
Wait(0.5);
myRobot->Drive(0.0, 0.0);
}




}

};
};



//////////////////////////////////////////////////////////////////////////////////////////////
/// OPERATOR CONTROL ///
void OperatorControl(void)
{

GetWatchdog().SetEnabled(false);
//GetWatchdog().Feed();
DriverStationEnhancedIO &dsio = DriverStation::GetInstance()->GetEnhancedIO();





/* while (IsOperatorControl())
{
AxisCamera &camera = AxisCamera::GetInstance();
camera.WriteResolution(AxisCamera::kResolution_640 x480);
camera.WriteCompression(20);
camera.WriteBrightness(0);
}*/
// Assume that the THIRDSTICK THROTTLE is initially set at 100% (max)
// float Max_DribblerSpeed = ThirdStick->GetThrottle();

/////////////////////////////////////////////////////////////////////////////////////////////
/// Robot Drive ///

while (IsOperatorControl())
{
myRobot->TankDrive(RightStick, LeftStick);

///////////////////////////////////////////////////////////////////////////////////////////
/// Dribbler Control ///

if (ThirdStick->GetRawButton(2))
{
Dribbler->Set(1.0);
};


//////////////////////////////////////////////////////////////////////////////////////////
/// ARM CONTROL ///

/// ARM RELEASE ///

if (ThirdStick->GetRawButton(6))
{
ArmRelease->Set(Relay::kForward);
Winch->Set(0.0);
Dribbler->Set(0.0);
CamKicker->Set(0.0);
}
else if (ThirdStick->GetRawButton(7))
{
ArmRelease->Set(Relay::kReverse);
Winch->Set(0.0);
Dribbler->Set(0.0);
CamKicker->Set(0.0);
}
else
{
ArmRelease->Set(Relay::kOff);
}

/// WINCH ///

if (ThirdStick->GetRawButton(11))
{
Winch->Set(1.0);
CamKicker->Set(0.0);
Dribbler->Set(0.0);
ArmRelease->Set(Relay::kOff);
}
else if (ThirdStick->GetRawButton(10))
{
Winch->Set(-1.0);
CamKicker->Set(0.0);
Dribbler->Set(0.0);
ArmRelease->Set(Relay::kOff);
}
else
{
Winch->Set(0.0);
}

/////////////////////////////////////////////////////////////////////////////////////////////////////
/// CAMKICKER && LIMITSWITCH-B ///

if(ThirdStick-> GetRawButton(4))
{
while (limitSwitchB->Get()== 0)
{
CamKicker->Set(-1.0);
Dribbler->Set(0.0);
Winch->Set(0.0);
ArmRelease->Set(Relay::kOff);
}
CamKicker->Set(0.0);
}


if (ThirdStick->GetTrigger() && (limitSwitchB->Get()== 1))
{
CamKicker->Set(-1.0);
Winch->Set(0.0);
Dribbler->Set(0.0);
ArmRelease->Set(Relay::kOff);

if (limitSwitchB->Get() == 0)
{
CamKicker->Set(0.0);
}
}


/////////////////////////////////////////////////////////////////////////////////////



//////////////////////////////////////////////////////////////////////////////////////////
/// GEAR CONTROL ///

if ( (LeftStick->GetTrigger()) )
{
Left_GearServo -> Set(127);
Right_GearServo -> Set(127);
};

if ( (RightStick->GetTrigger()) )
{
Left_GearServo ->Set(-127);
Right_GearServo ->Set(-127);
};

};
};
};


START_ROBOT_CLASS(RobotDemo);