/****************************************
* IsAutoDone:
* Input = None
* Output = True if no longer autonomous
* For autonomous only.
* This checks to see if autonomous is over.
* In case the the competition-broadcast-thing fails and doesn't
* send the single stating that Autonomous is over, this also
* provides some interrupts.
* Autonomous can be ended by turning safety off or by
* pushing a joystick nearly to the max (any direction)
* while holding the trigger down.
*/
bool IsAutoDone(void)
{
// Can disable autonomous by turning safety off.
bool safetyKill = stick1->GetRawButton(DISABLE_SAFETY_BUTTON) ||
stick2->GetRawButton(DISABLE_SAFETY_BUTTON);
// Can disable autonomous by attempting to move at max speed.
bool moveKill = (fabs(stick1->GetMagnitude()) > 0.9) &&
(stick1->GetRawButton(MOVE_FAST_BUTTON));
bool liftKill = (fabs(stick2->GetMagnitude()) > 0.9) &&
(stick2->GetRawButton(MOVE_FAST_BUTTON));
// Disable if a signal is sent out by the driver station.
bool systemKill = (false == IsAutonomous()) || IsOperatorControl();
return safetyKill || moveKill || liftKill || systemKill;
}
/****************************************
* DriveHost:
* Input = Joystick data
* Output = Robot movement (controls mechanum wheels)
* Radically altered code from last year.
* Altered so it uses the new buttons.
* TODO:
* - Find out how to pass robotDrive here without breaking anything
* (Not a necessary task).
*/
void DriveHost(GenericHID *moveStick)
{
// Safety primarily to prevent toppling or for safer demos.
isFastSpeedOn = false;
if (moveStick->GetRawButton(MOVE_FAST_BUTTON)) {
//if ((false == isSafetyModeOn) && (false == isLiftHigh)) {
isFastSpeedOn = true;
//}
}
// Magnitude: [-1.0 to 1.0] - How far to travel.
// Direction: In degrees - Which way to travel.
// Rotation : [-1.0 to 1.0] - How much to turn.
// Joystick returns a float in range [-1.0 to 1.0] automatically.
// Using moveStick will not compile.
float magnitude = fabs(stick1->GetMagnitude()); // fabs = Float abs
float direction = stick1->GetDirectionDegrees();
float rotationSpeed = (moveStick->GetThrottle() - 1.1) * -0.5 + 0.07;
float rotationPress = int(moveStick->GetRawButton(ROTATE_RIGHT_BUTTON))
- int(moveStick->GetRawButton(ROTATE_LEFT_BUTTON));
float rotation = rotationSpeed * rotationPress;
direction = (direction < 0.0) ? direction + 360.0 : direction;
// Just in case - prevents values from being over 1.0 (absolute value)
// Higher numbers cause motors to spin alarmingly fast.
magnitude = (magnitude > 1.0) ? 1.0 : magnitude;
rotation = (rotation > 1.0) ? 1.0 : rotation;
rotation = (rotation < -1.0) ? -1.0 : rotation;
if (!isFastSpeedOn) {
magnitude *= SPEED_DECREASE;
rotation *= SPEED_DECREASE;
}
// Prevents drift if values are too close to zero.
magnitude = (magnitude < 0.1) ? 0.0 : magnitude;
rotation = (fabs(rotation) < 0.04) ? 0.0 : rotation;
// This is where the magic happens.
robotDrive.HolonomicDrive(magnitude, direction, rotation);
SmartDashboard::Log(direction, "JS- Distance: ");
SmartDashboard::Log(magnitude, "JS- Magnitude: ");
SmartDashboard::Log(rotation, "JS- Rotation: ");
}