/** This function is called periodically during test mode */
public void testPeriodic() {
jag1.set(0);
jag2.set(0);
jag3.set(0);
jag4.set(0);
System.out.println(compressor.getPressureSwitchValue());
}
protected void usePIDOutput(double output) {
double speed = shootingMotor.get() + output;
if (speed > .75 && this.getSetpoint() < 2600) {
speed = .7;
} else if (speed > .85 && this.getSetpoint() < 3600) {
speed = .8;
} else if (speed < 0) {
speed = 0;
}
shooterSet(speed);
}
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 void autoForward() {
jag1.set(-AUTO_MOVE_FORWARD_SPEED); // - (gyro.getAngle()/AUTO_GYRO_REDUCTION));
jag2.set(-AUTO_MOVE_FORWARD_SPEED); // - (gyro.getAngle()/AUTO_GYRO_REDUCTION));
jag3.set(AUTO_MOVE_FORWARD_SPEED); // - (gyro.getAngle()/AUTO_GYRO_REDUCTION));
jag4.set(AUTO_MOVE_FORWARD_SPEED); // - (gyro.getAngle()/AUTO_GYRO_REDUCTION));
}
public void stop() {
jag1.set(0);
jag2.set(0);
jag3.set(0);
jag4.set(0);
}
public void run() {
/*
* Stream Names
*
* Voltage Output: "CANJAGUAROV"
* Current: "CANJAGUARI"
* Voltage From Battery: "CANJAGUARIV"
* Tempature: "CANJAGUART"
*/
while (true) {
for (int i = 0; i < jaguars.size(); i++) {
Jaguar j = ((Jaguar) jaguars.elementAt(i));
dataStreamingModule.sendPacket(j.getBusVoltage(), "CANJAGUARIV" + j.getID());
dataStreamingModule.sendPacket(j.getOutputVoltage(), "CANJAGUAROV" + j.getID());
dataStreamingModule.sendPacket(j.getOutputCurrent(), "CANJAGUARI" + j.getID());
dataStreamingModule.sendPacket(j.getTemperature(), "CANJAGUART" + j.getID());
dataStreamingModule.sendPacket(j.getX(), "CANJAGUARX" + j.getID());
}
try {
Thread.sleep((long) (1000d / hz));
} catch (InterruptedException ex) {
}
}
}
public void shooterSet(double speed) {
shootingMotor.set(speed);
shootingMotor2.set(speed);
}
public void stopShooter() {
shootingMotor.set(0);
shootingMotor2.set(0);
}
/** This function is called periodically during autonomous */
public void autonomousPeriodic() {
red.set(1);
black.set(1);
}