void TeleopPeriodic()
{
//printf("I done it\n");
//put the values of both the solenoids on SmartDashboard
SmartDashboard::PutBoolean("Solenoid One", singOne->GetState());
SmartDashboard::PutBoolean("Solenoid Two", singTwo->GetState());
// Piston One goes out when any of the top buttons are pressed
if (GetTopButtons() == true)
{
singOne->OverrideEnable();
printf("Piston One Out\n");
}
// if none of the top buttons are pressed then Piston One comes in
else if(GetTopButtons() == false)
{
singOne->OverrideDisable();
}
// Piston Two goes out if Piston One is out and the trigger is pulled
if (stick->GetTrigger() == true && GetTopButtons()==true)
{
singTwo->OverrideEnable();
printf("Piston Two Out\n");
}
//If the trigger is not pulled or none of the top buttons are pushed Piston Two comes in
else if (stick->GetTrigger() == false || GetTopButtons()== false)
{
singTwo->OverrideDisable();
}
}
/**
* Read the state of the trigger on the joystick.
*
* Look up which button has been assigned to the trigger and read its state.
*
* @param port The USB port for this joystick.
* @param hand This parameter is ignored for the Joystick class and is only here to complete the GenericHID interface.
* @return The state of the trigger.
*/
bool GetTrigger(UINT32 port, JoystickHand hand)
{
Joystick *stick = getJoystick(port);
if (stick == NULL)
return 0;
return stick->GetTrigger((Joystick::JoystickHand) hand);
}
/**
* @brief Sets a cached joystick value.
* @param joy_id Which joystick to set the cached value for.
* @param stick A Joystick object with the X, Y, and Z axes set, as well as each of the buttons.
*/
void Proxy::SetJoystick(int joy_id, Joystick & stick)
{
wpi_assert(joy_id < NUMBER_OF_JOYSTICKS+1 && joy_id >= 0);
char tmp[32];
sprintf(tmp, "Joy%d", joy_id);
string name = tmp;
if(!disableAxes[joy_id-1]) {
set(name + 'X', stick.GetX());
set(name + 'Y', stick.GetY());
set(name + 'Z', stick.GetZ());
set(name + 'R', stick.GetTwist());
set(name + 'T', stick.GetThrottle());
for(int AxisId=1; AxisId<=6; AxisId++) {
sprintf(tmp, "%sA%d", name.c_str(), AxisId);
set(tmp, stick.GetRawAxis(AxisId));
}
} else {
if(!stick.GetRawButton(disableAxes[joy_id-1])) {
set(name + 'X', stick.GetX());
set(name + 'Y', stick.GetY());
set(name + 'Z', stick.GetZ());
set(name + 'R', stick.GetTwist());
set(name + 'T', stick.GetThrottle());
for(int AxisId=1; AxisId<=6; AxisId++) {
sprintf(tmp, "%sA%d", name.c_str(), AxisId);
set(tmp, stick.GetRawAxis(AxisId));
}
}
}
if(!disableButtons[joy_id-1]) {
for(unsigned i=1;i<=NUMBER_OF_JOY_BUTTONS;i++) {
sprintf(tmp, "%sB%d", name.c_str(), i);
set(tmp,stick.GetRawButton(i));
}
set(name + "BT", stick.GetTrigger());
} else {
if(!stick.GetRawButton(disableButtons[joy_id-1])) {
for(unsigned i=1;i<=NUMBER_OF_JOY_BUTTONS;i++) {
sprintf(tmp, "%sB%d", name.c_str(), i);
set(tmp,stick.GetRawButton(i));
}
set(name + "BT", stick.GetTrigger());
}
}
}
void OperatorControl(void)
{
GetWatchdog().SetEnabled(true);
printf("Entered OperatorControl\n");
while (IsOperatorControl())
{
GetWatchdog().Feed();
// use the trigger to start recording.. at the moment,
// it just gets ignored if you call it more than once
if (stick.GetTrigger())
recorder.StartRecording();
myRobot.ArcadeDrive(stick); // drive with arcade style (use right stick)
// always call the recording routine
recorder.Record();
}
}
void TeleopPeriodic(void) {
// increment the number of teleop periodic loops completed
GetWatchdog().Feed();
m_telePeriodicLoops++;
/*
* No longer needed since periodic loops are now synchronized with incoming packets.
if (m_ds->GetPacketNumber() != m_priorPacketNumber) {
*/
/*
* Code placed in here will be called only when a new packet of information
* has been received by the Driver Station. Any code which needs new information
* from the DS should go in here
*/
m_dsPacketsReceivedInCurrentSecond++; // increment DS packets received
// put Driver Station-dependent code here
// Demonstrate the use of the Joystick buttons
// DemonstrateJoystickButtons(m_rightStick, m_rightStickButtonState, "Right Stick", &m_solenoids[1]);
// DemonstrateJoystickButtons(m_leftStick, m_leftStickButtonState, "Left Stick ", &m_solenoids[5]);
/***
// determine if tank or arcade mode, based upon position of "Z" wheel on kit joystick
if (m_rightStick->GetZ() <= 0) { // Logitech Attack3 has z-polarity reversed; up is negative
// use arcade drive
m_robotDrive->ArcadeDrive(m_rightStick); // drive with arcade style (use right stick)
if (m_driveMode != ARCADE_DRIVE) {
// if newly entered arcade drive, print out a message
printf("Arcade Drive\n");
m_driveMode = ARCADE_DRIVE;
}
} else {
// use tank drive
m_robotDrive->TankDrive(m_leftStick, m_rightStick); // drive with tank style
if (m_driveMode != TANK_DRIVE) {
// if newly entered tank drive, print out a message
printf("Tank Drive\n");
m_driveMode = TANK_DRIVE;
}
}
***/
/* Grab z-wheel value and transform from [1, -1] to [0,4600]
* 4600 rpm is a guess */
float rawZ, transformedZ;
rawZ = m_leftStick->GetZ();
// transformedZ = (1.0 - rawZ)/(-2.0);
transformedZ = -2300.0 * rawZ + 2300.0;
/* Driver station display output. */
char msg[256];
static DriverStationLCD *dsLCD = DriverStationLCD::GetInstance();
sprintf(msg, "Launcher Speed = %f RPM", transformedZ);
dsLCD->Printf(DriverStationLCD::kUser_Line1, 1, msg);
sprintf(msg, "Loader Limit = %u", m_loaderLimit->Get());
dsLCD->Printf(DriverStationLCD::kUser_Line2, 1, msg);
sprintf(msg, "Loader Trigger = %u", m_leftStick->GetTrigger());
dsLCD->Printf(DriverStationLCD::kUser_Line3, 1, msg);
// line number (enum), starting col, format string, args for format string ...
dsLCD->UpdateLCD();
// use arcade drive
//m_driveGain = (1.0 - m_rightStick->GetZ())/(-2.0);
m_robot->ArcadeDrive(m_rightStick); // drive with arcade style (use right stick)
// m_robotFrontDrive->Drive(-m_driveGain*m_rightStick->GetY(),-m_driveGain*m_rightStick->GetX()); // negative sign to fix bug in turn
// m_robotRearDrive->Drive(m_driveGain*m_rightStick->GetY(),-m_driveGain*m_rightStick->GetX()); // negative sign to fix bug in turn
// -Add an elevation control, add a motor controller to port 8, and add a joystick button pair to control up/down
if (m_rightStick->GetRawButton(m_elevatorUpButton)){
m_elevatorMotor->Set(m_elevatorUpSpeed);
}
else if (m_rightStick->GetRawButton(m_elevatorDownButton)){
m_elevatorMotor->Set(m_elevatorDownSpeed);
}
else {
m_elevatorMotor->Set(0.0);
}
// trigger button activates feeding and loading mechanisms
if ((m_leftStick->GetTop() || m_feeding) && (!m_loading) && (m_feedCounter <= m_feedCount)){
m_feeding = true;
m_feedCounter++;
m_feedMotor->Set(m_feedSpeed);
}
else {
m_feeding = false;
m_feedCounter = 0;
m_feedMotor->Set(0.);
}
m_loaderLimit->Get(); //just testing digital input port 1
// top(2) button uses the Z wheel to control the speed of the loading mechanisms
if (((m_leftStick->GetTrigger() || m_loading) && (!m_feeding)) && (m_loadCounter <= m_loadCount)){//comment out this line
// if (((m_leftStick->GetTrigger() || m_loading) && (!m_feeding)) && (bool) m_loaderLimit->Get()){ //uncomment this line
m_loading = true;
m_loadCounter++; // comment out this line
m_loadMotor->Set(m_loadSpeed); // load breech
}
else if ((m_loading && (!m_feeding)) && (m_loadCounter > m_loadCount) && (m_loadCounter <= ((m_loadCount+m_loadPauseCount)))) { // comment out the pause logic
m_loading = true;// comment out the pause logic
m_loadCounter++;// comment out the pause logic,
m_loadMotor->Set(0.); // pause loader // comment out the pause logic
}
// else if ((m_loading && (!m_feeding)) && (m_loadCounter >= (m_loadCount+m_loadPauseCount)) && (m_loadCounter<= 2*m_loadCount-13)) { // comment out this line
else if ((m_loading && (!m_feeding)) && (m_loadCounter >= (m_loadCount+m_loadPauseCount)) && (m_loaderLimit->Get()!=0)) {
//Retracting loader arm (feeder), have not yet hit the limit switch.
// else if ((m_loading &! m_feeding) && (bool) m_loaderResetLimit->Get()) { // uncomment this line
m_loading = true;
m_loadCounter++; // comment out the pause logic
m_loadMotor->Set(-m_loadSpeed);; // reset loader
}
else {
//Finished retracting loader arm (feeder), so stop it.
m_loading = false;
m_loadCounter = 0;
m_loadMotor->Set(0.);; // stop loader
}
// Set launcher motor command
m_launchMotor->Set(-(1.0 - m_leftStick->GetZ())/(-2.0)); // transform from [-1.0, +1.0] to [0.0, +1.0]
/*
} // if (m_ds->GetPacketNumber()...
*/
// use buttons 8 and 9 to trim load motor position in maintenance mode
/*if (m_leftStick->GetRawButton(8) && (!m_feeding) && (!m_loading)){
m_loadMotor->Set(0.12);
}
else if (m_leftStick->GetRawButton(9) && (!m_feeding) && (!m_loading)){
m_loadMotor->Set(-0.1);
}*/
//Use buttons 10 and 11 to trim feed motor position in maintenance mode
if (m_leftStick->GetRawButton(11) && (!m_feeding) && (!m_loading)){
m_feedMotor->Set(0.15);
}
else if (m_leftStick->GetRawButton(10) && (!m_feeding) && (!m_loading)){
m_feedMotor->Set(-0.1);
}
} // TeleopPeriodic(void)
void OperatorControl(void)
{
// Teleoperated Code.
/*double WaitDash = 0.0;
double FireDash = 0.0;
double intPause = 0.0;
SmartDashboard::PutNumber("P", 3.0);
SmartDashboard::PutNumber("W", 0.0);
SmartDashboard::PutNumber("A", 0.0);
WaitDash = SmartDashboard::GetNumber("P");
FireDash = SmartDashboard::GetNumber("W");
intPause = SmartDashboard::GetNumber("A");
SmartDashboard::PutNumber("P", WaitDash);
SmartDashboard::PutNumber("W", FireDash);
SmartDashboard::PutNumber("A", WaitDash);
*/
// Enable and start the compressor.
//compressor->Enabled();
compressor->Start();
// Enable drive motor safety timeout.
myRobot.SetSafetyEnabled(true);
// Enable watchdog and initial feed.
GetWatchdog().SetEnabled(true);
GetWatchdog().SetExpiration(1);
GetWatchdog().Feed();
// Set robot in low gear by default. Not active.
//s[0]->Set(false);
GetWatchdog().Feed();
//bool blnShoot = false;
bool blnLowHang = false;
bool blnShift = false;
GetWatchdog().Feed();
bool blnShooterSpd = false;
bool blnReverse = false;
float fltShoot;
float fltSpeed = 1;
int intFail = 0;
timerLowHang.Reset();
timerShift.Reset();
timerFire.Reset();
timerShooter.Reset();
timerDriveCtrl.Reset();
timerCamera.Reset();
timerReverse.Reset();
timerLowHang.Start();
timerShift.Start();
timerFire.Start();
timerShooter.Start();
timerDriveCtrl.Start();
timerCamera.Start();
timerReverse.Start();
GetWatchdog().Feed();
//sd->sendIOPortData();
// Local variables.
//float fltStick1X, fltStick1Y;
while (IsOperatorControl())
{
if(timerReverse.Get() > 0.5)
{
if(stick1->GetRawButton(8) && blnReverse == false )
{
blnReverse = true;
GetWatchdog().Feed();
timerReverse.Reset();
timerReverse.Start();
}
else if(stick1->GetRawButton(9) && blnReverse == true)
{
blnReverse = false;
GetWatchdog().Feed();
timerReverse.Reset();
timerReverse.Start();
}
}
if(blnReverse == false)
{
fltStick1Y = stick1->GetY();
fltStick1X = stick1->GetX();
SmartDashboard::PutBoolean("Reverse",false);
GetWatchdog().Feed();
}
else if(blnReverse == true)
{
fltStick1Y = ((stick1->GetY())*(-1));
fltStick1X = ((stick1->GetX())*(1));
SmartDashboard::PutBoolean("Reverse",true);
GetWatchdog().Feed();
}
myRobot.ArcadeDrive(fltStick1Y,fltStick1X);
//myRobot.ArcadeDrive(stick1);
GetWatchdog().Feed(); // Feed hungary demonic Watchdog.
SmartDashboard::PutBoolean("Touching Tower?",LimitSwitch->Get());
SmartDashboard::PutNumber("Throttle (%)",stick1->GetY()*(-100));
SmartDashboard::PutNumber("Steering (%)",stick1->GetX()*(100));
GetWatchdog().Feed();
//End Stick1 arcade drive code.
GetWatchdog().Feed();
fltShoot = (((-(stick2->GetRawAxis(3)))+1)/2);
GetWatchdog().Feed();
SmartDashboard::PutNumber("Shooter Power (%)", fltShoot);
SmartDashboard::PutNumber("Shooter Set Speed (%)", (fltSpeed*100));
//float fltPressureSwitch = m_pressureSwitch;
//float fltRelay = m_relay;
//SmartDashboard::PutNumber("Demo",3);
GetWatchdog().Feed();
if(timerShift.Get() > 0.2)
{
if(stick1->GetRawButton(7) || stick1->GetTrigger())
{
if(blnShift == false)
{
GetWatchdog().Feed();
s[0]->Set(false);
s[1]->Set(true);
SmartDashboard::PutString("Gear","Low");
blnShift = true;
timerShift.Stop();
timerShift.Reset();
timerShift.Start();
GetWatchdog().Feed();
}
else if (blnShift == true)
{
GetWatchdog().Feed();
s[0]->Set(true);
s[1]->Set(false);
SmartDashboard::PutString("Gear","High");
blnShift = false;
timerShift.Stop();
timerShift.Reset();
timerShift.Start();
GetWatchdog().Feed();
}
}
}
if(stick1->GetRawButton(2) && blnLowHang == false && timerLowHang.Get() > 0.5)
{
blnLowTime = true;
blnLowHang = true;
timerLowHang.Stop();
timerLowHang.Reset();
timerLowHang.Start();
GetWatchdog().Feed();
}
else if(stick1->GetRawButton(2) && blnLowHang == true && timerLowHang.Get() > 0.5)
{
blnLowTime = false;
blnLowHang = false;
timerLowHang.Stop();
timerLowHang.Reset();
timerLowHang.Start();
GetWatchdog().Feed();
}
if(blnLowTime == true)
{
s[3]->Set(true);
GetWatchdog().Feed();
}
else if (blnLowTime == false)
{
s[3]->Set(false);
GetWatchdog().Feed();
}
if(stick1->GetRawButton(3))
{
mtdCameraCode();
GetWatchdog().Feed();
}
if(stick2->GetTrigger() && intFail == 0 && timerFire.Get() > 0.8)
{
s[2]->Set(true);
SmartDashboard::PutString("Shooter Piston","In");
intFail = 1;
GetWatchdog().Feed();
timerFire.Stop();
timerFire.Reset();
timerFire.Start();
GetWatchdog().Feed();
}
else if(stick2->GetTrigger() && intFail == 1 && timerFire.Get() > 0.8)
{
s[2]->Set(false);
intFail = 0;
SmartDashboard::PutString("Shooter Piston","Out");
GetWatchdog().Feed();
timerFire.Stop();
timerFire.Reset();
timerFire.Start();
GetWatchdog().Feed();
}
if(stick2->GetRawButton(2) && blnShooterSpd == false && timerShooter.Get() > 0.5)
{
GetWatchdog().Feed();
myShooter1.Set(-fltSpeed);
myShooter2.Set(-fltSpeed);
SmartDashboard::PutString("Shooter","On");
SmartDashboard::PutNumber("Shooter Speed (%)",(fltSpeed)*(100));
blnShooterSpd = true;
GetWatchdog().Feed();
timerShooter.Stop();
timerShooter.Reset();
timerShooter.Start();
GetWatchdog().Feed();
}
else if(stick2->GetRawButton(2) && blnShooterSpd == true && timerShooter.Get() > 0.5)
{
GetWatchdog().Feed();
myShooter1.Set(0);
myShooter2.Set(0);
SmartDashboard::PutString("Shooter","Off");
SmartDashboard::PutNumber("Shooter Speed (%)",0);
blnShooterSpd = false;
GetWatchdog().Feed();
//Wait(0.2);
timerShooter.Stop();
timerShooter.Reset();
timerShooter.Start();
GetWatchdog().Feed();
}
if(stick2->GetRawButton(10))
{
fltSpeed = 0.6;
GetWatchdog().Feed();
}
if(stick2->GetRawButton(9))
{
fltSpeed = 0.7;
GetWatchdog().Feed();
}
if(stick2->GetRawButton(8))
{
fltSpeed = 0.8;
GetWatchdog().Feed();
}
if(stick2->GetRawButton(7))
{
fltSpeed = 0.9;
GetWatchdog().Feed();
}
if(stick2->GetRawButton(6))
{
fltSpeed = 1;
GetWatchdog().Feed();
}
if(stick2->GetRawButton(11))
{
fltSpeed = fltShoot;
GetWatchdog().Feed();
}
GetWatchdog().Feed();
}
}