void DriveControl() { //Drives la wheels of the robot
flightX = flightStick->GetRawAxis(0); //Pull joystick side motion for later use
flightY = flightStick->GetRawAxis(1); //Pull joystick forward motion for later use (forward is -, backwards is +)
flightZ = flightStick->GetRawAxis(4); //Pull joystick twist motion for later use
flightThrottle = ((((shootStick->GetThrottle() - 1)*-1)/2) * .8 + .2); //Pull throttle to modify drive variables
//Throttle value is between .2 and 1.0
if (fabs(flightX) < deadZone) { //Deaden x
flightX = 0;
}
if (fabs(flightY) < deadZone) { //Deaden y
flightY = 0;
}
if (fabs(flightZ) < deadZone) { //Deaden z
flightZ = 0;
}
if (flightStick->GetRawButton(strafeButtonChannel)){ //Set drive to strafe mode
driveMode = 0;
}
if (flightStick->GetRawButton(arcadeButtonChannel)){ //Set drive to arcade mode
driveMode = 1;
}
if (flightStick->GetRawButton(fieldButtonChannel)){ //Set drive to field-centric mode
driveMode = 2;
}
if (shootStick->GetRawButton(gyroResetChannel)){ //Reset gyro with the trigger
yawGyro->Reset();
}
flightX = flightX * flightThrottle;
flightY = flightY * flightThrottle;
flightZ = flightZ * flightThrottle;
if(driveMode == 1){
robotDrive->MecanumDrive_Cartesian(flightZ, flightY, flightX, 0);
SmartDashboard::PutString("DriveMode", "Arcade");
}
else if(driveMode == 2){
robotDrive->MecanumDrive_Cartesian(flightX, flightY, flightZ, yawGyro->GetAngle());
SmartDashboard::PutString("DriveMode", "Field");
}
else{
robotDrive->MecanumDrive_Cartesian(flightX, flightY, flightZ, 0);
SmartDashboard::PutString("DriveMode", "Strafe");
}
SmartDashboard::PutNumber("GyroAngle", yawGyro->GetAngle());
}
/******************************************************************************
* Function: DisabledPeriodic
*
* Description: Run the functions that are expected during the period when the
* robot is disabled.
******************************************************************************/
void DisabledPeriodic()
{
Scheduler::GetInstance()->Run();
Light1.Set(1);
Light2.Set(1);
while(IsDisabled())
{
UpdateActuatorCmnds(0,0,false,false,false,false,false,false,false,0,0,0,0,0);
ReadAutonSwitch();
UpdateSmartDashboad(Sw1.Get(),
Sw2.Get(),
Sw3.Get(),
Sw4.Get(),
BlSw.Get(),
(double)AutonState,
(double)0,
ballarm.GetDistance(),
gyroOne.GetAngle(),
accel.GetX(),
accel.GetY(),
accel.GetZ(),
(double)0,
(double)0);
wait(kUpdatePeriod);
}
}
void RobotInit ()
{
lw = LiveWindow::GetInstance();
CameraServer::GetInstance()->SetQuality(50);
//the camera name (ex "cam0") can be found through the roborio web interface
CameraServer::GetInstance()->StartAutomaticCapture("cam1");
AutonState = 0;
ballarm.Reset();
ballarm.SetMaxPeriod(.01);
ballarm.SetMinRate(.02);
ballarm.SetDistancePerPulse(.9);
gyroOne.Calibrate();
UpdateActuatorCmnds(0,0,false,false,false,false,false,false,false,0,0,0,0,0);
UpdateSmartDashboad(false,
false,
false,
false,
false,
0,
0,
0,
0,
0,
0,
0,
0,
0);
}
/******************************************************************************
* Function: TeleopPeriodic
*
* Description: This is the function that is called during the periodic period
* for teleop.
******************************************************************************/
void TeleopPeriodic()
{
float DrvRY, DrvLY;
float BallRollerSpd;
float FishTapeSpd;
float WenchSpd;
float LgtY, LgtZ;
double LgtPOV;
int LgtP1;
bool LgtT0;
bool LgtB1,LgtB3,LgtB4,LgtB5,LgtB6,LgtB7,LgtB8,LgtB9,LgtB11,LgtB10,LgtB12;
bool XbxB4, XbxB5, XbxB2, XbxB1;
bool BallSw;
int XbxPOV;
double ArmP;
double ArmError;
double desiredPos;
double LoArmCurve;
while (IsOperatorControl() && IsEnabled())
{
/* Read sensor values here: */
ArmP = ballarm.GetDistance();
BallSw = BlSw.Get();
/* Read Xbox controller commands here: */
DrvLY = -xboxDrive.GetRawAxis(1);
DrvRY = -xboxDrive.GetRawAxis(5);
XbxB1 = xboxDrive.GetRawButton(1);
XbxB2 = xboxDrive.GetRawButton(2);
XbxB4 = xboxDrive.GetRawButton(4);
XbxB5 = xboxDrive.GetRawButton(5);
XbxPOV = xboxDrive.GetPOV(0);
/* Read Logictec joystick commands here: */
LgtY = Logitech.GetRawAxis(1);
LgtZ = Logitech.GetRawAxis(2);
LgtP1 = Logitech.GetPOV(0);
LgtT0 = Logitech.GetRawButton(1);
LgtB1 = Logitech.GetRawButton(2);
LgtB3 = Logitech.GetRawButton(3);
LgtB4 = Logitech.GetRawButton(4);
LgtB5 = Logitech.GetRawButton(5);
LgtB6 = Logitech.GetRawButton(6);
LgtB7 = Logitech.GetRawButton(7);
LgtB8 = Logitech.GetRawButton(8);
LgtB9 = Logitech.GetRawButton(9);
LgtB10 = Logitech.GetRawButton(10);
LgtB11 = Logitech.GetRawButton(11);
LgtB12 = Logitech.GetRawButton(12);
LgtPOV = (double)LgtP1;
// DriverStation::
//double GetMatchTime();
// SmartDashboard::PutNumber("MatchTime", GetMatchTime());
if(LgtB12 && LgtB10)
{
ballarm.Reset();
}
/* Set the desired position of the ball arm. */
if (LgtB7)
{
/* This is the middle position of the arm: */
desiredPos = 120;
}
else if (LgtB8)
{
/* This is the upper position of the arm: */
desiredPos = 210;
}
else if((LgtB3) || (LgtB5) ||(LgtB6) || (LgtB9) || (LgtB11))
{
/* Default */
desiredPos = 0;
}
/* Set the ball roller state: */
if (LgtT0 == true &&
BallSw == true)
{
BallRollerSpd = 1;
}
else if (LgtB1 == true)
{
BallRollerSpd = -1;
}
else
{
BallRollerSpd = 0.0;
}
/* Set the output to the fish tape: */
if (LgtP1 == 180)
{
FishTapeSpd = -1.0;
}
else if(LgtP1 == 0)
{
FishTapeSpd = 1.0;
}
else
{
FishTapeSpd = 0.0;
}
/* Set the output to the lower arm: */
LoArmCurve = LgtY*LowArmGx;
// SmartDashboard::
/* Determine the wench state */
if (LgtB4 == true)
{
WenchSpd = 1;
}
else if (XbxB4)
{
WenchSpd = -.5;
}
else
{
WenchSpd = 0.0;
}
/* if(GetMatchTime() < 30)
{
Light1.Set(0);
Light2.Set(0);
}*/
/* Output data to the smart dashboard: */
ReadAutonSwitch();
UpdateSmartDashboad(Sw1.Get(),
Sw2.Get(),
Sw3.Get(),
Sw4.Get(),
BlSw.Get(),
AutonState,
0,
ballarm.GetDistance(),
gyroOne.GetAngle(),
accel.GetX(),
accel.GetY(),
accel.GetZ(),
(double)DrvLY,
(double)DrvRY);
UpdateActuatorCmnds(BallRollerSpd,
desiredPos,
LgtB3,
LgtB5,
LgtB6,
LgtB7,
LgtB8,
LgtB9,
LgtB11,
DrvLY,
DrvRY,
FishTapeSpd,
LoArmCurve,
WenchSpd);
/* Force the program to wait a period of time in order to conserve power: */
Wait(kUpdatePeriod); // Wait 5ms for the next update.
Scheduler::GetInstance()->Run();
}
}
/******************************************************************************
* Function: AutonomousPeriodic
*
* Description: This is the function that is called during the autonomous period.
******************************************************************************/
void AutonomousPeriodic()
{
double accelX, desiredPos;
float DriveL, DriveR;
int AutonStateLocal;
double ArmP;
double ArmError;
// Light1.Set(1);
// Light2.Set(1);
AutonStateLocal = 0;
while (IsAutonomous())
{
/* Read sensors here */
accelX = accel.GetX();
// ArmP = ballarm.GetDistance();
if (AutonDisabled == false)
{
switch (AutonState)
{
case 2: /* Low bar and rough terrain */
{
if (AutonStateLocal == 0)
{
/* State 1: Raise arm to desired position */
AutonStateLocal = 1;
DriveL = 0.6;
DriveR = 0.6;
desiredPos = 100;
}
else if (AutonStateLocal == 1 && AutonTime < 1.8)
{
DriveL = 0.6;
DriveR = 0.6;
desiredPos = 100;
}
else if (AutonTime >= 1.8)
{
AutonDisabled = true;
AutonStateLocal = 3;
}
break;
}
case 4: /* Low bar and rough terrain */
{
if (AutonStateLocal == 0)
{
/* State 1: Raise arm to desired position */
AutonStateLocal = 1;
DriveL = 0.8;
DriveR = 0.8;
desiredPos = 180;
}
else if (AutonStateLocal == 1 && AutonTime < 1.8)
{
DriveL = 0.85;
DriveR = 0.85;
desiredPos = 180;
}
else if (AutonTime >= 1.8)
{
AutonDisabled = true;
AutonStateLocal = 3;
}
break;
}
case 0:
default:
{
/* Default, no auton */
desiredPos = 0.0;
DriveL = 0.0;
DriveR = 0.0;
break;
}
}
}
else
{
DriveL = 0.0;
DriveR = 0.0;
desiredPos = 0;
AutonStateLocal = 10;
}
UpdateSmartDashboad(Sw1.Get(),
Sw2.Get(),
Sw3.Get(),
Sw4.Get(),
BlSw.Get(),
AutonState,
0,
ballarm.GetDistance(),
gyroOne.GetAngle(),
accel.GetX(),
accel.GetY(),
accel.GetZ(),
(double)DriveL,
(double)DriveR);
UpdateActuatorCmnds(0,
desiredPos,
false,
false,
false,
false,
false,
false,
false,
DriveL,
DriveR,
0,
0,
0);
Wait(kUpdatePeriod); // Wait 5ms for the next update.
AutonTime += kUpdatePeriod;
}
Scheduler::GetInstance()->Run();
}
