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();
}
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();
}
/** 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 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();
}
/** 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();
}
// Called once after isFinished returns true
protected void end() {
launcher.retract();
display.println(DriverStationLCD.Line.kUser2, 1, "Pass command ended " + counter + " ");
display.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();
}