Smart talon class

LICENSE

@@ -1,21 +1,21 @@
-MIT License
-
-Copyright (c) 2017 Team 2342
-
-Permission is hereby granted, free of charge, to any person obtaining a copy
-of this software and associated documentation files (the "Software"), to deal
-in the Software without restriction, including without limitation the rights
-to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
-copies of the Software, and to permit persons to whom the Software is
-furnished to do so, subject to the following conditions:
-
-The above copyright notice and this permission notice shall be included in all
-copies or substantial portions of the Software.
-
-THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
-IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
-FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
-AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
-LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
-OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
-SOFTWARE.
+MIT License
+
+Copyright (c) 2017 Team 2342
+
+Permission is hereby granted, free of charge, to any person obtaining a copy
+of this software and associated documentation files (the "Software"), to deal
+in the Software without restriction, including without limitation the rights
+to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
+copies of the Software, and to permit persons to whom the Software is
+furnished to do so, subject to the following conditions:
+
+The above copyright notice and this permission notice shall be included in all
+copies or substantial portions of the Software.
+
+THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
+AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
+LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
+OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
+SOFTWARE.

src/org/usfirst/frc/team2342/PIDLoops/DistancePIDController.java

@@ -1,15 +1,15 @@
 package org.usfirst.frc.team2342.PIDLoops;
 
