/*
* reads the value of the digital input of the specified channel
* requires that the readArudiono() method has already been called
* param 1: the channel to be read
* return: value of the specified digital input
*/
public boolean getInput(int channelID) {
byte temp;
if (timer.get() > 2.0) {
DriverStationLCD.getInstance()
.println(DriverStationLCD.Line.kUser3, 1, " ");
DriverStationLCD.getInstance().updateLCD();
timer.stop();
timer.reset();
}
if (Math.abs(channelID - (13 / 2)) - (13 / 2)
> 0) // returns true if less than zero or greater than thirteen
{
timer.start();
DriverStationLCD.getInstance().println(DriverStationLCD.Line.kUser3, 1, "Invalid ID");
DriverStationLCD.getInstance().updateLCD();
return false;
} else if (channelID < 8) {
temp = (byte) (loByte & (1 << channelID));
return temp != 0;
} else {
temp = (byte) (hiByte & (1 << channelID - 8));
return temp != 0;
}
}
public void printToDriverStation(int row, String text) {
StringBuffer sb = new StringBuffer(text);
while (sb.length() < DriverStationLCD.kLineLength) sb.append(' ');
lcd.println(lines[row], 1, sb.toString());
lcd.updateLCD();
}
// Called just before this Command runs the first time
protected void initialize() {
counter++;
delayTimer = RobotMap.ShooterDelayTimer;
delayTimer.start();
display.println(
DriverStationLCD.Line.kUser2, 1, "Pass command " + counter + " ");
display.updateLCD();
}
// Called repeatedly when this Command is scheduled to run
protected void execute() {
DriverStationLCD.getInstance()
.println(DriverStationLCD.Line.kUser3, 1, drive.getCurrentCommand().getName() + " ");
DriverStationLCD.getInstance()
.println(DriverStationLCD.Line.kUser4, 1, hanger.getCurrentCommand().getName() + " ");
DriverStationLCD.getInstance()
.println(DriverStationLCD.Line.kUser5, 1, climb.getCurrentCommand().getName() + " ");
DriverStationLCD.getInstance()
.println(DriverStationLCD.Line.kUser6, 1, launcher.getCurrentCommand().getName() + " ");
DriverStationLCD.getInstance().updateLCD();
}
/**
* This function is run when the robot is first started up and should be used for any
* initialization code.
*/
public void robotInit() {
black = new Jaguar(4, 1);
red = new Jaguar(4, 2);
leftEncoder = new Encoder(4, 5);
rightEncoder = new Encoder(6, 7);
left = new Joystick(1);
right = new Joystick(2);
gamePad = new Joystick(3);
watchdog = Watchdog.getInstance();
dsLCD = DriverStationLCD.getInstance();
photoreceptorL = new DigitalInput(4, 1);
photoreceptorM = new DigitalInput(4, 2);
photoreceptorR = new DigitalInput(4, 3);
camera = AxisCamera.getInstance();
driveMode = 0; // 0 = Tank; 1 = Arcade; 2 = Kaj
driveToggle = false;
cruiseControl = false;
camera.writeResolution(AxisCamera.ResolutionT.k160x120);
camera.writeWhiteBalance(AxisCamera.WhiteBalanceT.hold);
camera.writeExposureControl(AxisCamera.ExposureT.hold);
camera.writeExposurePriority(AxisCamera.ExposurePriorityT.frameRate);
leftEncoder.start();
rightEncoder.start();
}
/**
* Class used to print to the DriverStationLCD display
*
* @author Jeremy
*/
public class DsLcdStream {
private static DriverStationLCD lcd = DriverStationLCD.getInstance();
public static void println1(String data) {
lcd.println(DriverStationLCD.Line.kUser2, 1, data);
lcd.updateLCD();
}
public static void println2(String data) {
lcd.println(DriverStationLCD.Line.kUser3, 1, data);
lcd.updateLCD();
}
public static void println3(String data) {
lcd.println(DriverStationLCD.Line.kUser4, 1, data);
lcd.updateLCD();
}
public static void println4(String data) {
lcd.println(DriverStationLCD.Line.kUser5, 1, data);
lcd.updateLCD();
}
public static void println5(String data) {
lcd.println(DriverStationLCD.Line.kUser6, 1, data);
lcd.updateLCD();
}
public static void printlnMain(String data) {
lcd.println(DriverStationLCD.Line.kMain6, 1, data);
lcd.updateLCD();
}
}
public Pass() {
super("Pass");
launcher = AeronauticalFacilitation.getLauncher();
requires(launcher);
display = DriverStationLCD.getInstance();
// Use requires() here to declare subsystem dependencies
// eg. requires(chassis);
}
/*
* reads the arduino and recieves 2 bytes to represent the digital inputs attached to it
* saves the 2 bytes to 'loByte' and 'hiByte'
* return: true if and only if it's successful
*/
public boolean readArduino() {
byte[] buffer = {0, 0};
successful =
!m_I2C.transaction(
null, 0, buffer,
2); // doesn't send anything and recieves 2 bytes of data; successful==true if
// transaction is succesful
DriverStationLCD.getInstance()
.println(DriverStationLCD.Line.kUser1, 1, " ");
DriverStationLCD.getInstance()
.println(DriverStationLCD.Line.kUser2, 1, " ");
DriverStationLCD.getInstance()
.println(DriverStationLCD.Line.kUser1, 1, "success=" + successful);
DriverStationLCD.getInstance()
.println(DriverStationLCD.Line.kUser2, 1, "buffer=" + buffer[1] + ", " + buffer[0]);
DriverStationLCD.getInstance().updateLCD();
hiByte = buffer[1];
loByte = buffer[0];
return successful;
}
/** This function is called periodically during autonomous */
public void autonomousPeriodic() {
wedge.moveDown();
lights.set(Relay.Value.kForward);
// tracking.run();
try {
driveMainBelt(true, true);
csc.setSetpoint(2300); // 15' = ~2400rpm
if (launcher.getSpeed() < -2250) {
try {
belt2.setX(-1);
} catch (Exception e) {
}
} else {
belt2.setX(0);
}
station.println(DriverStationLCD.Line.kMain6, 1, "RPM: " + String.valueOf(csc.tmpResult));
} catch (CANTimeoutException ex) {
}
/*try{
if(tracking.foundTarget()){
station.println(DriverStationLCD.Line.kUser2, 1, "Distance: " + String.valueOf(tracking.getTarget("HIGH").distance));
station.println(DriverStationLCD.Line.kUser3, 1, "Suggested RPM: " + String.valueOf(tracking.getTarget("HIGH").distance*62.8));
if(x>0)
station.println(DriverStationLCD.Line.kUser4, 1, "Rotation: <--");
else if(x<0)
station.println(DriverStationLCD.Line.kUser4, 1, "Roation: -->");
else
station.println(DriverStationLCD.Line.kUser4, 1, "I don't need to rotate");
}
else{
station.println(DriverStationLCD.Line.kUser2, 1, "Help, I can't find target by myself!");
station.println(DriverStationLCD.Line.kUser3, 1, "");
station.println(DriverStationLCD.Line.kUser4, 1, "");
}
}catch(Exception e){}*/
station.updateLCD();
}
public void robotInit() {
try {
drive_control = new Joystick(1);
control = new Joystick(2);
drive = new Drive();
station = DriverStationLCD.getInstance();
camera = AxisCamera.getInstance();
camera.writeResolution(AxisCamera.ResolutionT.k320x240);
camera.writeBrightness(0);
camera.writeMaxFPS(10);
launcher = new CANJaguar(7);
belt1 = new Relay(1);
// belt1.setDirection(Relay.Direction.kBoth);
belt2 = new CANJaguar(8);
turret = new Victor(4);
/*launcher.setPID(0.5, 0.001, 0.0);
launcher.configEncoderCodesPerRev(360);
launcher.changeControlMode(CANJaguar.ControlMode.kSpeed);
launcher.enableControl();*/
launcher.configEncoderCodesPerRev(360);
launcher.setSpeedReference(CANJaguar.SpeedReference.kQuadEncoder);
launcher.setPositionReference(CANJaguar.PositionReference.kQuadEncoder);
/*wheelEncoder = new Encoder(1, 2, true, CounterBase.EncodingType.k4X);
wheelEncoder.setDistancePerPulse(1);
wheelEncoder.setReverseDirection(true);
wheelEncoder.start();*/
csc = new CANSpeedController(.02, 0, 0, launcher);
csc.setInputRange(0, 3000);
csc.enable();
timer = new Timer();
// tracking = new EagleEye();
lights = new Relay(3);
/*try {
serial = new SerialPort(115200);
} catch (Exception ex) {
System.out.println("Cannot open serial connection " + ex);
}*/
} catch (CANTimeoutException ex) {
System.out.println(ex);
}
wedge = new wedge(2, 1, 2);
// servo1 = new Servo(1);
// servo2 = new Servo(2);
// wedge = new Relay(2);
}
public void printToClassmate() {
DriverStationLCD driverStation = DriverStationLCD.getInstance();
driverStation.println(DriverStationLCD.Line.kUser2, 1, "");
driverStation.println(DriverStationLCD.Line.kUser3, 1, "");
driverStation.println(DriverStationLCD.Line.kUser4, 1, "");
driverStation.println(DriverStationLCD.Line.kUser5, 1, "");
driverStation.println(DriverStationLCD.Line.kUser6, 1, "");
driverStation.updateLCD();
}
public class DriverStationModule extends Module {
private static final Line[] lines = {
Line.kMain6, Line.kUser2, Line.kUser3, Line.kUser4, Line.kUser5, Line.kUser6
};
private DriverStationLCD lcd = DriverStationLCD.getInstance();
public void printToDriverStation(int row, String text) {
StringBuffer sb = new StringBuffer(text);
while (sb.length() < DriverStationLCD.kLineLength) sb.append(' ');
lcd.println(lines[row], 1, sb.toString());
lcd.updateLCD();
}
private static DriverStationModule instance;
public static synchronized DriverStationModule getInstance() {
if (instance == null) instance = new DriverStationModule();
return instance;
}
}
public void teleopPeriodic() {
watchdog.feed(); // feed the watchdog
// Toggle drive mode
if (!driveToggle && left.getRawButton(2)) {
driveMode = (driveMode + 1) % 3;
driveToggle = true;
} else if (driveToggle && !left.getRawButton(2)) driveToggle = false;
// Print drive mode to DS & send values to Jaguars
switch (driveMode) {
case 0:
dsLCD.println(DriverStationLCD.Line.kMain6, 1, "Drive mode: Tank ");
red.set(left.getY() + gamePad.getY());
black.set(-right.getY() - gamePad.getThrottle());
break;
case 1:
dsLCD.println(DriverStationLCD.Line.kMain6, 1, "Drive mode: Arcade");
red.set(right.getX() - right.getY());
black.set(right.getX() + right.getY());
break;
case 2:
dsLCD.println(DriverStationLCD.Line.kMain6, 1, "Drive mode: Kaj ");
red.set(left.getX() - right.getY());
black.set(left.getX() + right.getY());
break;
} /**/
dsLCD.println(
DriverStationLCD.Line.kUser3,
1,
"1" + photoreceptorL.get() + "2" + photoreceptorM.get() + "3" + photoreceptorR.get());
dsLCD.println(DriverStationLCD.Line.kUser4, 1, "Encoder in 4,5: " + leftEncoder.get() + " ");
dsLCD.println(
DriverStationLCD.Line.kUser5, 1, "Encoder in 6,7: " + rightEncoder.get() + " ");
dsLCD.updateLCD();
}
public static void printlnMain(String data) {
lcd.println(DriverStationLCD.Line.kMain6, 1, data);
lcd.updateLCD();
}
public static void println4(String data) {
lcd.println(DriverStationLCD.Line.kUser5, 1, data);
lcd.updateLCD();
}
// Called repeatedly when this Command is scheduled to run
protected void execute() {
launcher.pass();
display.println(DriverStationLCD.Line.kUser3, 1, "Pass timer: " + delayTimer.get() + " ");
display.updateLCD();
}
public class Team3699Robot extends SimpleRobot {
public Joystick joystick_left = new Joystick(Constants.joystick_left_USB);
public Joystick joystick_right = new Joystick(Constants.joystick_right_USB);
public Joystick joystick_elevator = new Joystick(3);
public DriverStationLCD userMessages = DriverStationLCD.getInstance();
public DriverStation driverstation = DriverStation.getInstance();
public NetworkTable server;
/*<<<<<<< HEAD
// public ShooterControl shooter = new ShooterControl();
public Jaguar shooterMotor = new Jaguar(Constants.shooterPWMChannel);
=======*/
public ShooterControl shooter = new ShooterControl();
public Jaguar shooterMotor = new Jaguar(Constants.shooterPWMChannel);
// >>>>>>> pULL dOWN sHOOTER dURING aUTONOMUS, wITH jUST a tIMED sCRIPT
public ArmControl3 arm = new ArmControl3(this);
public Jaguar armMotor = new Jaguar(Constants.armMotorPWM);
public IntegrationControl2 integ = new IntegrationControl2(this);
// public AnalogChannel Optical_Sensor = new AnalogChannel(Constants.OpticalSensorChannel);
// public Jaguar test_CIM_1 = new Jaguar(7);
// public Jaguar test_CIM_2 = new Jaguar(8);
/*
public DigitalInput Disc_Check = new DigitalInput(Constants.Disc_Check_Channel);
public DigitalInput Disc_Top_Check = new DigitalInput(Constants.Disc_Top_Check_Channel);
public DigitalInput Upside_Down_Check = new DigitalInput(Constants.Upside_Down_Check_Channel);
*/
public RobotDrive robotdrive =
new RobotDrive(Constants.robotdrive_left_PWM, Constants.robotdrive_right_PWM);
public Jaguar Intake_motor = new Jaguar(Constants.intake_PWM);
public Jaguar Elevator_motor = new Jaguar(Constants.elevator_PWM);
public Jaguar Elevator_intake_motor = new Jaguar(Constants.elevator_intake_PWM);
public Jaguar pullDownJaguar = new Jaguar(Constants.pulldownPWM);
// public Jaguar Elevator_intake = new Jaguar(Constants.elevator_intake_PWM);
// public Jaguar Elevator_outtake = new Jaguar(Constants.elevator_outtake_PWM);
public ElevatorControl elevator = new ElevatorControl(this);
public IntakeControl intake = new IntakeControl(this);
// public Auto_Integration2 integ2 = new Auto_Integration2(this);
public Shoot3Auto auto_shoot = new Shoot3Auto(this);
// SendableChooser autoChooser;
int dev_ = 0;
public boolean[] errors = new boolean[10];
public AutoPullDown pullDown = new AutoPullDown();
// ^ Yeah, yeah. Way overinitizized
// ERRNO 0: Init error
// ERRNO 1: SmartDashboard error
// ERRNO 2: Teleop error
public boolean allowUnlimitedErrors = true;
// Set to true to allow unlimeted errors
public void error(String message, int errno) {
if (!(this.errors[errno]) || this.allowUnlimitedErrors) {
log(message);
errors[errno] = true;
}
}
public void resetError(int errno) {
errors[errno] = false;
}
public void log(String message) {
System.out.println(message);
}
public void robotInit() {
log("RobotInit called. Initializing NetworkTabes...");
try {
NetworkTable.setIPAddress("10.36.99.2");
NetworkTable.setServerMode();
NetworkTable.setTeam(3699);
NetworkTable.initialize();
} catch (Exception be) {
error("Error in robotInit: \n " + be.getMessage(), 0);
error("Reocourrence Test! You shouldnt see this!", 0);
}
log("Init SmartDashboard...");
this.server = NetworkTable.getTable("SmartDashboard");
log("" + this.server.containsKey("/SmartDashboard/"));
log("" + this.server.containsKey("/SmartDashboard/FPS"));
log("" + NetworkTable.DEFAULT_PORT);
this.server.putString("_PREINIT0", "_PREINIT::0");
this.server.putString("_PREINIT1", "_PREINIT::1");
this.server.putString("_PREINIT2", "_PREINIT::2");
/*LiveWindow.addActuator("Elevator", "Elevator Intake", Elevator_intake);
LiveWindow.addActuator("Elevator", "Elevator Gearbox", Elevator_motor);
LiveWindow.addActuator("Elevator", "Elevator Outtake", Elevator_outtake);
LiveWindow.addActuator("Intake", "Intake", Elevator_motor);*/
log("Disabling RobotDrive Saftey... :D");
robotdrive.setSafetyEnabled(false);
// autoChooser = new SendableChooser();
// autoChooser.addDefault("Default 3 Discs", this.auto_shoot);
// SmartDashboard.putData("Autonomous mode chooser",autoChooser);
}
public void operatorControl() {
log("Teleop Called Once!");
resetError(1);
resetError(2);
this.elevator.resetState();
// try{
showUserMessages("TeleOp");
this.arm.state = 0;
while (isOperatorControl() && isEnabled()) {
// robotdrive.setSafetyEnabled(true); //In case the program were to stop, this stops the
// motors from continuing to run.
if (!Util.checkButton(this, Constants.robotdrive_break_button)) {
// robotdrive.tankDrive(-doRobotdriveScaling(joystick_left.getY()),
// -doRobotdriveScaling(joystick_right.getY()));
robotdrive.arcadeDrive(
doRobotdriveScaling(joystick_left.getY()), doRobotdriveScaling(joystick_left.getX()));
}
// x, y int := 0, 1;
// if (driverstation.getDigitalIn(3)){
// this.test_CIM_1.set(driverstation.getAnalogIn(2));
// }else{
// this.test_CIM_1.set(0-driverstation.getAnalogIn(2));
// }
//
// if (driverstation.getDigitalIn(4)){
// this.test_CIM_2.set(driverstation.getAnalogIn(3));
// }else{
// this.test_CIM_2.set(0-driverstation.getAnalogIn(3));
// }
updateSmartDashboard();
this.shooter.updateStates(this);
this.shooterMotor.set(this.shooter.calculateShooterSpeed());
if (this.driverstation.getDigitalIn(4)) {
this.armMotor.set(this.driverstation.getAnalogIn(3));
} else {
this.armMotor.set(-this.driverstation.getAnalogIn(3));
}
this.arm.update();
if (this.arm.getArmSpeed() != 0D) {
this.armMotor.set(this.arm.getArmSpeed());
}
if (this.integ.doElevatorUpdate()) {
this.elevator.update();
this.Elevator_motor.set(this.elevator.getElevatorSpeed());
}
this.intake.update();
this.Intake_motor.set(this.intake.calculateIntakeSpeed());
if (this.intake.state == 1) {
this.Elevator_intake_motor.set(0.6D);
} else {
this.Elevator_intake_motor.set(0D);
}
if (this.driverstation.getDigitalIn(3)) {
this.pullDownJaguar.set(this.driverstation.getAnalogIn(2));
} else {
this.pullDownJaguar.set(-this.driverstation.getAnalogIn(2));
}
this.integ.update();
SmartDashboardUpdater.updateSmartDashboard(this);
if (Util.checkButton(this, 9)) {
this.elevator.numDiscs = 0;
}
if (Util.checkButton(this, 8)) {
this.elevator.state = 0;
this.integ.state = 0;
this.intake.state = 0;
this.integ.elevator_out_roller.set(0d);
this.arm.state = 0;
}
{
try {
SmartDashboard.putString("DEBUG*isConnected", "" + this.server.isConnected());
SmartDashboard.putString("DEBUG*isServer", "" + this.server.isServer());
SmartDashboard.putDouble("FPS~!", this.server.getNumber("FPS", -1d));
SmartDashboard.putDouble("NewDistance~!", this.server.getNumber("NewDistance", -1d));
// SmartDashboard.putString("NetworkTable Keys", this.server.);
log("W0RK1NG!");
} catch (Exception be) {
error("ERROR UPDATING SmartDashboard dtt STACK TRACE: " + be.getMessage(), 1);
}
}
Timer.delay(0.005); // and make sure we dont overload the cRIO
}
// }catch (Exception be){
// error("ERROR IN TELEOP! STACKTRACE: \n "+be.getMessage(), 2);
// }
}
public void disabled() {
log("Disabled.");
showUserMessages("Disabled");
}
public void autonomous() {
log("Autonomus! (XD I am bad at spelling)");
showUserMessages("Autonomous"); /*
<<<<<<< HEAD
//autoChooser.getSelected();
auto_shoot.update();
// robotdrive.setSafetyEnabled(false);
// robotdrive.tankDrive(doRobotdriveScaling(-0.45),doRobotdriveScaling(-0.45));
// Timer.delay(2.0);
// robotdrive.tankDrive(doRobotdriveScaling(0.0),doRobotdriveScaling(0.0)); // Use to determine ft/s at this power.
=======
this.pullDown.reset();
while (this.isAutonomous()&&isEnabled()){
this.pullDown.pullDown(this);
}
//robotdrive.tankDrive(doRobotdriveScaling(-0.45),doRobotdriveScaling(-0.45));
//Timer.delay(2.0);
//robotdrive.tankDrive(doRobotdriveScaling(0.0),doRobotdriveScaling(0.0)); // Use to determine ft/s at this power.
>>>>>>> pULL dOWN sHOOTER dURING aUTONOMUS, wITH jUST a tIMED sCRIPT
*/
}
public double doRobotdriveScaling(double value) {
if (driverstation.getDigitalIn(Constants.driverstation_scale_toggle_channel)) {
if (driverstation.getDigitalIn(Constants.driverstation_scale_toggle_channel2)) {
driverstation.setDigitalOut(2, true);
driverstation.setDigitalOut(1, false);
driverstation.setDigitalOut(3, false);
if (driverstation.getAnalogIn(Constants.driverstation_scale_channel) < 1) {
return value * driverstation.getAnalogIn(Constants.driverstation_scale_channel);
}
driverstation.setDigitalOut(3, true);
return 0;
}
driverstation.setDigitalOut(3, false);
driverstation.setDigitalOut(1, true);
driverstation.setDigitalOut(2, false);
return value * driverstation.getAnalogIn(Constants.driverstation_scale_channel) * 0.2;
}
driverstation.setDigitalOut(3, false);
driverstation.setDigitalOut(1, false);
driverstation.setDigitalOut(2, false);
return value * Constants.robotdrive_scale_hi;
}
public void showUserMessages(String status) {
this.userMessages.println(DriverStationLCD.Line.kUser2, 1, "FRC Robotics Team ");
this.userMessages.println(
DriverStationLCD.Line.kUser3, 1, "3699 Robot " + status + " ");
this.userMessages.println(DriverStationLCD.Line.kUser4, 1, Constants.version);
this.userMessages.println(DriverStationLCD.Line.kUser5, 1, "===VENDETTA LIST:===");
this.userMessages.println(DriverStationLCD.Line.kUser6, 1, "(Names Forthcoming) ");
this.userMessages.updateLCD();
}
public void updateSmartDashboard() {
if (joystick_left.getRawButton(2) && dev_ < 100) {
dev_++;
} else if (joystick_left.getRawButton(3) && dev_ > 0) {
dev_--;
}
SmartDashboard.putDouble("X", joystick_left.getX());
SmartDashboard.putDouble("Y", joystick_left.getY());
// SmartDashboard.putDouble("Optical Sensor", Optical_Sensor.getVoltage());
SmartDashboard.putDouble("Battery Voltage", DriverStation.getInstance().getBatteryVoltage());
SmartDashboard.putInt("dev_", dev_);
SmartDashboard.putDouble(
"Elevator Sensor Value (.getAverageVoltage())",
this.elevator.ana_chana.getAverageVoltage());
String state = "ROBOT STATE INFORMATION:";
if (this.elevator.state == 0) {
state = state + "\nElevator Stopped.";
}
if (this.elevator.state == 1) {
state = state + "\nElevator Moving.";
}
if (this.elevator.state == 2) {
state = state + "\nElevator E-Stopped.";
}
// state = state+"\nShooter Power Level (Raw):
// "+this.shooter.calculateShooterSpeed();
// state = state+"\nShooter Power Level (Setting): "+this.shooter.shooterSpeedState;
// if (this.integ.globalState==1){
// state = state+"\nMoving Shooter Up To Shoot.";
// }if (this.integ.globalState==2){
// state = state+"\nShooting!";
// }if (this.integ.globalState2LOCK){
// state = state+"\nREALLY SHOOTING!.";
// }
state = state + "\nNumber Of Discs: " + this.elevator.numDiscs;
state =
state
+ "\n Louis + Bobni Are Amazing (Hiding This Message Is ILLEGAL! IT WILL STOP THE ROBOT!)";
SmartDashboard.putString("State", state);
SmartDashboard.putBoolean("Intake", this.intake.toggle.get());
}
}
// Called once after isFinished returns true
protected void end() {
launcher.retract();
display.println(DriverStationLCD.Line.kUser2, 1, "Pass command ended " + counter + " ");
display.updateLCD();
}
/** This function is called periodically during operator control */
public void teleopPeriodic() {
lights.set(Relay.Value.kForward);
// tracking.run();
/*try {
readString = serial.readString();
readString = readString.substring(readString.indexOf("data{"), readString.indexOf("}"));
xTurn = readString.substring(readString.indexOf("x("), readString.indexOf(")"));
xTurnVal = Integer.parseInt(xTurn);
} catch (VisaException ex) {
System.out.println("Error Reading string - " + ex);
}*/
y = drive_control.getRawAxis(1);
x = drive_control.getRawAxis(2);
t = drive_control.getRawAxis(3);
if (y > 1) y = 1;
else if (y < -1) y = -1;
if (x > 1) x = 1;
else if (x < -1) x = -1;
drive.setDriveJ(x, y, t);
/*if(drive_control.getRawButton(12))
//wheelsOn = true;
else if(drive_control.getRawButton(11));
wheelsOn = false;*/
/*try{
launcher.setX((drive_control.getRawAxis(4)-1)/2);
}catch(Exception e){}*/
// System.out.println((drive_control.getRawAxis(4)-1)*-1350);
if (!autoWheels) {
csc.setSetpoint((drive_control.getRawAxis(4) - 1) * -1500);
try {
// launcher.setX((drive_control.getRawAxis(4)+1)*2000);
System.out.println(
-launcher.getSpeed()
+ " -- "
+ csc.tmpResult
+ " -- "
+ (drive_control.getRawAxis(4) - 1) * -1500);
} catch (Exception e) {
System.out.println(e);
}
if (drive_control.getRawButton(11)) autoWheels = true;
} else {
csc.setSetpoint(2300);
if (drive_control.getRawButton(11)) autoWheels = false;
}
/*if(wheelsOn)
shoot(1);
else
shoot(0);*/
if (drive_control.getRawButton(5)) {
wedge.moveDown();
// servo1.set(1);
// servo2.set(0);
// wedge.set(Relay.Value.kForward);
} else if (drive_control.getRawButton(6)) {
wedge.moveUp();
// servo1.set(0);
// servo2.set(1);
// wedge.set(Relay.Value.kReverse);
} else {
// servo1.set(.5);
// servo2.set(.5);
// wedge.set(Relay.Value.kOff);
}
if (drive_control.getRawButton(3)) turret.set(.5);
else if (drive_control.getRawButton(4)) turret.set(-.5);
else turret.set(0);
if (drive_control.getRawButton(2)) driveMainBelt(true, true);
else if (drive_control.getRawButton(7)) driveMainBelt(false, true);
else driveMainBelt(false, false);
if (drive_control.getTrigger()) {
try {
belt2.setX(-1);
} catch (Exception e) {
}
} else if (drive_control.getRawButton(8)) {
try {
belt2.setX(1);
} catch (Exception e) {
}
} else {
try {
belt2.setX(0);
} catch (Exception e) {
}
}
if (control.getRawButton(3)) {
turret.set(.25);
} else if (control.getRawButton(4)) {
turret.set(-.25);
} else {
turret.set(0);
}
try {
station.println(
DriverStationLCD.Line.kMain6, 1, "RPM: " + String.valueOf(-launcher.getSpeed()));
} catch (CANTimeoutException ex) {
}
/*try{
if(tracking.foundTarget()){
station.println(DriverStationLCD.Line.kUser2, 1, "Distance: " + String.valueOf(tracking.getTarget("HIGH").distance));
station.println(DriverStationLCD.Line.kUser3, 1, "Suggested RPM: " + String.valueOf(tracking.getTarget("HIGH").distance*62.8));
if(x>0)
station.println(DriverStationLCD.Line.kUser4, 1, "Rotation: <--");
else if(x<0)
station.println(DriverStationLCD.Line.kUser4, 1, "Roation: -->");
else
station.println(DriverStationLCD.Line.kUser4, 1, "I don't need to rotate");
}
else{
station.println(DriverStationLCD.Line.kUser2, 1, "Help, I can't find target by myself!");
station.println(DriverStationLCD.Line.kUser3, 1, "");
station.println(DriverStationLCD.Line.kUser4, 1, "");
}
}catch(Exception e){}*/
station.updateLCD();
}