That helps to know that the two Victors are used to operate a belt and that running them in one direction will lift the launcher and that running them in the other direction will cause them to lower the launcher.
However, I don't understand how you know when to stop? If you keep running the Victors to lift the launcher, how will you know when you are at the launch point? If the Victors run too long, will it damage your robot?
From what I know so far, it sounds like your only option is to run your Victors at certain power levels for a specific time period and hope for the best.
Here's an example which has a autonomous routine that drives for a bit, then raises your launcher for a period of time and then stops (I don't know what you need to do to actually fire).
The operator control has been adjusted slightly so that it uses two buttons on the joystick so the operator should be able to raise or lower the launcher depending which button they hold down (when they release both buttons it should stop). There are several comments flagged with TODO indicating that there are values that will need to be determined/adjusted.
Code:
package edu.wpi.first.wpilibj.templates;
import edu.wpi.first.wpilibj.SimpleRobot;
import edu.wpi.first.wpilibj.Joystick;
import edu.wpi.first.wpilibj.RobotDrive;
import edu.wpi.first.wpilibj.Talon;
import edu.wpi.first.wpilibj.Timer;
import edu.wpi.first.wpilibj.Victor;
/**
* * The VM is configured to automatically run this class, and to call the *
* functions corresponding to each mode, as described in the SimpleRobot *
* documentation. If you change the name of this class or the package after *
* creating this project, you must also update the manifest file in the resource
* * directory.
*/
public class Drive extends SimpleRobot {
private RobotDrive myDrive, Shooter;
private Joystick driveStick, driveStick2;
private Talon FrontRight, FrontLeft, BackRight, BackLeft;
private Victor M1, M2;
/**
* * This function is called once each time the robot enters autonomous
* mode.
*/
public void robotInit() {
FrontRight = new Talon(1);
BackLeft = new Talon(2);
FrontLeft = new Talon(3);
BackRight = new Talon(4);
myDrive = new RobotDrive(FrontLeft, BackLeft, FrontRight, BackRight);
myDrive.setInvertedMotor(RobotDrive.MotorType.kFrontRight, true);
M1 = new Victor(5);
M2 = new Victor(6);
Shooter = new RobotDrive(M1, M2);
driveStick = new Joystick(1);
driveStick2 = new Joystick(2);
}
public void autonomous() {
// Turn off drive safety so we can use long timer delays before
// changing drive power (if you don't do this, your program
// may get interrupted and killed during autonomous)
myDrive.setSafetyEnabled(false);
// Example of driving at half power for 3 seconds (not sure if this
// will be forward or backwards on your robot)
myDrive.drive(-.5, 0.0);
Timer.delay(3.0);
// Stop drive motors
myDrive.stopMotor();
// Raise the launcher for one and a half seconds
// TODO: Determine time value or better yet add a switch so you
// know when launcher is in position
startRaisingLauncher();
Timer.delay(1.5);
stopMovingLauncher();
// TODO: INSERT FIRE OPERATION HERE
// Stop everything
stopAll();
}
public void operatorControl() {
// You can enable the drive safety check here (optional) as you
// should be calling the arcadeDrive() method very frequently)
myDrive.setSafetyEnabled(true);
while (isOperatorControl() && isEnabled()) {
// Let driver control robot
myDrive.arcadeDrive(driveStick);
// Check to see if operator is raising or lowering the launcher
checkLauncherControl();
Timer.delay(0.01);
}
// Stop everything
stopAll();
}
/**
* Allow operator to raise/lower launcher using two buttons (stops if neither pressed).
*/
private void checkLauncherControl() {
int RAISE_BUTTON = 1;
int LOWER_BUTTON = 2;
if (driveStick2.getRawButton(RAISE_BUTTON) == true) {
startRaisingLauncher();
} else if (driveStick2.getRawButton(LOWER_BUTTON) == true) {
startLoweringLauncher();
} else {
stopMovingLauncher();
}
}
/**
* Helper method to stop all motors on the robot.
*/
private void stopAll() {
myDrive.stopMotor();
stopMovingLauncher();
}
/**
* Set output power to 0 to both launcher motors that move the lift.
*/
private void stopMovingLauncher() {
setLiftPower(0);
}
/**
* Turn launcher motors on so launcher starts going up.
*/
private void startRaisingLauncher() {
// TODO: Determine a reasonable power level for your shooter motors
// (you will need to experiment)
setLiftPower(0.5);
}
/**
* Turn launcher motors on so launcher starts going down.
*/
private void startLoweringLauncher() {
// TODO: Determine a reasonable power level for your shooter motors
// (you will need to experiment)
setLiftPower(-0.5);
}
/**
* Set the power on the motors that raise and lower the launcher.
*
* @param power The power level in the range of [-1.0, +1.0] where
* negative values will lower the launcher and positive values will
* lift it.
*/
private void setLiftPower(double power) {
// Apply power to shooter motors
// TODO: Determine if one or both power values need to be inverted
// (see comment block below)
M1.set(power);
M2.set(power);
// IMPORTANT: If one or both of the motor connections end up being the
// opposite of what you expect, you may need to invert the power applied
// to one or both of the motors.
// Commented example of how to invert the power applied to both motors:
// (this would replace the set methods above)
//M1.set(-power);
//M2.set(-power);
}
}
The above code does not actually fire the ball (unless your ball automatically fires once the launcher has been lifted to a certain height).
Let me mention again that it is typically desirable to add limit switches so you can tell when the launcher has reached a certain point and should not be raised or lowered any more. The is especially important if moving the lift too far could damage the robot.