Issue with Servo and Motor Program

[LFRobotics]

I am a beginner, first year programmer and I wrote a couple programs in CommandBase java and they have a few errors. I do have the robot driving.

One of them was supposed to have two commands defined by a subsystem - one command would move a servo 45 degrees away from its default location and the second one would move the servo back to its default location. When I press the buttons assigned to complete these tasks nothing happens.

Here it is:

Subsystem(called LaunchServo):

package edu.wpi.first.LFRobot2014.subsystems;

import edu.wpi.first.LFRobot2014.RobotMap;
import edu.wpi.first.wpilibj.Servo;
import edu.wpi.first.wpilibj.command.Subsystem;

/**
*
* @author FrankyMonezz
*/
public class LaunchServo extends Subsystem {
// Put methods for controlling this subsystem
// here. Call these from Commands.

Servo servo = new Servo(RobotMap.LaunchServoPort);

public void initDefaultCommand() {
// Set the default command for a subsystem here.
//setDefaultCommand(new MySpecialCommand());
}

public void setPos(double pos){
servo.set(pos);
}

public double getPos(){
double pos = servo.get();
return pos;
}}

Command to Move it to 45(called PrepBall):

package edu.wpi.first.LFRobot2014.commands;

/**
*
* @author FrankyMonezz
*/
public class PrepBall extends CommandBase {

public PrepBall() {
// Use requires() here to declare subsystem dependencies
// eg. requires(chassis);
requires(launchservo);
}

// Called just before this Command runs the first time
protected void initialize() {
}

// Called repeatedly when this Command is scheduled to run
protected void execute() {
//move servo to 45 degrees
launchservo.setPos(45);
}

// Make this return true when this Command no longer needs to run execute()
protected boolean isFinished() {
if(launchservo.getPos() == 45){

return true;
}
else {
return false;
}
}

// Called once after isFinished returns true
protected void end() {
}

// Called when another command which requires one or more of the same
// subsystems is scheduled to run
protected void interrupted() {
}
}

Command to Move back to Default(called PrepBall2):

package edu.wpi.first.LFRobot2014.commands;

/**
*
* @author FrankyMonezz
*/
public class PrepBall2 extends CommandBase {

public PrepBall2() {
// Use requires() here to declare subsystem dependencies
// eg. requires(chassis);
requires(launchservo);
}

// Called just before this Command runs the first time
protected void initialize() {
}

// Called repeatedly when this Command is scheduled to run
protected void execute() {
//move servo back to default location
launchservo.setPos(0);
}

// Make this return true when this Command no longer needs to run execute()
protected boolean isFinished() {
if(launchservo.getPos() == 0){
return true;
}
else {
return false;
}
}

// Called once after isFinished returns true
protected void end() {
}

// Called when another command which requires one or more of the same
// subsystems is scheduled to run
protected void interrupted() {
}
}

Port Assignment:

public static final int
LaunchServoPort = 5;

CommandBase(subsystem is called LaunchServo):

public abstract class CommandBase extends Command {

public static OI oi;
// Create a single static instance of all of your subsystems
public static ForkMotor forkmotor = new ForkMotor();
public static ArmMotor armmotor = new ArmMotor();
public static Chassis chassis = new Chassis();
public static LaunchServo launchservo = new LaunchServo();

Button Assignments(PrepBall - moves it to 45 -- PrepBall2 - moves it to default):

public Joystick
rightStick,
leftStick;

private final Button ForksDown, ForksUp, prepball, prepball2, motor1, motor2; //add Auto



public OI() {

rightStick = new Joystick(RobotMap.rightJoystickPort);
leftStick = new Joystick(RobotMap.leftJoystickPort);
ForksDown = new JoystickButton(rightStick, 5);
ForksUp = new JoystickButton(rightStick, 4);
prepball = new JoystickButton(leftStick, 4);
prepball2 = new JoystickButton(leftStick, 5);
motor1 = new JoystickButton(rightStick, 2);
motor2 = new JoystickButton(rightStick, 3);

//Auto = new JoystickButton(leftStick, 4);

ForksUp.whileHeld(new LiftForks());
ForksDown.whileHeld(new LowerForks());
prepball.whenPressed(new PrepBall());
prepball2.whenPressed(new PrepBall2());
motor1.whenPressed(new ArmLauncher());
motor2.whenPressed(new ReleaseTension());
//Auto.whenPressed(new AutoCommandGroup());
}

public Joystick getRightStick() {
return rightStick;
}

public Joystick getLeftStick() {
return leftStick;
}

}

