Permalink
Browse files

WIP teleop conversion to iterative

  • Loading branch information...
Linnea
Linnea committed Oct 12, 2018
1 parent d9ca040 commit b767227f270445273e39ee17fa8560ae4c930dfc
@@ -121,33 +121,13 @@ public void autonomousPeriodic() {

@Override
public void teleopInit() {
// This makes sure that the autonomous stops running when
// teleop starts running. If you want the autonomous to
// continue until interrupted by another command, remove
// this line or comment it out.
// DriverStation.getInstance().reportWarning("1", false);
if (autonomousCommand != null) {

// DriverStation.getInstance().reportWarning("2", false);
autonomousCommand.cancel();
// DriverStation.getInstance().reportWarning("3", false);
}
// DriverStation.getInstance().reportWarning("4", false);
System.out.println("\nTELEOPINIT!!\n");
Scheduler.getInstance().add(new GrabIntakeCommand(true));
Scheduler.getInstance().add(new RunIntakeCommand());
driveSubsystem.resetEncoders();
Scheduler.getInstance().add(new ElevatorMoveCommand());
// DriverStation.getInstance().reportWarning("TeleopInit!", false);
// new ElevatorUpCommand(); //ElevatorUp currently used as coJoy speed control
}

/**
* This function is called periodically during operator control
*/
@Override
public void teleopPeriodic() {
Scheduler.getInstance().run();

// INTAKE
OI.foldUp.whenPressed(new FoldIntakeCommand(true));
@@ -164,12 +144,10 @@ public void teleopPeriodic() {
OI.releaseIntakeSlow.whileHeld(new ReleaseIntakeCommand(-0.4));
OI.releaseIntakeSlow.whenReleased(new RunIntakeCommand());
OI.releaseIntakeSlow.whenReleased(new GrabIntakeCommand(true));

// ELEVATOR
OI.elevatorSwitch.whenPressed(new ElevatorToInchesCommand(elevatorSubsystem.switchHeight));
OI.elevatorScale.whenPressed(new ElevatorToInchesCommand(elevatorSubsystem.scaleHeight));
OI.elevatorReset.whenPressed(new ElevatorResetCommand());
// OI.elevatorOverride.whenPressed(new ElevatorMoveCommand());

// EXECUTE
DriveSubsystem.driverControl();
ElevatorSubsystem.driverControl();
}

/**
@@ -10,9 +10,8 @@
/**
*
*/
public class DriveCommand extends Command {
public class DriveCommand {
public DriveCommand() {
requires(Robot.driveSubsystem);
}

// Called just before this Command runs the first time
@@ -18,7 +18,7 @@
*
*/
// ITERATIVE CONVERSION NOTE: This class doesn't need to extend anything
public class DriveSubsystem extends Subsystem {
public class DriveSubsystem{

// AUTO CONSTANTS
public static double inchesToTicks = 11.94; // 40 ticks/in. * 54/20*50/12 gearbox reduction / (12pi in/shaft rotation)
@@ -72,10 +72,6 @@ public DriveSubsystem() {
RE.setReverseDirection(true);
}

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

public void driveTank(double leftSpeed, double rightSpeed) {
LF.set(ControlMode.PercentOutput, -leftSpeed);
@@ -122,6 +118,15 @@ public boolean nearZero(double number, double tolerance) {
public Double getVelocity() {
return (LE.getRate()+RE.getRate())/2;
}

public void driverControl() {
double leftSpeed = OI.leftJoy.getRawAxis(1) /2.0 ;
double rightSpeed = OI.rightJoy.getRawAxis(1) /2.0 ;
String LencVal = Double.toString(getLeftEncoderValue());
String Rencval = Double.toString(getRightEncoderValue());
driveTank(leftSpeed, rightSpeed);
DriverStation.getInstance().reportWarning("LEFT ENCODER: " + LencVal, false);
}


}
@@ -17,7 +17,7 @@
*
*/
// ITERATIVE CONVERSION NOTE: This class doesn't need to extend anything
public class ElevatorSubsystem extends Subsystem {
public class ElevatorSubsystem {

// Put methods for controlling this subsystem
TalonSRX EM = RobotMap.elevatorMotor;
@@ -38,11 +38,6 @@ public ElevatorSubsystem() {
enc.reset();
enc.setDistancePerPulse(0.0645);
}

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

public void elevatorMove(double speed) {
EM.set(ControlMode.PercentOutput, speed);
@@ -67,6 +62,10 @@ public void resetEncoder() {
public double getVelocity() {
return enc.getRate();
}

public void driverControl(){
elevatorMove(-OI.coJoy.getRawAxis(1));
}

}

@@ -18,7 +18,7 @@
*
*/
// ITERATIVE CONVERSION NOTE: This class doesn't need to extend anything
public class IntakeSubsystem extends Subsystem {
public class IntakeSubsystem {

public TalonSRX leftMotor = RobotMap.leftIntakeMotor;
public TalonSRX rightMotor = RobotMap.rightIntakeMotor;

0 comments on commit b767227

Please sign in to comment.