/**************************************** * 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: "); }