The other one had two commands defined by a subsystem called ForkMotor. One command was supposed to move a motor forward(1) while a button was pressed. The other was supposed to move a motor backwards(-1) while a button is pressed. When I hold the button down that is supposed to move it forward the motor glitches off and on but does stop when I let go - the motor is fine I tested it. The button that SHOULD turn it in the other direction doesn't work at all

Here it is:

Subsystem(called ForkMotor):

package edu.wpi.first.LFRobot2014.subsystems;

import edu.wpi.first.LFRobot2014.RobotMap;
import edu.wpi.first.wpilibj.Jaguar;
import edu.wpi.first.wpilibj.SpeedController;
import edu.wpi.first.wpilibj.command.Subsystem;

/**
*
* @author FrankyMonezz
*/
public class ForkMotor extends Subsystem {
// Put methods for controlling this subsystem
// here. Call these from Commands.
SpeedController motor = new Jaguar(RobotMap.ForkMotorPort);

public void initDefaultCommand() {
// Set the default command for a subsystem here.
//setDefaultCommand(new MySpecialCommand());
}

public void up() {
motor.set(1);
}

public void down() {
motor.set(-1);
}

public void stop(){
motor.set(0);
}
}

First Command(to move it forward -- called LiftForks):

package edu.wpi.first.LFRobot2014.commands;


/**
*
* @author FrankyMonezz
*/
public class LiftForks extends CommandBase {

public LiftForks() {
// Use requires() here to declare subsystem dependencies
// eg. requires(chassis);
requires(forkmotor);
setTimeout(.5);
}

// Called just before this Command runs the first time
protected void initialize() {
}

// Called repeatedly when this Command is scheduled to run
protected void execute() {
forkmotor.up();
}

// Make this return true when this Command no longer needs to run execute()
protected boolean isFinished() {
return isTimedOut();
}

// Called once after isFinished returns true
protected void end() {
forkmotor.stop();
}

// Called when another command which requires one or more of the same
// subsystems is scheduled to run
protected void interrupted() {
end();
}
}

Second Command(to move it in reverse -- called LowerForks):

package edu.wpi.first.LFRobot2014.commands;

/**
*
* @author FrankyMonezz
*/
public class LowerForks extends CommandBase {

public LowerForks() {
// Use requires() here to declare subsystem dependencies
// eg. requires(chassis);
requires(forkmotor);
setTimeout(.5);
}

// Called just before this Command runs the first time
protected void initialize() {
}

// Called repeatedly when this Command is scheduled to run
protected void execute() {
forkmotor.down();
}

// Make this return true when this Command no longer needs to run execute()
protected boolean isFinished() {
return isTimedOut();
}

// Called once after isFinished returns true
protected void end() {
forkmotor.stop();
}

// Called when another command which requires one or more of the same
// subsystems is scheduled to run
protected void interrupted() {
end();
}
}

Port Assignment(called ForkMotorPort):

public static final int
leftMotorPort = 1,
rightMotorPort = 2,
ArmMotorPort = 3,
ForkMotorPort = 4;

CommandBase(called ForkMotor):

public abstract class CommandBase extends Command {

public static OI oi;
// Create a single static instance of all of your subsystems
public static ForkMotor forkmotor = new ForkMotor();
public static ArmMotor armmotor = new ArmMotor();
public static Chassis chassis = new Chassis();
public static LaunchServo launchservo = new LaunchServo();

Button Assignments(LiftForks - moves it forward(1) -- LowerForks - moves it in reverse(-1):

public Joystick
rightStick,
leftStick;

private final Button ForksDown, ForksUp, prepball, prepball2, motor1, motor2; //add Auto



public OI() {

rightStick = new Joystick(RobotMap.rightJoystickPort);
leftStick = new Joystick(RobotMap.leftJoystickPort);
ForksDown = new JoystickButton(rightStick, 5);
ForksUp = new JoystickButton(rightStick, 4);
prepball = new JoystickButton(leftStick, 4);
prepball2 = new JoystickButton(leftStick, 5);
motor1 = new JoystickButton(rightStick, 2);
motor2 = new JoystickButton(rightStick, 3);

//Auto = new JoystickButton(leftStick, 4);

ForksUp.whileHeld(new LiftForks());
ForksDown.whileHeld(new LowerForks());
prepball.whenPressed(new PrepBall());
prepball2.whenPressed(new PrepBall2());
motor1.whenPressed(new ArmLauncher());
motor2.whenPressed(new ReleaseTension());
//Auto.whenPressed(new AutoCommandGroup());
}

public Joystick getRightStick() {
return rightStick;
}

public Joystick getLeftStick() {
return leftStick;
}

}

ANY help WILL be appreciated - THANKS!