-import com.ctre.phoenix.motorcontrol.can.WPI_TalonSRX;
+import org.usfirst.frc.team2342.util.SmartTalon;
 
 import edu.wpi.first.wpilibj.PIDController;
 import edu.wpi.first.wpilibj.PIDOutput;
 import edu.wpi.first.wpilibj.PIDSource;
 import edu.wpi.first.wpilibj.PIDSourceType;
 
 public class DistancePIDController implements PIDOutput, PIDSource {
-	private WPI_TalonSRX left;
-	private WPI_TalonSRX right;
+	private SmartTalon left;
+	private SmartTalon right;
 	private double Kp = 0.0d;
 	private double Ki = 0.0d;
 	private double Kd = 0.0d;
@@ -18,7 +18,7 @@ public class DistancePIDController implements PIDOutput, PIDSource {
 	private PIDSourceType type = PIDSourceType.kDisplacement;
 	private double correction = 0.0d;
 
-	public DistancePIDController(double p, double i, double d, double ff, WPI_TalonSRX talonMasterLeft, WPI_TalonSRX talonMasterRight){
+	public DistancePIDController(double p, double i, double d, double ff, SmartTalon talonMasterLeft, SmartTalon talonMasterRight){
 		this.left = talonMasterLeft;
 		this.right = talonMasterRight;
 		this.Kp = p;

src/org/usfirst/frc/team2342/PIDLoops/SingleTalonDistancePIDController.java

@@ -1,14 +1,14 @@
 package org.usfirst.frc.team2342.PIDLoops;
 
-import com.ctre.phoenix.motorcontrol.can.WPI_TalonSRX;
+import org.usfirst.frc.team2342.util.SmartTalon;
 
 import edu.wpi.first.wpilibj.PIDController;
 import edu.wpi.first.wpilibj.PIDOutput;
 import edu.wpi.first.wpilibj.PIDSource;
 import edu.wpi.first.wpilibj.PIDSourceType;
 
 public class SingleTalonDistancePIDController implements PIDOutput, PIDSource {
-	private WPI_TalonSRX talon;
+	private SmartTalon talon;
 	private double Kp = 0.0d;
 	private double Ki = 0.0d;
 	private double Kd = 0.0d;
@@ -17,7 +17,7 @@ public class SingleTalonDistancePIDController implements PIDOutput, PIDSource {
 	private PIDSourceType type = PIDSourceType.kDisplacement;
 	private double correction = 0.0d;
 
-	public SingleTalonDistancePIDController(double p, double i, double d, double ff, WPI_TalonSRX talon){
+	public SingleTalonDistancePIDController(double p, double i, double d, double ff, SmartTalon talon){
 		this.talon= talon;
 		this.Kp = p;
 		this.Ki = i;

src/org/usfirst/frc/team2342/json/Talon.java

@@ -9,4 +9,5 @@ public class Talon {
 		public int mode;
 		public PIDGains velocityGains;
 		public PIDGains distanceGains;
+		public int deviceNumber;
 }

src/org/usfirst/frc/team2342/robot/Robot.java

@@ -17,15 +17,16 @@
 import org.usfirst.frc.team2342.robot.subsystems.WestCoastTankDrive;
 import org.usfirst.frc.team2342.util.Constants;
 import org.usfirst.frc.team2342.util.FMS;
+import org.usfirst.frc.team2342.util.SmartTalon;
 
 import com.ctre.phoenix.ParamEnum;
 import com.ctre.phoenix.motorcontrol.ControlMode;
-import com.ctre.phoenix.motorcontrol.can.WPI_TalonSRX;
 
 import edu.wpi.cscore.UsbCamera;
 import edu.wpi.cscore.VideoSink;
 import edu.wpi.first.wpilibj.IterativeRobot;
 import edu.wpi.first.wpilibj.Joystick;
+import edu.wpi.first.wpilibj.Solenoid;
 import edu.wpi.first.wpilibj.command.Command;
 import edu.wpi.first.wpilibj.command.Scheduler;
 import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
@@ -39,14 +40,18 @@ public class Robot extends IterativeRobot {
 
 	Joystick gamepad;
 	PCMHandler PCM;
-	WPI_TalonSRX talonFR;
-	WPI_TalonSRX talonFL;
-	WPI_TalonSRX talonBR;
-	WPI_TalonSRX talonBL;
-	WPI_TalonSRX talonCascade;
-	WPI_TalonSRX talonIntakeRight;
-	WPI_TalonSRX talonIntakeLeft;
-	WPI_TalonSRX talonTip;
+
+	SmartTalon talonFR;
+	SmartTalon talonFL;
+	SmartTalon talonBR;
+	SmartTalon talonBL;
+	SmartTalon talonCascade;
+	SmartTalon talonIntakeRight;
+	SmartTalon talonIntakeLeft;
+	SmartTalon talonTip;
+	//Solenoid solenoidLowGear = new Solenoid(Constants.PCM_CAN_ID ,Constants.PCM_SLOT_LOWGEAR);
+	//Solenoid solenoidHighGear = new Solenoid(Constants.PCM_CAN_ID ,Constants.PCM_SLOT_HIGHGEAR);
+	Solenoid solenoid1;
 
 	WestCoastTankDrive westCoast;
 	Joystick joystickR;
@@ -63,15 +68,21 @@ public class Robot extends IterativeRobot {
 	public Robot() {
 		gamepad = new Joystick(0);
 		PCM = new PCMHandler(11);
-		talonFR = new WPI_TalonSRX(Constants.RIGHT_MASTER_TALON_ID);
-		talonFL = new WPI_TalonSRX(Constants.LEFT_MASTER_TALON_ID);
-		talonBR = new WPI_TalonSRX(Constants.RIGHT_SLAVE_TALON_ID);
-		talonBL = new WPI_TalonSRX(Constants.LEFT_SLAVE_TALON_ID);
-		talonCascade = new WPI_TalonSRX(Constants.TALON_CASCADE);
-		talonIntakeRight = new WPI_TalonSRX(Constants.TALON_INTAKE_RIGHT);
-		talonIntakeLeft = new WPI_TalonSRX(Constants.TALON_INTAKE_LEFT);
-		talonTip = new WPI_TalonSRX(Constants.TALON_TIP);
+
+		talonFR = new SmartTalon(Constants.RIGHT_MASTER_TALON_ID);
+		talonFL = new SmartTalon(Constants.LEFT_MASTER_TALON_ID);
+		talonBR = new SmartTalon(Constants.RIGHT_SLAVE_TALON_ID);
+		talonBL = new SmartTalon(Constants.LEFT_SLAVE_TALON_ID);
+		talonCascade = new SmartTalon(Constants.TALON_CASCADE);
+		talonIntakeRight = new SmartTalon(Constants.TALON_INTAKE_RIGHT);
+		talonIntakeLeft = new SmartTalon(Constants.TALON_INTAKE_LEFT);
+		talonTip = new SmartTalon(Constants.TALON_TIP);
+		//Solenoid solenoidLowGear = new Solenoid(Constants.PCM_CAN_ID ,Constants.PCM_SLOT_LOWGEAR);
+		//Solenoid solenoidHighGear = new Solenoid(Constants.PCM_CAN_ID ,Constants.PCM_SLOT_HIGHGEAR);
+		//solenoid1 = new Solenoid(Constants.PCM_CAN_ID, Constants.PCM_BOX_MANIPULATOR);
+
 
+
 		westCoast = new WestCoastTankDrive(PCM, talonFL, talonFR, talonBL, talonBR);
 		joystickR = new Joystick(2);
 		XBOX = new Joystick(1);
@@ -185,6 +196,49 @@ else if(Math.abs(XBOX.getRawAxis(Constants.XBOX_RIGHTTRIGGER)) > 0.1) {
 			boxManipulator.talonIntakeRight.set(ControlMode.PercentOutput, 0);
 			boxManipulator.talonIntakeLeft.set(ControlMode.PercentOutput, 0);
 		}
+
+		/*Scheduler.getInstance().run();
+    	//Drive with joystick control in velocity mode
+		westCoast.outputToSmartDashboard();
+		//Buttons 8 & 9 or (gamepad) 5 & 6 are Low & High gear, respectively
+		if (gamepad.getRawButton(5))
+			westCoast.setLowGear();
+		else if (gamepad.getRawButton(6))
+			westCoast.setHighGear();
+		else
+			westCoast.setNoGear();
+		//Sleep for 0.01s
+		try {
+		    Thread.sleep(100);
+		} catch(InterruptedException ex) {
+		    Thread.currentThread().interrupt();
+		}
+		//teliopInity
+		if (joystick1.getRawButton(1)) {
+			talon1.goDistance(0.25, 0.4);
+			talon2.goDistance(-0.25, 0.4);
+			talon3.goDistance(0.25, 0.4);
+			talon4.goDistance(-0.25, 0.4);
+		}
+
+		// PCM.turnOn();
+		// SmartTalon talon1 = new SmartTalon(0);
+		// SmartTalon talon2 = new oO(1);
+		// boxManipulator = new BoxManipulator(talon1, talon2, PCM);
+		// cascadeElevator = new CascadeElevator(talon1, talon2);
+
+		 */	
+		/*Scheduler.getInstance().run();
+		try {
+			Thread.sleep(10);
+		} catch (InterruptedException e) {
+			// TODO Auto-generated catch block
+			e.printStackTrace();
+		}*/
+
+
+
+
 	}
 
 	public void disabledInit() {

src/org/usfirst/frc/team2342/robot/subsystems/BoxManipulator.java

@@ -1,21 +1,25 @@
 package org.usfirst.frc.team2342.robot.subsystems;
 
+
 import org.usfirst.frc.team2342.robot.PCMHandler;
+import org.usfirst.frc.team2342.util.SmartTalon;
 
 import com.ctre.phoenix.motorcontrol.ControlMode;
 import com.ctre.phoenix.motorcontrol.FeedbackDevice;
-import com.ctre.phoenix.motorcontrol.can.WPI_TalonSRX;
 
-import edu.wpi.first.wpilibj.Solenoid;
 import edu.wpi.first.wpilibj.Timer;
 import edu.wpi.first.wpilibj.command.Subsystem;
 import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
 
 public class BoxManipulator extends Subsystem {
-	public WPI_TalonSRX talonIntakeRight;
-	public WPI_TalonSRX talonIntakeLeft;
-	private WPI_TalonSRX talonTip;
+
+	public SmartTalon talonIntakeRight;
+	public SmartTalon talonIntakeLeft;
+	private SmartTalon talonTip;
+
+
 	private PCMHandler pcm;
+
 	Timer timer = new Timer();
 
 	public static final int PULL = 0;
@@ -27,7 +31,9 @@ public class BoxManipulator extends Subsystem {
 	private final boolean InvertMotor = false;
 
 
-	public BoxManipulator(WPI_TalonSRX talonIntakeRight, WPI_TalonSRX talonIntakeLeft, WPI_TalonSRX talonTip, PCMHandler pcm) {
+
+	public BoxManipulator(SmartTalon talonIntakeRight, SmartTalon talonIntakeLeft, SmartTalon talonTip, PCMHandler pCM2) {
+
 		this.talonIntakeRight = talonIntakeRight;
 		this.talonIntakeLeft = talonIntakeLeft;
 		this.talonTip = talonTip;

src/org/usfirst/frc/team2342/robot/subsystems/CascadeElevator.java

@@ -1,17 +1,17 @@
 package org.usfirst.frc.team2342.robot.subsystems;
 
 import org.usfirst.frc.team2342.util.Constants;
+import org.usfirst.frc.team2342.util.SmartTalon;
 
 import com.ctre.phoenix.motorcontrol.ControlMode;
 import com.ctre.phoenix.motorcontrol.FeedbackDevice;
-import com.ctre.phoenix.motorcontrol.can.WPI_TalonSRX;
 
 import edu.wpi.first.wpilibj.DigitalInput;
 import edu.wpi.first.wpilibj.command.Subsystem;
 import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
 
 public class CascadeElevator extends Subsystem {
-	public WPI_TalonSRX talonCascade;
+	public SmartTalon talonCascade;
 
 	public static final int BASE = 0;
 	public static final int SWITCH = 1;
@@ -26,7 +26,7 @@ public class CascadeElevator extends Subsystem {
 	public DigitalInput lowerLimit;
 	public DigitalInput upperLimit;
 
-	public CascadeElevator(WPI_TalonSRX talonCascade) {
+	public CascadeElevator(SmartTalon talonCascade) {
 		this.talonCascade = talonCascade;
 		this.talonCascade.setInverted(false);
 		talonCascade.configSelectedFeedbackSensor(FeedbackDevice.QuadEncoder, PidLoopIndex, PidTimeOutMs);
@@ -109,6 +109,7 @@ public void setVelocity(double speed) {
 			/*System.out.println("limit: " + talonCascade.getSensorCollection().isFwdLimitSwitchClosed() + 
 					" limitupper: " + talonCascade.getSensorCollection().isRevLimitSwitchClosed() + 
 					" position: " + talonCascade.getSelectedSensorPosition(PidLoopIndex));*/
+			System.out.println(speed);
 
 		} catch (Exception e) {
 

src/org/usfirst/frc/team2342/robot/subsystems/WestCoastTankDrive.java

@@ -7,25 +7,25 @@
 import org.usfirst.frc.team2342.robot.PCMHandler;
 import org.usfirst.frc.team2342.robot.TalonNWT;
 import org.usfirst.frc.team2342.util.Constants;
+import org.usfirst.frc.team2342.util.SmartTalon;
 
 import com.ctre.phoenix.motorcontrol.ControlMode;
 import com.ctre.phoenix.motorcontrol.FeedbackDevice;
-import com.ctre.phoenix.motorcontrol.can.WPI_TalonSRX;
 
 import edu.wpi.first.wpilibj.command.Subsystem;
 import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
 
 public class WestCoastTankDrive extends Subsystem {
 
-	private WPI_TalonSRX leftA, rightA, leftB, rightB;
+	private SmartTalon leftA, rightA, leftB, rightB;
 	private PCMHandler m_PCM;
 	public DistancePIDController dpidc;
 	public GyroPIDController pidc;       // GyroPIDController
 	public boolean debug = false;        // debug messages
 	private boolean gyroControl = false; // gyro Control
 	private boolean isLeftInner = false; // is the left wheel inner
 
-	public WestCoastTankDrive(PCMHandler PCM, WPI_TalonSRX leftFR, WPI_TalonSRX rightFR, WPI_TalonSRX leftBA, WPI_TalonSRX rightBA) {
+	public WestCoastTankDrive(PCMHandler PCM, SmartTalon leftFR, SmartTalon rightFR, SmartTalon leftBA, SmartTalon rightBA) {
 		//Json config = JsonHelpe.getConfig();
 		m_PCM = PCM;
 		leftA = leftFR;
@@ -137,14 +137,12 @@ public void setPercentage(double left, double right) {
 
 	public void goDistance(double distanceInFeet){
 		double distance = distanceInFeet/Constants.TALON_RPS_TO_FPS * Constants.TALON_TICKS_PER_REV;
-		dpidc.setGoal(distance);
-		if (!leftA.getControlMode().equals(ControlMode.Velocity)) {
-			leftA.selectProfileSlot(Constants.TALON_VELOCITY_SLOT_IDX, 0);
-		}
+		dpidc.setGoal(distance);		
+
 		double vel = Constants.WESTCOAST_MAX_SPEED * dpidc.getCorrection();
 		if (!this.gyroControl) {
-			leftA.set(ControlMode.Velocity,  vel);
-			rightA.set(ControlMode.Velocity, vel);
+			leftA.setMode(ControlMode.Velocity,  vel);
+			rightA.setMode(ControlMode.Velocity, vel);
 		}
 		else {
 			SmartDashboard.putString("DB/String 2", String.valueOf(vel));
@@ -158,8 +156,8 @@ public void distanceLoop(){
 		}
 		double vel = Constants.WESTCOAST_MAX_SPEED * dpidc.getCorrection();
 		if (!this.gyroControl) {
-			leftA.set(ControlMode.Velocity,  vel);
-			rightA.set(ControlMode.Velocity, vel);
+			leftA.ControlModeLoop(dpidc.getCorrection());
+			rightA.ControlModeLoop(dpidc.getCorrection());
 		}
 		else {
 			SmartDashboard.putString("DB/String 2", String.valueOf(vel));
@@ -295,11 +293,11 @@ public void zeroSnesors() {
 		this.pidc.reset();
 	}
 
-	private static void zeroEncoders(WPI_TalonSRX talon) {
+	private static void zeroEncoders(SmartTalon talon) {
 		talon.setSelectedSensorPosition(0, Constants.TALON_VELOCITY_SLOT_IDX, 0);
 	}
 
-	private void loadGains(WPI_TalonSRX talon, int slotIdx, PIDGains talonPID) {
+	private void loadGains(SmartTalon talon, int slotIdx, PIDGains talonPID) {
 		talon.config_kP(slotIdx, talonPID.p, 0);
 		talon.config_kI(slotIdx, talonPID.i, 0);
 		talon.config_kD(slotIdx, talonPID.d, 0);