diff --git a/.classpath b/.classpath index 294130b..9d41f0e 100644 --- a/.classpath +++ b/.classpath @@ -7,6 +7,7 @@ - + + diff --git a/bin/.gitignore b/bin/.gitignore deleted file mode 100644 index cf1db2e..0000000 --- a/bin/.gitignore +++ /dev/null @@ -1 +0,0 @@ -/org/ diff --git a/src/org/usfirst/frc/team334/robot/Constants.java b/src/org/usfirst/frc/team334/robot/Constants.java index bf4791e..d6b0a24 100644 --- a/src/org/usfirst/frc/team334/robot/Constants.java +++ b/src/org/usfirst/frc/team334/robot/Constants.java @@ -25,10 +25,14 @@ public class Constants { public static final int ELEVATOR_TO_EXCHANGE_BUTTON = 4; public static final int COLLAPSE_ELEVATOR_BUTTON = 5; - // Motors - public static final int DRIVETRAIN_LEFT = 0; - public static final int DRIVETRAIN_RIGHT = 1; - + // DriveTrain + public static final int DRIVETRAIN_MC_L = 5; + public static final int DRIVETRAIN_C1_L = 3; + public static final int DRIVETRAIN_C2_L = 4; + public static final int DRIVETRAIN_MC_R = 0; + public static final int DRIVETRAIN_C1_R = 1; + public static final int DRIVETRAIN_C2_R = 2; + // Pancake Solenoids - Drivetrain public static final int PANCAKE_L_INPUT = 0; public static final int PANCAKE_L_OUTPUT = 1; diff --git a/src/org/usfirst/frc/team334/robot/subsystems/Drive.java b/src/org/usfirst/frc/team334/robot/subsystems/Drive.java index 51d6257..ec6635d 100644 --- a/src/org/usfirst/frc/team334/robot/subsystems/Drive.java +++ b/src/org/usfirst/frc/team334/robot/subsystems/Drive.java @@ -1,11 +1,14 @@ package org.usfirst.frc.team334.robot.subsystems; +import java.util.ArrayList; + import org.usfirst.frc.team334.robot.Constants; import org.usfirst.frc.team334.robot.pids.BNO055; +import com.ctre.phoenix.motorcontrol.can.TalonSRX; + import edu.wpi.first.wpilibj.Encoder; import edu.wpi.first.wpilibj.Ultrasonic; -import edu.wpi.first.wpilibj.VictorSP; import edu.wpi.first.wpilibj.command.Subsystem; /** @@ -31,13 +34,24 @@ public enum DriveControlState { public static Ultrasonic rUltrasonicL = new Ultrasonic(Constants.ULTRASONIC_R_DRIVETRAIN_PING, Constants.ULTRASONIC_R_DRIVETRAIN_ECHO); public static BNO055 rGyro = BNO055.getInstance(BNO055.opmode_t.OPERATION_MODE_IMUPLUS, BNO055.vector_type_t.VECTOR_EULER); - private VictorSP left; - private VictorSP right; + private TalonSRX miniCimL,miniCimR,cim1L,cim2L,cim1R,cim2R; + + ArrayList left,right; public Drive() { - left = new VictorSP(Constants.DRIVETRAIN_LEFT); - right = new VictorSP(Constants.DRIVETRAIN_RIGHT); - right.setInverted(true); + miniCimL = new TalonSRX(Constants.DRIVETRAIN_MC_L); + cim1L = new TalonSRX(Constants.DRIVETRAIN_C1_L); + cim2L = new TalonSRX(Constants.DRIVETRAIN_C2_L); + miniCimR = new TalonSRX(Constants.DRIVETRAIN_MC_R); + cim1R = new TalonSRX(Constants.DRIVETRAIN_C1_R); + cim2R = new TalonSRX(Constants.DRIVETRAIN_C2_R); + + left.add(miniCimL); + left.add(cim1L); + left.add(cim2L); + right.add(miniCimR); + right.add(cim1R); + right.add(cim2R); } public void stop() { @@ -46,15 +60,19 @@ public void stop() { } public void setLeft(double speed) { - left.set(speed); + for(TalonSRX TSRX : left) { + TSRX.set(com.ctre.phoenix.motorcontrol.ControlMode.Current, speed); + } } public void setRight(double speed) { - right.set(speed); + for(TalonSRX TSRX : right) { + TSRX.set(com.ctre.phoenix.motorcontrol.ControlMode.Current, speed); + } } public void initDefaultCommand() { } -} +} \ No newline at end of file