void TeleopPeriodic(void)
{
bool flag = true; //flag object initial declaration to ensure passing Piston toggle works properly
float leftStick = gamePad->GetRawAxis(4);
float rightStick = gamePad->GetRawAxis(2);
bool rightBumper = gamePad->GetRawButton(6);
bool leftBumper = gamePad->GetRawButton(5);
bool buttonA = gamePad->GetRawButton(2);
if(fabs(leftStick) >= 0.05 || fabs(rightStick >= 0.05))
{
m_robotDrive->TankDrive(leftStick, rightStick);
}
else if(rightBumper || leftBumper)
{
if(rightBumper && !leftBumper)
{
ShiftHigh();
}
else if(leftBumper && !rightBumper)
{
ShiftLow();
}
}
else if(buttonA && flag)
{
flag = false;
passingPiston->Set(true);
}
else
{
if(!buttonA)
{
flag = false;
MotorControlLeft(0.0);
MotorControlRight(0.0);
}
else
{
MotorControlLeft(0.0);
MotorControlRight(0.0);
}
}
}
/*******************************************************************************
*
* FUNCTION: Teleop()
*
* PURPOSE: This is where you put code that you want to execute while
* your robot is in teleoperation mode. While in teleoperation
* mode, this function is called every 26.2ms after new data
* is received from the master processor.
*
* CALLED FROM: main() in ifi_frc.c
*
* PARAMETERS: None
*
* RETURNS: Nothing
*
* COMMENTS: While in this mode, all operator interface data is valid
* and all robot controller outputs are enabled.
*
*******************************************************************************/
void Teleop(void)
{
static int launch_stick = 0;
static unsigned char out=127;
//printf("in loop\r\n");
TankDrive();
if(p2_sw_top) ShiftHigh();
if(p2_sw_trig) ShiftLow();
if(p1_sw_top) OrientationFwd();
if(p1_sw_trig) OrientationRev();
DriveHandler();
Compressor_Handler();
// Launcher Control
/*launch_stick = (int)(p4_y) - 127;
if( launch_stick >=0) launch_stick = launch_stick;
else launch_stick = launch_stick;
out = (unsigned char)LimitChar(launch_stick);
//LAUNCHER_PWM = out;
*/
//
//printf("Top: %d | Trig: %d | aux1: %d | aux2: %d | y: %d \r\n",p3_sw_top,p3_sw_trig,p3_sw_aux1,p3_sw_aux2,p3_y);
Launcher_Handler();
// update the state of the LEDs on the operator interface
Update_OI_LEDs(); // located in ifi_code.c
}
