/**
* Read the state of the top button on the joystick.
*
* Look up which button has been assigned to the top 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 top button.
*/
bool GetTop(UINT32 port, JoystickHand hand)
{
Joystick *stick = getJoystick(port);
if (stick == NULL)
return 0;
return stick->GetTop((Joystick::JoystickHand) hand);
}
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)