This is my first time using Java to program a robot. Our team has just finished putting together the robot and we tried to drive it today. The robot did not move a single inch. I am sure that the wiring are correct. There must be something wrong about this. I need some help on fixing this
. Thanks!
Code:
package edu.wpi.first.wpilibj.templates;
import edu.wpi.first.wpilibj.Compressor;
import edu.wpi.first.wpilibj.DoubleSolenoid;
import edu.wpi.first.wpilibj.DriverStationLCD;
import edu.wpi.first.wpilibj.IterativeRobot;
import edu.wpi.first.wpilibj.Jaguar;
import edu.wpi.first.wpilibj.Joystick;
import edu.wpi.first.wpilibj.RobotDrive;
import edu.wpi.first.wpilibj.SpeedController;
public class UltimateAscends extends IterativeRobot {
//Declare Joysticks.
private Joystick leftStick;
private Joystick rightStick;
private Joystick shootStick;
//Declare Speed Controllers for Drive Motors.
private SpeedController m_frontLeftMotor;
private SpeedController m_rearLeftMotor;
private SpeedController m_frontRightMotor;
private SpeedController m_rearRightMotor;
//Declare a RobotDrive.
private RobotDrive drive;
//Declare Pneumatics.
private Compressor compressor;
private DoubleSolenoid doublePiston;
//Declare and initialize the Driver Station Output.
private DriverStationLCD lcd = DriverStationLCD.getInstance();
/**
* Called once when the robot is first turned on.
*/
public void robotInit() {
joystickInit(); //Joysticks initialized here.
jaguarInit(); //Jaguars initialized here. Modify this if use Victors!!!!!!!!!!!!!
robotDriveInit(); //RobotDrive initialized here. No need to modify this.
invertMotor(); //Invert the motors if needed.
startCompressor(); //Start the Compressor when the robot is turned on.
getWatchdog().setExpiration(0.1); //The watchdog can be neglected for .1 seconds before it starves to death.
lcd.println(DriverStationLCD.Line.kUser2, 1, "Enabled! ");
lcd.updateLCD();
}
/**
* Joysticks initialized here.
*/
public void joystickInit() {
leftStick = new Joystick(1); //Left Joystick connected to port 1.
rightStick = new Joystick(2); //Right Joystick connected to port 2.
shootStick = new Joystick(3); //Shoot Joystick connected to port 3.
}
/**
* Jaguars initialized here. Modify this if use Victors!!!!!!!!!!!!!
*/
public void jaguarInit() {
m_frontLeftMotor = new Jaguar(1); //Front Left Motor plugged in to PWM channel 1 on Digital Sidecar through a Jaguar.
m_rearLeftMotor = new Jaguar(2); //Rear Left Motor plugged in to PWM channel 2 on Digital Sidecar through a Jaguar.
m_frontRightMotor = new Jaguar(3); //Front Right Motor plugged in to PWM channel 3 on Digital Sidecar through a Jaguar.
m_rearRightMotor = new Jaguar(4); //Rear Right Motor plugged in to PWM channel 4 on Digital Sidecar through a Jaguar.
}
/**
* RobotDrive initialized here. No need to modify this.
*/
public void robotDriveInit() {
drive = new RobotDrive(m_frontLeftMotor, m_rearLeftMotor, m_frontRightMotor, m_rearRightMotor);
}
/**
* Invert the motors if needed.
*/
public void invertMotor() {
drive.setInvertedMotor(RobotDrive.MotorType.kFrontLeft, false); //The Front Left Motor is not inverted.
drive.setInvertedMotor(RobotDrive.MotorType.kFrontRight, false); //The Rear Left Motor is not inverted.
drive.setInvertedMotor(RobotDrive.MotorType.kRearLeft, false); //The Front Right Motor is not inverted.
drive.setInvertedMotor(RobotDrive.MotorType.kRearRight, false); //The Rear Right Motor is not inverted.
}
/**
* Initialize the compressor, pistons and relay and start the compressor
* when the robot is turned on.
*/
public void startCompressor() {
compressor = new Compressor(1, 1); //The pressure switch is attached to GPIO channel 1. The compressor relay is attached to relay channel 1
doublePiston = new DoubleSolenoid(1, 2); //the forward piston is attached to channel 1 and the reverse piston is attached to channel 2 on the Solenoid Breakout.
compressor.start(); //Start the compressor.
lcd.println(DriverStationLCD.Line.kUser3, 1, "The Compressor is started! ");
lcd.updateLCD();
}
public void disabledInit() {
lcd.println(DriverStationLCD.Line.kUser2, 1, "Disabled! ");
lcd.updateLCD();
}
public void disabledContinuous() {
//Do nothing.
}
public void disabledPeriodic() {
//Do nothing.
}
public void autonomousInit() {
getWatchdog().setEnabled(false); //Disables the watchdog for the entire Autonomous period.
lcd.println(DriverStationLCD.Line.kUser2, 1, "Autonomous! ");
lcd.updateLCD();
}
/**
* This function is called once each time the robot enters autonomous mode.
*/
public void autonomousContinuous() {
while (isAutonomous() && isEnabled()) {
//Add Autonomous Code Here!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!
}
}
public void autonomousPeriodic() {
//Do nothing.
}
public void teleopInit() {
getWatchdog().setEnabled(true); //Enables the watchdog for the entire Teleoperated period.
lcd.println(DriverStationLCD.Line.kUser2, 1, "Teleop! ");
lcd.updateLCD();
}
/**
* This function is called once each time the robot enters operator control.
*/
long x = 1;
public void teleopContinuous() {
while (isOperatorControl() && isEnabled()) {
//Drives the robot holonomically using joysticks for teleop.
drive.mecanumDrive_Polar(leftStick.getMagnitude(), leftStick.getDirectionDegrees(), rightStick.getX());
//Pushes the pistons forward using button 1 and backward using button 2 on the Shoot Stick.
if (shootStick.getRawButton(1)) {
doublePiston.set(DoubleSolenoid.Value.kForward); //Botton 1 of the Shoot Stick moves the pistons forward.
lcd.println(DriverStationLCD.Line.kUser4, 1, "The Pistons are moving forward! ");
lcd.updateLCD();
} else if (shootStick.getRawButton(2)) {
doublePiston.set(DoubleSolenoid.Value.kReverse); //Button 2 of the Shoot Stick moves the pistons backward.
lcd.println(DriverStationLCD.Line.kUser4, 1, "The Pistons are moving backward! ");
lcd.updateLCD();
} else {
doublePiston.set(DoubleSolenoid.Value.kOff); //The pistons does not do anything while no buttons on the Shhot Stick are pushed.
lcd.println(DriverStationLCD.Line.kUser4, 1, "The Pistons are turned off! ");
lcd.updateLCD();
}
//Stops the compressor using button 10 on the Shoot Stick.
if (shootStick.getRawButton(10)) {
compressor.stop();
lcd.println(DriverStationLCD.Line.kUser3, 1, "The Compressor is stopped! ");
lcd.updateLCD();
} else if (shootStick.getRawButton(11)) {
compressor.start();
lcd.println(DriverStationLCD.Line.kUser3, 1, "The Compressor is started! ");
lcd.updateLCD();
} else {
//Do nothing.
continue;
}
lcd.println(DriverStationLCD.Line.kUser5, 1, "" + x);
lcd.updateLCD();
x++;
}
}
public void teleopPeriodic() {
//Do nothing.
}
}