diff --git a/src/main/java/org/usfirst/frc/team2412/robot/subsystems/DriveBaseSubsystem.java b/src/main/java/org/usfirst/frc/team2412/robot/subsystems/DriveBaseSubsystem.java index f60bcbc..52c642b 100644 --- a/src/main/java/org/usfirst/frc/team2412/robot/subsystems/DriveBaseSubsystem.java +++ b/src/main/java/org/usfirst/frc/team2412/robot/subsystems/DriveBaseSubsystem.java @@ -1,12 +1,14 @@ package org.usfirst.frc.team2412.robot.subsystems; +import org.usfirst.frc.team2412.robot.RobotMap; import org.usfirst.frc.team2412.robot.commands.JoystickDriveCommand; +import edu.wpi.first.wpilibj.drive.DifferentialDrive; import edu.wpi.first.wpilibj.Joystick; import edu.wpi.first.wpilibj.command.Subsystem; public class DriveBaseSubsystem extends Subsystem { - // private DifferentialDrive robotDrive = RobotMap.drive; + private DifferentialDrive robotDrive = RobotMap.drive; @Override protected void initDefaultCommand() { diff --git a/src/main/java/org/usfirst/frc/team2412/robot/subsystems/InTakeUpDownSubsystem.java b/src/main/java/org/usfirst/frc/team2412/robot/subsystems/InTakeUpDownSubsystem.java index 3976c22..9fa9f4c 100644 --- a/src/main/java/org/usfirst/frc/team2412/robot/subsystems/InTakeUpDownSubsystem.java +++ b/src/main/java/org/usfirst/frc/team2412/robot/subsystems/InTakeUpDownSubsystem.java @@ -1,8 +1,12 @@ package org.usfirst.frc.team2412.robot.subsystems; +import org.usfirst.frc.team2412.robot.RobotMap; + import edu.wpi.first.wpilibj.Joystick; import edu.wpi.first.wpilibj.command.PIDSubsystem; +import com.ctre.phoenix.motorcontrol.can.WPI_VictorSPX; + public class InTakeUpDownSubsystem extends PIDSubsystem { // PID conversion and unit values @@ -25,7 +29,7 @@ public class InTakeUpDownSubsystem extends PIDSubsystem { // private DigitalInput limitSwitchUp = RobotMap.limitSwitchUp; // private DigitalInput limitSwitchDown = RobotMap.limitSwitchDown; - // private WPI_VictorSPX armMotor1 = RobotMap.armMotor1; + private WPI_VictorSPX armMotor1 = RobotMap.armMotor1; public InTakeUpDownSubsystem() { this(DEFAULT_KP, DEFAULT_KI, DEFAULT_KD); diff --git a/src/main/java/org/usfirst/frc/team2412/robot/subsystems/LiftSubsystem.java b/src/main/java/org/usfirst/frc/team2412/robot/subsystems/LiftSubsystem.java index 8c223b0..2614158 100644 --- a/src/main/java/org/usfirst/frc/team2412/robot/subsystems/LiftSubsystem.java +++ b/src/main/java/org/usfirst/frc/team2412/robot/subsystems/LiftSubsystem.java @@ -1,6 +1,9 @@ package org.usfirst.frc.team2412.robot.subsystems; import com.revrobotics.ControlType; +import com.revrobotics.CANSparkMax; +import com.revrobotics.CANPIDController; +import com.revrobotics.CANEncoder; import org.usfirst.frc.team2412.robot.RobotMap; @@ -27,9 +30,9 @@ public class LiftSubsystem extends Subsystem { double motorRotationsToInches = outputGearCircumference * pullyRatio / gearboxRatio; - // CANSparkMax liftMotorLeader = RobotMap.liftMotors[0]; // Motors from RobotMap - // CANPIDController PIDController = liftMotorLeader.getPIDController(); - // CANEncoder motorEncoder = liftMotorLeader.getEncoder(); + CANSparkMax liftMotorLeader = RobotMap.liftMotors[0]; // Motors from RobotMap + CANPIDController PIDController = liftMotorLeader.getPIDController(); + CANEncoder motorEncoder = liftMotorLeader.getEncoder(); double P = 0.01; //.11 double P_safe = 0.03;