its really messy at the moment, just have been trying lots of things sorry.
Code:
package org.usfirst.frc.team2282.robot;
import edu.wpi.first.wpilibj.SampleRobot;
import edu.wpi.first.wpilibj.RobotDrive;
import edu.wpi.first.wpilibj.CANTalon;
import edu.wpi.first.wpilibj.CANTalon.FeedbackDevice;
import edu.wpi.first.wpilibj.CANTalon.TalonControlMode;
import edu.wpi.first.wpilibj.CameraServer;
import edu.wpi.first.wpilibj.DriverStation;
import edu.wpi.first.wpilibj.Joystick;
import edu.wpi.first.wpilibj.Timer;
import edu.wpi.first.wpilibj.Victor;
import edu.wpi.first.wpilibj.smartdashboard.SendableChooser;
import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
/**
* This is a demo program showing the use of the RobotDrive class.
* The SampleRobot class is the base of a robot application that will automatically call your
* Autonomous and OperatorControl methods at the right time as controlled by the switches on
* the driver station or the field controls.
*
* The VM is configured to automatically run this class, and to call the
* functions corresponding to each mode, as described in the SampleRobot
* 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.
*
* WARNING: While it may look like a good choice to use for your code if you're inexperienced,
* don't. Unless you know what you are doing, complex code will be much more difficult under
* this system. Use IterativeRobot or Command-Based instead if you're new.
*/
public class Robot extends SampleRobot {
RobotDrive myRobot;
Joystick gamepad,coDriver;
DriverStation DS;
Victor L1,L2,L3,R1,R2,R3,intake, shooter;
CANTalon awesomeBar, shooterFlap;
CameraServer cam;
double leftValue, rightValue;
boolean Arcade;
final String defaultAuto = "Default";
final String customAuto = "My Auto";
SendableChooser chooser;
int loopCount;
public Robot() {
gamepad = new Joystick(0);
coDriver = new Joystick(1);
L1 = new Victor(0);
L2 = new Victor(1);
L3 = new Victor(2);
R1 = new Victor(3);
R2 = new Victor(4);
R3 = new Victor(5);
shooter = new Victor(7);
awesomeBar = new CANTalon(2);
//awesomeBar.configMaxOutputVoltage(12.0);
shooterFlap = new CANTalon(1);
shooterFlap.setFeedbackDevice(FeedbackDevice.PulseWidth);
shooterFlap.setInverted(true);
shooterFlap.reverseSensor(true);
awesomeBar.setFeedbackDevice(FeedbackDevice.PulseWidth);
awesomeBar.setInverted(false);
awesomeBar.reverseSensor(true);
intake = new Victor(6);
// cam = CameraServer.getInstance();
// cam.startAutomaticCapture("cam0");
DS = DriverStation.getInstance();
}
public void robotInit() {
chooser = new SendableChooser();
chooser.addDefault("Default Auto", defaultAuto);
chooser.addObject("My Auto", customAuto);
SmartDashboard.putData("Auto modes", chooser);
SmartDashboard.putBoolean("Arcade", true);
}
/**
* This autonomous (along with the chooser code above) shows how to select between different autonomous modes
* using the dashboard. The sendable chooser code works with the Java SmartDashboard. If you prefer the LabVIEW
* Dashboard, remove all of the chooser code and uncomment the getString line to get the auto name from the text box
* below the Gyro
*
* You can add additional auto modes by adding additional comparisons to the if-else structure below with additional strings.
* If using the SendableChooser make sure to add them to the chooser code above as well.
*/
public void autonomous() {
while(isAutonomous() && isEnabled()){
}
}
/**
* Runs the motors with arcade steering.
*/
public void operatorControl() {
//Calibrate Shooter Flap
shooterFlap.changeControlMode(TalonControlMode.PercentVbus);
while(!shooterFlap.isFwdLimitSwitchClosed()){
shooterFlap.set(-0.2);
}
shooterFlap.set(0.0);
shooterFlap.changeControlMode(TalonControlMode.Position);
Timer.delay(0.02);
shooterFlap.setPosition(0);
//Calibrate Awesome Bar
awesomeBar.changeControlMode(TalonControlMode.PercentVbus);
while(!awesomeBar.isRevLimitSwitchClosed()){
awesomeBar.set(-0.2);
}
awesomeBar.set(0.0);
awesomeBar.changeControlMode(TalonControlMode.Position);
Timer.delay(0.02);
awesomeBar.setPosition(0.0);
while (isOperatorControl() && isEnabled()) {
shooterFlap.enable();
Arcade = SmartDashboard.getBoolean("Arcade");
if(Arcade) {
leftValue = (gamepad.getY() + -gamepad.getRawAxis(4));
rightValue = -(gamepad.getY() - -gamepad.getRawAxis(4));
}
else {
leftValue = gamepad.getY();
rightValue = -gamepad.getRawAxis(5);
}
SmartDashboard.putNumber("Arm Position", awesomeBar.getPosition());
L1.set(leftValue);
L2.set(leftValue);
L3.set(leftValue);
R1.set(rightValue);
R2.set(rightValue);
R3.set(rightValue);
awesomeBar.changeControlMode(TalonControlMode.Position);
SmartDashboard.putNumber("Pulse witdth pos", awesomeBar.getPulseWidthPosition());
awesomeBar.enable();
intake.set(coDriver.getY());
if(coDriver.getRawButton(1)){
shooter.set(1.0);
shooterFlap.set(-1.2);
loopCount = 0;
}
else if(coDriver.getRawButton(4)){
shooter.set(-1.0);
}
else{
shooter.set(0);
if(loopCount >= 300) {
//shooterFlap.set(0.0);
}
}
SmartDashboard.putNumber("flap pos", shooterFlap.getPosition());
SmartDashboard.putNumber("output voltage", shooterFlap.getOutputVoltage());
SmartDashboard.putString("cont mode", shooterFlap.getControlMode().name());
SmartDashboard.putNumber("Awesome Bar POS", awesomeBar.getPosition());
awesomeBar.enable();
if(gamepad.getRawButton(6)){
awesomeBar.set(0.98);
}
else if (gamepad.getRawButton(5)){
awesomeBar.set(0.0);
}
else if (gamepad.getRawButton(2)){
awesomeBar.set(3.7);
}
else{
}
Timer.delay(0.01);
loopCount++;
//1.194 good shot position
/*
awesomeBar.changeControlMode(TalonControlMode.PercentVbus);
shooterFlap.changeControlMode(TalonControlMode.PercentVbus);
if(gamepad.getRawButton(5)){
awesomeBar.set(1.0);
}
else if(gamepad.getRawButton(6)){
awesomeBar.set(-1.0);
}
else {
awesomeBar.set(0.0);
}
shooterFlap.set(-coDriver.getRawAxis(5));
*/
}
}
/**
* Runs during test mode
*/
public void test() {
while(isTest() && isEnabled()){
}
}
}