diff --git a/2019robot/src/main/java/org/usfirst/frc4388/robot/commands/ArmSetMode.java b/2019robot/src/main/java/org/usfirst/frc4388/robot/commands/ArmSetMode.java index 3eb2046..6a082a4 100644 --- a/2019robot/src/main/java/org/usfirst/frc4388/robot/commands/ArmSetMode.java +++ b/2019robot/src/main/java/org/usfirst/frc4388/robot/commands/ArmSetMode.java @@ -25,8 +25,8 @@ public class ArmSetMode extends Command { else if (controlMode == ArmControlMode.JOYSTICK_MANUAL) { Robot.arm.setSpeedJoystick(0); } - else if (controlMode == ArmControlMode.MOTION_MAGIC){ - Robot.arm.setPositionMM(Robot.arm.getPositionInches()); + else if (controlMode == ArmControlMode.SMART_MOTION){ + Robot.arm.setPositionSM(Robot.arm.getPositionInches()); } else { Robot.arm.setSpeed(0.0); diff --git a/2019robot/src/main/java/org/usfirst/frc4388/robot/commands/ArmSetPositionMM.java b/2019robot/src/main/java/org/usfirst/frc4388/robot/commands/ArmSetPositionMM.java index ad87894..db483fe 100644 --- a/2019robot/src/main/java/org/usfirst/frc4388/robot/commands/ArmSetPositionMM.java +++ b/2019robot/src/main/java/org/usfirst/frc4388/robot/commands/ArmSetPositionMM.java @@ -5,7 +5,7 @@ import org.usfirst.frc4388.robot.Robot; import edu.wpi.first.wpilibj.command.Command; /** - * + * @deprecated */ public class ArmSetPositionMM extends Command { @@ -26,7 +26,7 @@ public class ArmSetPositionMM extends Command { } else { isAtTarget = false; - Robot.arm.setPositionMM(targetPositionInches); + //Robot.arm.setPositionMM(targetPositionInches); } // System.out.println("Arm set MP initialized, target = " + targetPositionInches); } @@ -42,7 +42,7 @@ public class ArmSetPositionMM extends Command { // Called once after isFinished returns true protected void end() { - Robot.arm.setPositionMM(Robot.arm.getPositionInches()); + //Robot.arm.setPositionMM(Robot.arm.getPositionInches()); // System.out.println("Arm set MP end"); } @@ -51,6 +51,6 @@ public class ArmSetPositionMM extends Command { protected void interrupted() { // System.out.println("ArmSetPositionMP interrupted"); Robot.arm.setFinished(true); - Robot.arm.setPositionMM(Robot.arm.getPositionInches()); + //Robot.arm.setPositionMM(Robot.arm.getPositionInches()); } } diff --git a/2019robot/src/main/java/org/usfirst/frc4388/robot/commands/ArmSetPositionMP.java b/2019robot/src/main/java/org/usfirst/frc4388/robot/commands/ArmSetPositionMP.java index baad693..4036e98 100644 --- a/2019robot/src/main/java/org/usfirst/frc4388/robot/commands/ArmSetPositionMP.java +++ b/2019robot/src/main/java/org/usfirst/frc4388/robot/commands/ArmSetPositionMP.java @@ -5,7 +5,7 @@ import org.usfirst.frc4388.robot.Robot; import edu.wpi.first.wpilibj.command.Command; /** - * + * @deprecated */ public class ArmSetPositionMP extends Command { @@ -25,7 +25,7 @@ public class ArmSetPositionMP extends Command { } else { isAtTarget = false; - Robot.arm.setPositionMP(targetPositionInches); + //Robot.arm.setPositionMP(targetPositionInches); } // System.out.println("Arm set MP initialized, target = " + targetPositionInches); } diff --git a/2019robot/src/main/java/org/usfirst/frc4388/robot/commands/ArmSetPositionSM.java b/2019robot/src/main/java/org/usfirst/frc4388/robot/commands/ArmSetPositionSM.java new file mode 100644 index 0000000..c4a206c --- /dev/null +++ b/2019robot/src/main/java/org/usfirst/frc4388/robot/commands/ArmSetPositionSM.java @@ -0,0 +1,56 @@ +package org.usfirst.frc4388.robot.commands; + +import org.usfirst.frc4388.robot.Robot; + +import edu.wpi.first.wpilibj.command.Command; + +/** + * + */ +public class ArmSetPositionSM extends Command { + + private double targetPositionInches; + private boolean isAtTarget; + private static final double MIN_DELTA_TARGET = 20; + + public ArmSetPositionSM(double targetPositionInches) { + this.targetPositionInches = targetPositionInches; + requires(Robot.arm); + } + + // Called just before this Command runs the first time + protected void initialize() { + + if (Math.abs(targetPositionInches - Robot.arm.getPositionInches()) < MIN_DELTA_TARGET) { + isAtTarget = true; + } + else { + isAtTarget = false; + Robot.arm.setPositionSM(targetPositionInches); + } +// System.out.println("Arm set MP initialized, target = " + targetPositionInches); + } + + // Called repeatedly when this Command is scheduled to run + protected void execute() { + } + + // Make this return true when this Command no longer needs to run execute() + protected boolean isFinished() { + return isAtTarget || Robot.arm.isFinished(); + } + + // Called once after isFinished returns true + protected void end() { + Robot.arm.setPositionSM(Robot.arm.getPositionInches()); +// System.out.println("Arm set MP end"); + } + + // Called when another command which requires one or more of the same + // subsystems is scheduled to run + protected void interrupted() { +// System.out.println("ArmSetPositionMP interrupted"); + Robot.arm.setFinished(true); + Robot.arm.setPositionSM(Robot.arm.getPositionInches()); + } +} diff --git a/2019robot/src/main/java/org/usfirst/frc4388/robot/commands/ArmTest.java b/2019robot/src/main/java/org/usfirst/frc4388/robot/commands/ArmTest.java index 9373add..eabdf4c 100644 --- a/2019robot/src/main/java/org/usfirst/frc4388/robot/commands/ArmTest.java +++ b/2019robot/src/main/java/org/usfirst/frc4388/robot/commands/ArmTest.java @@ -14,7 +14,7 @@ public class ArmTest extends CommandGroup { * Add your docs here. */ public ArmTest() { - addSequential(new ArmSetPositionMM(3600)); + addSequential(new ArmSetPositionSM(3600)); // Add Commands here: // e.g. addSequential(new Command1()); // addSequential(new Command2()); diff --git a/2019robot/src/main/java/org/usfirst/frc4388/robot/commands/ArmTest2.java b/2019robot/src/main/java/org/usfirst/frc4388/robot/commands/ArmTest2.java index 5e7c0c9..eda48bf 100644 --- a/2019robot/src/main/java/org/usfirst/frc4388/robot/commands/ArmTest2.java +++ b/2019robot/src/main/java/org/usfirst/frc4388/robot/commands/ArmTest2.java @@ -14,7 +14,7 @@ public class ArmTest2 extends CommandGroup { * Add your docs here. */ public ArmTest2() { - addSequential(new ArmSetPositionMM(600)); + addSequential(new ArmSetPositionSM(600)); // e.g. addSequential(new Command1()); // addSequential(new Command2()); // these will run in order. diff --git a/2019robot/src/main/java/org/usfirst/frc4388/robot/commands/ArmToHeight1.java b/2019robot/src/main/java/org/usfirst/frc4388/robot/commands/ArmToHeight1.java index 14d38a4..cd92348 100644 --- a/2019robot/src/main/java/org/usfirst/frc4388/robot/commands/ArmToHeight1.java +++ b/2019robot/src/main/java/org/usfirst/frc4388/robot/commands/ArmToHeight1.java @@ -18,7 +18,7 @@ public class ArmToHeight1 extends CommandGroup { public ArmToHeight1() { addSequential(new HatchFlip(false)); addParallel(new WristPlacement(true)); - addParallel(new ArmSetPositionMM(1300), 4); + addParallel(new ArmSetPositionSM(1300), 4); addSequential(new WaitCommand(1)); addSequential(new WristSetPositionPID(2300), 2); // Add Commands here: diff --git a/2019robot/src/main/java/org/usfirst/frc4388/robot/commands/GrabFromLoadingSatation.java b/2019robot/src/main/java/org/usfirst/frc4388/robot/commands/GrabFromLoadingSatation.java index 9878e62..4a96852 100644 --- a/2019robot/src/main/java/org/usfirst/frc4388/robot/commands/GrabFromLoadingSatation.java +++ b/2019robot/src/main/java/org/usfirst/frc4388/robot/commands/GrabFromLoadingSatation.java @@ -15,7 +15,7 @@ public class GrabFromLoadingSatation extends CommandGroup { * Add your docs here. */ public GrabFromLoadingSatation() { - addSequential(new ArmSetPositionMM(550), 1); + addSequential(new ArmSetPositionSM(550), 1); //addParallel(new WaitCommand(1)); addSequential(new WristSetPositionPID(450)); // e.g. addSequential(new Command1()); diff --git a/2019robot/src/main/java/org/usfirst/frc4388/robot/commands/presets/CargoHigh.java b/2019robot/src/main/java/org/usfirst/frc4388/robot/commands/presets/CargoHigh.java index 58da5ab..ce96573 100644 --- a/2019robot/src/main/java/org/usfirst/frc4388/robot/commands/presets/CargoHigh.java +++ b/2019robot/src/main/java/org/usfirst/frc4388/robot/commands/presets/CargoHigh.java @@ -7,7 +7,7 @@ package org.usfirst.frc4388.robot.commands.presets; -import org.usfirst.frc4388.robot.commands.ArmSetPositionMM; +import org.usfirst.frc4388.robot.commands.ArmSetPositionSM; import org.usfirst.frc4388.robot.commands.HatchFlip; import org.usfirst.frc4388.robot.commands.WristSetPositionPID; @@ -21,7 +21,7 @@ public class CargoHigh extends CommandGroup { addSequential(new HatchFlip(false)); addParallel(new setWrist(2781)); addParallel(new DelayHatch()); - addSequential(new ArmSetPositionMM(4038)); + addSequential(new ArmSetPositionSM(4038)); // Add Commands here: // e.g. addSequential(new Command1()); diff --git a/2019robot/src/main/java/org/usfirst/frc4388/robot/commands/presets/CargoLow.java b/2019robot/src/main/java/org/usfirst/frc4388/robot/commands/presets/CargoLow.java index 5b57c0d..faa56bc 100644 --- a/2019robot/src/main/java/org/usfirst/frc4388/robot/commands/presets/CargoLow.java +++ b/2019robot/src/main/java/org/usfirst/frc4388/robot/commands/presets/CargoLow.java @@ -7,7 +7,7 @@ package org.usfirst.frc4388.robot.commands.presets; -import org.usfirst.frc4388.robot.commands.ArmSetPositionMM; +import org.usfirst.frc4388.robot.commands.ArmSetPositionSM; import org.usfirst.frc4388.robot.commands.WristSetPositionPID; import edu.wpi.first.wpilibj.command.CommandGroup; @@ -19,7 +19,7 @@ public class CargoLow extends CommandGroup { public CargoLow() { addParallel(new setWrist(2200)); - addSequential(new ArmSetPositionMM(1300)); + addSequential(new ArmSetPositionSM(1300)); // Add Commands here: // e.g. addSequential(new Command1()); diff --git a/2019robot/src/main/java/org/usfirst/frc4388/robot/commands/presets/CargoMid.java b/2019robot/src/main/java/org/usfirst/frc4388/robot/commands/presets/CargoMid.java index ae3dc76..c63449a 100644 --- a/2019robot/src/main/java/org/usfirst/frc4388/robot/commands/presets/CargoMid.java +++ b/2019robot/src/main/java/org/usfirst/frc4388/robot/commands/presets/CargoMid.java @@ -7,7 +7,7 @@ package org.usfirst.frc4388.robot.commands.presets; -import org.usfirst.frc4388.robot.commands.ArmSetPositionMM; +import org.usfirst.frc4388.robot.commands.ArmSetPositionSM; import org.usfirst.frc4388.robot.commands.WristSetPositionPID; import edu.wpi.first.wpilibj.command.CommandGroup; @@ -19,7 +19,7 @@ public class CargoMid extends CommandGroup { public CargoMid() { addParallel(new setWrist(2330)); - addSequential(new ArmSetPositionMM(2580)); + addSequential(new ArmSetPositionSM(2580)); // Add Commands here: // e.g. addSequential(new Command1()); diff --git a/2019robot/src/main/java/org/usfirst/frc4388/robot/commands/presets/HatchHigh.java b/2019robot/src/main/java/org/usfirst/frc4388/robot/commands/presets/HatchHigh.java index 9867d8e..77653b0 100644 --- a/2019robot/src/main/java/org/usfirst/frc4388/robot/commands/presets/HatchHigh.java +++ b/2019robot/src/main/java/org/usfirst/frc4388/robot/commands/presets/HatchHigh.java @@ -7,7 +7,7 @@ package org.usfirst.frc4388.robot.commands.presets; -import org.usfirst.frc4388.robot.commands.ArmSetPositionMM; +import org.usfirst.frc4388.robot.commands.ArmSetPositionSM; import org.usfirst.frc4388.robot.commands.HatchFlip; import org.usfirst.frc4388.robot.commands.WristSetPositionPID; @@ -21,7 +21,7 @@ public class HatchHigh extends CommandGroup { addSequential(new HatchFlip(false)); addParallel(new setWrist(852)); addParallel(new DelayHatch()); - addSequential(new ArmSetPositionMM(3451)); + addSequential(new ArmSetPositionSM(3451)); // Add Commands here: // e.g. addSequential(new Command1()); diff --git a/2019robot/src/main/java/org/usfirst/frc4388/robot/commands/presets/HatchLow.java b/2019robot/src/main/java/org/usfirst/frc4388/robot/commands/presets/HatchLow.java index f0c3efe..8429a8e 100644 --- a/2019robot/src/main/java/org/usfirst/frc4388/robot/commands/presets/HatchLow.java +++ b/2019robot/src/main/java/org/usfirst/frc4388/robot/commands/presets/HatchLow.java @@ -7,7 +7,7 @@ package org.usfirst.frc4388.robot.commands.presets; -import org.usfirst.frc4388.robot.commands.ArmSetPositionMM; +import org.usfirst.frc4388.robot.commands.ArmSetPositionSM; import org.usfirst.frc4388.robot.commands.WristSetPositionPID; import edu.wpi.first.wpilibj.command.CommandGroup; @@ -19,7 +19,7 @@ public class HatchLow extends CommandGroup { public HatchLow() { addParallel(new setWrist(200)); - addSequential(new ArmSetPositionMM(550)); + addSequential(new ArmSetPositionSM(550)); // Add Commands here: // e.g. addSequential(new Command1()); diff --git a/2019robot/src/main/java/org/usfirst/frc4388/robot/commands/presets/HatchMid.java b/2019robot/src/main/java/org/usfirst/frc4388/robot/commands/presets/HatchMid.java index b38ba4e..447c786 100644 --- a/2019robot/src/main/java/org/usfirst/frc4388/robot/commands/presets/HatchMid.java +++ b/2019robot/src/main/java/org/usfirst/frc4388/robot/commands/presets/HatchMid.java @@ -7,7 +7,7 @@ package org.usfirst.frc4388.robot.commands.presets; -import org.usfirst.frc4388.robot.commands.ArmSetPositionMM; +import org.usfirst.frc4388.robot.commands.ArmSetPositionSM; import org.usfirst.frc4388.robot.commands.WristSetPositionPID; import edu.wpi.first.wpilibj.command.CommandGroup; @@ -19,7 +19,7 @@ public class HatchMid extends CommandGroup { public HatchMid() { addParallel(new setWrist(525)); - addSequential(new ArmSetPositionMM(2050)); + addSequential(new ArmSetPositionSM(2050)); // Add Commands here: // e.g. addSequential(new Command1()); diff --git a/2019robot/src/main/java/org/usfirst/frc4388/robot/commands/presets/SetPositionArmWrist.java b/2019robot/src/main/java/org/usfirst/frc4388/robot/commands/presets/SetPositionArmWrist.java index fe64532..89d9f51 100644 --- a/2019robot/src/main/java/org/usfirst/frc4388/robot/commands/presets/SetPositionArmWrist.java +++ b/2019robot/src/main/java/org/usfirst/frc4388/robot/commands/presets/SetPositionArmWrist.java @@ -7,7 +7,7 @@ package org.usfirst.frc4388.robot.commands.presets; -import org.usfirst.frc4388.robot.commands.ArmSetPositionMM; +import org.usfirst.frc4388.robot.commands.ArmSetPositionSM; import org.usfirst.frc4388.robot.commands.WristSetPositionPID; import edu.wpi.first.wpilibj.command.CommandGroup; @@ -19,7 +19,7 @@ public class SetPositionArmWrist extends CommandGroup { public SetPositionArmWrist(double arm, double wrist) { addParallel(new WristSetPositionPID(wrist)); - addSequential(new ArmSetPositionMM(arm)); + addSequential(new ArmSetPositionSM(arm)); // Add Commands here: // e.g. addSequential(new Command1()); diff --git a/2019robot/src/main/java/org/usfirst/frc4388/robot/commands/presets/StowArm.java b/2019robot/src/main/java/org/usfirst/frc4388/robot/commands/presets/StowArm.java index 3b1e20e..37adbc6 100644 --- a/2019robot/src/main/java/org/usfirst/frc4388/robot/commands/presets/StowArm.java +++ b/2019robot/src/main/java/org/usfirst/frc4388/robot/commands/presets/StowArm.java @@ -7,7 +7,7 @@ package org.usfirst.frc4388.robot.commands.presets; -import org.usfirst.frc4388.robot.commands.ArmSetPositionMM; +import org.usfirst.frc4388.robot.commands.ArmSetPositionSM; import org.usfirst.frc4388.robot.commands.HatchFlip; import org.usfirst.frc4388.robot.commands.WristPlacement; import org.usfirst.frc4388.robot.commands.WristSetPositionPID; @@ -26,7 +26,7 @@ public class StowArm extends CommandGroup { addParallel(new WristPlacement(true)); addParallel(new setLEDPattern(LEDPatterns.SOLID_GREEN)); addParallel(new WristSetPositionPID(110), 2); - addSequential(new ArmSetPositionMM(10)); + addSequential(new ArmSetPositionSM(10)); // Add Commands here: // e.g. addSequential(new Command1()); // addSequential(new Command2()); diff --git a/2019robot/src/main/java/org/usfirst/frc4388/robot/subsystems/Arm.java b/2019robot/src/main/java/org/usfirst/frc4388/robot/subsystems/Arm.java index 2f84e1c..c45453a 100644 --- a/2019robot/src/main/java/org/usfirst/frc4388/robot/subsystems/Arm.java +++ b/2019robot/src/main/java/org/usfirst/frc4388/robot/subsystems/Arm.java @@ -5,7 +5,7 @@ import java.util.ArrayList; import org.usfirst.frc4388.robot.Constants; import org.usfirst.frc4388.robot.Robot; import org.usfirst.frc4388.robot.RobotMap; -import org.usfirst.frc4388.robot.commands.ArmSetPositionMM; +import org.usfirst.frc4388.robot.commands.ArmSetPositionSM; import org.usfirst.frc4388.robot.commands.presets.CargoHigh; import org.usfirst.frc4388.robot.commands.presets.CargoLow; import org.usfirst.frc4388.robot.commands.presets.CargoMid; @@ -31,6 +31,13 @@ import com.ctre.phoenix.motorcontrol.RemoteLimitSwitchSource; import com.ctre.phoenix.motorcontrol.NeutralMode; import com.ctre.phoenix.motorcontrol.SensorCollection; import com.ctre.phoenix.motorcontrol.can.WPI_TalonSRX; +import com.revrobotics.CANDigitalInput; +import com.revrobotics.CANEncoder; +import com.revrobotics.CANPIDController; +import com.revrobotics.CANSparkMax; +import com.revrobotics.ControlType; +import com.revrobotics.CANDigitalInput.LimitSwitchPolarity; +import com.revrobotics.CANSparkMaxLowLevel.MotorType; import edu.wpi.first.wpilibj.DigitalInput; import edu.wpi.first.wpilibj.Solenoid; @@ -46,7 +53,7 @@ public class Arm extends Subsystem implements ControlLoopable { private static Arm instance; - public static enum ArmControlMode { MOTION_PROFILE, JOYSTICK_PID, JOYSTICK_MANUAL, MANUAL, MOTION_MAGIC}; + public static enum ArmControlMode { MOTION_PROFILE, JOYSTICK_PID, JOYSTICK_MANUAL, MANUAL, MOTION_MAGIC, SMART_MOTION}; public static enum PlaceMode { HATCH, CARGO }; // One revolution of the 30T Drive 1.880" PD pulley = Pi * PD inches = 36/24 revs due to pulleys * 34/24 revs due to gears * 36/12 revs due mag encoder gear on ball shifter * 4096 ticks @@ -79,10 +86,12 @@ public class Arm extends Subsystem implements ControlLoopable public static final double MP_T2 = 150; // Fast = 150 // Motor controllers - //TODO//private ArrayList motorControllers = new ArrayList(); + public CANSparkMax motor1; + private CANSparkMax motor2; - //TODO//public TalonSRXEncoder motor1; - //TODO//private TalonSRX motor2; + private CANPIDController motorController; + private CANEncoder motorEncoder; + private CANDigitalInput motorForwardLimitSwitch, motorReverseLimitSwitch; // PID controller and params private MPTalonPIDController mpController; @@ -93,21 +102,24 @@ public class Arm extends Subsystem implements ControlLoopable private PIDParams mpPIDParams = new PIDParams(0.2, 0.0, 0.0, 0.0, 0.005, 0.0); private PIDParams pidPIDParamsHiGear = new PIDParams(0.075, 0.0, 0.0, 0.0, 0.0, 0.0); - public static final double KF_UP = 1;//0.01; - public static final double KF_DOWN = 0;// 0.0; - public static final double P_Value = 4;// 2; - public static final double I_Value = 0.0001;// 0.00030; - public static final double D_Value = 200;// 200; - public static final double F_Value = 0.75; // 1023 / 1360 max speed (ticks/100ms) - public static final double maxGravityComp = 0.01; - public static final double RampRate = 0;// 0.0; - public static final int A_value = 400; - public static final int CV_value = 500; + public static double kFF_Up = 1;//0.01; + public static double kFF_Down = 0;// 0.0; + public static double kP = 4;// 2; + public static double kI = 0.0001;// 0.00030; + public static double kD = 200;// 200; + public static double kIz = 0; + public static double kFF = 0.75; // 1023 / 1360 max speed (ticks/100ms) + public static double maxGravityComp = 0.01; + public static double RampRate = 0;// 0.0; + public static int maxAcc = 400; + public static int maxVel = 500; + public static double kMinOutput = -1; + public static double kMaxOutput = 1; - private PIDParams armPIDParams = new PIDParams(P_Value, I_Value, D_Value, KF_DOWN); // KF gets updated later + private PIDParams armPIDParams = new PIDParams(kP, kI, kD, kFF_Down); // KF gets updated later public static final double PID_ERROR_INCHES = 50; LimitSwitchSource limitSwitchSource; @@ -126,7 +138,7 @@ public class Arm extends Subsystem implements ControlLoopable private ArmControlMode armControlMode = ArmControlMode.MOTION_MAGIC; public PlaceMode placeMode = PlaceMode.HATCH; public double targetPositionInchesPID = 0; - public double targetPositionInchesMM = 0; + public double targetPositionInchesSM = 0; private boolean firstMpPoint; private double joystickInchesPerMs = JOYSTICK_INCHES_PER_MS_LO; private double p = 0; @@ -134,35 +146,52 @@ public class Arm extends Subsystem implements ControlLoopable public Arm() { try { - //TODO// motor1 = TalonSRXFactory.createTalonEncoder(RobotMap.ARM_MOTOR1_ID, (ENCODER_TICKS_TO_INCHES), false, FeedbackDevice.QuadEncoder); - //TODO// motor2 = TalonSRXFactory.createPermanentSlaveTalon(RobotMap.ARM_MOTOR2_ID, RobotMap.ARM_MOTOR1_ID); + // motor1 = TalonSRXFactory.createTalonEncoder(RobotMap.ARM_MOTOR1_ID, (ENCODER_TICKS_TO_INCHES), false, FeedbackDevice.QuadEncoder); + // motor2 = TalonSRXFactory.createPermanentSlaveTalon(RobotMap.ARM_MOTOR2_ID, RobotMap.ARM_MOTOR1_ID); + + motor1 = new CANSparkMax(RobotMap.ARM_MOTOR1_ID, MotorType.kBrushless); + motor2 = new CANSparkMax(RobotMap.ARM_MOTOR2_ID, MotorType.kBrushless); + motor1.restoreFactoryDefaults(); + motor2.restoreFactoryDefaults(); + motor2.follow(motor1); + motorController = motor1.getPIDController(); + motorEncoder = motor1.getEncoder(); + motorForwardLimitSwitch = motor1.getForwardLimitSwitch(LimitSwitchPolarity.kNormallyOpen); + motorReverseLimitSwitch = motor1.getReverseLimitSwitch(LimitSwitchPolarity.kNormallyOpen); + + motorController.setP(kP); + motorController.setI(kI); + motorController.setD(kD); + motorController.setIZone(kIz); + motorController.setFF(kFF); + motorController.setOutputRange(kMinOutput, kMaxOutput); - //TODO// motor1.setInverted(true); - //TODO//motor2.setInverted(true); + // motor1.setInverted(true); + // motor2.setInverted(true); // if (motor1.isSensorPresent(CANTalon.FeedbackDevice.QuadEncoder) != CANTalon.FeedbackDeviceStatus.FeedbackStatusPresent) { // Driver.reportError("Could not detect elevator motor 1 encoder encoder!", false); // } - //TODO// motor1.configNominalOutputForward(0, TalonSRXEncoder.TIMEOUT_MS); - //TODO// motor1.configNominalOutputReverse(0, TalonSRXEncoder.TIMEOUT_MS); - //TODO// motor1.configPeakOutputForward(1, TalonSRXEncoder.TIMEOUT_MS); - //TODO// motor1.configPeakOutputReverse(-1, TalonSRXEncoder.TIMEOUT_MS); + // motor1.configNominalOutputForward(0, TalonSRXEncoder.TIMEOUT_MS); + // motor1.configNominalOutputReverse(0, TalonSRXEncoder.TIMEOUT_MS); + // motor1.configPeakOutputForward(1, TalonSRXEncoder.TIMEOUT_MS); + // motor1.configPeakOutputReverse(-1, TalonSRXEncoder.TIMEOUT_MS); - //TODO// motor1.selectProfileSlot(MM_SLOT, 0); - //TODO// motor1.config_kF(MM_SLOT, F_Value, TalonSRXEncoder.TIMEOUT_MS); - //TODO// motor1.config_kP(MM_SLOT, P_Value, TalonSRXEncoder.TIMEOUT_MS); - //TODO// motor1.config_kI(MM_SLOT, I_Value, TalonSRXEncoder.TIMEOUT_MS); - //TODO// motor1.config_kD(MM_SLOT, D_Value, TalonSRXEncoder.TIMEOUT_MS); - //TODO// motor1.setSensorPhase(true); + // motor1.selectProfileSlot(MM_SLOT, 0); + // motor1.config_kF(MM_SLOT, F_Value, TalonSRXEncoder.TIMEOUT_MS); + // motor1.config_kP(MM_SLOT, P_Value, TalonSRXEncoder.TIMEOUT_MS); + // motor1.config_kI(MM_SLOT, I_Value, TalonSRXEncoder.TIMEOUT_MS); + // motor1.config_kD(MM_SLOT, D_Value, TalonSRXEncoder.TIMEOUT_MS); + // motor1.setSensorPhase(true); - //TODO// motor1.configForwardLimitSwitchSource(limitSwitchSource, LimitSwitchNormal.NormallyOpen, 0); - //TODO// motor1.configReverseLimitSwitchSource(limitSwitchSource, LimitSwitchNormal.NormallyOpen, 0); - //TODO// motor1.setNeutralMode(NeutralMode.Brake); - //TODO// motor2.setNeutralMode(NeutralMode.Brake); - //TODO// motor1.enableCurrentLimit(true); - //TODO// motorControllers.add(motor1); + // motor1.configForwardLimitSwitchSource(limitSwitchSource, LimitSwitchNormal.NormallyOpen, 0); + // motor1.configReverseLimitSwitchSource(limitSwitchSource, LimitSwitchNormal.NormallyOpen, 0); + // motor1.setNeutralMode(NeutralMode.Brake); + // motor2.setNeutralMode(NeutralMode.Brake); + // motor1.enableCurrentLimit(true); + // motorControllers.add(motor1); //motor1.setSelectedSensorPosition(0, , ); @@ -181,7 +210,8 @@ public class Arm extends Subsystem implements ControlLoopable } public void resetEncoder(){ - //TODO// motor1.setPosition(0); + motorEncoder.setPosition(0); + //motor1.setEncPosition(0); //targetPositionInchesMM = 0; //targetPositionInchesPID = 0; } @@ -195,48 +225,28 @@ public class Arm extends Subsystem implements ControlLoopable } public void setSpeed(double speed) { - //TODO// motor1.set(ControlMode.PercentOutput, speed); + // motor1.set(ControlMode.PercentOutput, speed); + motorController.setReference(speed, ControlType.kVoltage); setArmControlMode(ArmControlMode.MANUAL); } public void setSpeedJoystick(double speed) { - //TODO// motor1.set(ControlMode.PercentOutput, speed); + setSpeed(speed); setArmControlMode(ArmControlMode.JOYSTICK_MANUAL); } - public void setPositionMM(double targetPositionInches){ - //TODO// motor1.set(ControlMode.MotionMagic, targetPositionInches); - //System.err.println(motor1.getControlMode()); - //TODO// motor1.selectProfileSlot(MM_SLOT, 0); - setArmControlMode(ArmControlMode.MOTION_MAGIC); - updatePositionMM(targetPositionInches); - setFinished(false); - } public double calcGravityCompensationAtCurrentPosition() { - //TODO// int ticks = motor1.getSelectedSensorPosition(); - //TODO// double degreesFromDown = (ticks+920)*(360.0/(4096*3)); - //TODO// double compensation = maxGravityComp * Math.sin(degreesFromDown*Math.PI/180); + double rot = motorEncoder.getPosition(); + //double degreesFromDown = (rot+920)*(360.0/(4096*3)); + // double compensation = maxGravityComp * Math.sin(degreesFromDown*Math.PI/180); //System.err.println("comp(" + degreesFromDown + "^) = " + compensation); //TODO// return compensation; return 0.0; } - public void updatePositionMM(double targetPositionInches){ - targetPositionInchesMM = limitPosition(targetPositionInches); - //double startPositionInches = motor1.getPositionWorld(); - double compensation = calcGravityCompensationAtCurrentPosition(); - //System.err.println("compensation = " + compensation); - // motor1.set(ControlMode.MotionMagic, targetPositionInches); - //TODO// motor1.set(ControlMode.MotionMagic, targetPositionInches, DemandType.ArbitraryFeedForward, compensation); - //System.err.println(motor1.getControlMode()); - //TODO// motor1.configMotionCruiseVelocity(CV_value, TalonSRXEncoder.TIMEOUT_MS); - //TODO// motor1.configMotionAcceleration(A_value, TalonSRXEncoder.TIMEOUT_MS); - - - } public void setPositionPID(double targetPositionInches) { - //TODO// motor1.set(ControlMode.Position, targetPositionInches); - mpController.setPIDSlot(PID_SLOT); //TODO: verify that motor's selectProfileSlot() should be called AFTER its control mode is set + // motorController.setReference(targetPositionInches, ControlType.kPosition); + // mpController.setPIDSlot(PID_SLOT); //TODO: verify that motor's selectProfileSlot() should be called AFTER its control mode is set updatePositionPID(targetPositionInches); setArmControlMode(ArmControlMode.JOYSTICK_PID); setFinished(false); @@ -244,31 +254,27 @@ public class Arm extends Subsystem implements ControlLoopable public void updatePositionPID(double targetPositionInches) { targetPositionInchesPID = limitPosition(targetPositionInches); - //TODO// if (limitPosition(motor1.getPositionWorld()) == MIN_POSITION_INCHES){ + if (limitPosition(motorEncoder.getPosition()) == MIN_POSITION_INCHES){ resetEncoder(); - //TODO// } - //TODO// double startPositionInches = motor1.getPositionWorld(); - //mpController.setTarget(targetPositionInchesPID, targetPositionInchesPID > startPositionInches ? KF_UP : KF_DOWN); - //TODO// motor1.set(ControlMode.Position, targetPositionInches); - //TODO// motor1.configClosedloopRamp(0); - //motor1.configPeakCurrentLimit(5); - //TODO// motor1.configContinuousCurrentLimit(2); - //TODO// motor1.config_kP(0, P_Value, TalonSRXEncoder.TIMEOUT_MS); - //TODO// motor1.config_kI(0, I_Value, TalonSRXEncoder.TIMEOUT_MS); - //TODO// motor1.config_kD(0, D_Value, TalonSRXEncoder.TIMEOUT_MS); - //TODO// motor1.config_kF(0, targetPositionInchesPID > startPositionInches ? KF_UP : KF_DOWN, TalonSRXEncoder.TIMEOUT_MS); + } + double startPositionInches = motorEncoder.getPosition(); + // mpController.setTarget(targetPositionInchesPID, targetPositionInchesPID > startPositionInches ? KF_UP : KF_DOWN); + motorController.setReference(targetPositionInches, ControlType.kPosition); + // motor1.setClosedLoopRampRate(RampRate); + motorController.setFF(targetPositionInchesPID > startPositionInches ? kFF_Up : kFF_Down); + + + // motor1.configClosedloopRamp(0); + // motor1.configPeakCurrentLimit(5); + // motor1.configContinuousCurrentLimit(2); + // motor1.config_kP(0, P_Value, TalonSRXEncoder.TIMEOUT_MS); + // motor1.config_kI(0, I_Value, TalonSRXEncoder.TIMEOUT_MS); + // motor1.config_kD(0, D_Value, TalonSRXEncoder.TIMEOUT_MS); + // motor1.config_kF(0, targetPositionInchesPID > startPositionInches ? KF_UP : KF_DOWN, TalonSRXEncoder.TIMEOUT_MS); //System.err.println(motor1.getControlMode()); //System.err.print(motor1.getClosedLoopError()); } - public void setPositionMP(double targetPositionInches) { - //TODO// double startPositionInches = motor1.getPositionWorld(); - //TODO// mpController.setMPTarget(startPositionInches, limitPosition(targetPositionInches), MP_MAX_VELOCITY_INCHES_PER_SEC, MP_T1, MP_T2); - setFinished(false); - firstMpPoint = true; - setArmControlMode(ArmControlMode.MOTION_PROFILE); - } - private double limitPosition(double targetPosition) { if (targetPosition < MIN_POSITION_INCHES) { return MIN_POSITION_INCHES; @@ -281,10 +287,10 @@ public class Arm extends Subsystem implements ControlLoopable } @Override public void setPeriodMs(long periodMs) { - //TODO// mpController = new MPTalonPIDController(periodMs, motorControllers); - mpController.setPID(mpPIDParams, MP_SLOT); - mpController.setPID(armPIDParams, PID_SLOT); - mpController.setPIDSlot(PID_SLOT); + // mpController = new MPTalonPIDController(periodMs, motorControllers); + //mpController.setPID(mpPIDParams, MP_SLOT); + //mpController.setPID(armPIDParams, PID_SLOT); + //mpController.setPIDSlot(PID_SLOT); this.periodMs = periodMs; } @@ -331,14 +337,14 @@ public class Arm extends Subsystem implements ControlLoopable dPadButtons(); - //TODO// if (motor1.getSensorCollection().isRevLimitSwitchClosed()){ - //TODO// resetEncoder(); - //TODO// } + if (motorReverseLimitSwitch.get()){ + resetEncoder(); + } // Do the update if (armControlMode == ArmControlMode.JOYSTICK_MANUAL) { controlManualWithJoystick(); - //TODO// System.err.println(motor1.getControlMode()); + // System.err.println(motorController.getControlMode()); } else if (!isFinished) { if (armControlMode == ArmControlMode.MOTION_PROFILE) { @@ -347,13 +353,15 @@ public class Arm extends Subsystem implements ControlLoopable } if (armControlMode == ArmControlMode.JOYSTICK_PID){ controlPidWithJoystick(); - //TODO// System.err.println(motor1.getControlMode()); - } - if (armControlMode == ArmControlMode.MOTION_MAGIC){ - controlMMWithJoystick(); - //TODO// System.err.println(motor1.getControlMode()); + // System.err.println(motor1.getControlMode()); } + if (armControlMode == ArmControlMode.SMART_MOTION){ + } + /*if (armControlMode == ArmControlMode.MOTION_MAGIC){ + controlMMWithJoystick(); + // System.err.println(motor1.getControlMode()); + }*/ /*else if (armControlMode == ArmControlMode.MP_PATH_VELOCITY) { isFinished = mpPathVelocityController.controlLoopUpdate(getGyroAngleDeg()); } @@ -376,21 +384,10 @@ public class Arm extends Subsystem implements ControlLoopable targetPositionInchesPID = targetPositionInchesPID + deltaPosition; updatePositionPID(targetPositionInchesPID); } - private void controlMMWithJoystick() { - double joystickPosition = -Robot.oi.getOperatorController().getLeftYAxis(); - double deltaPosition = joystickPosition * joystickInchesPerMs; - targetPositionInchesMM = targetPositionInchesMM + deltaPosition; - updatePositionMM(targetPositionInchesMM); - //Robot.wrist.targetPositionInchesPID = targetPositionInchesPID - (deltaPosition/3); - //Robot.wrist.updatePositionPID(Robot.wrist.targetPositionInchesPID); - - - } private void ControlWithJoystickhold(){ - //TODO// double holdPosition = motor1.getPositionWorld(); - //TODO// updatePositionPID(holdPosition); - + double holdPosition = motorEncoder.getPosition(); + updatePositionPID(holdPosition); } private void controlManualWithJoystick() { @@ -416,7 +413,7 @@ public class Arm extends Subsystem implements ControlLoopable } */ public double getPositionInches() { - //TODO// return motor1.getPositionWorld(); + return motorEncoder.getPosition(); } // public double getAverageMotorCurrent() { @@ -424,7 +421,7 @@ public class Arm extends Subsystem implements ControlLoopable // } public double getAverageMotorCurrent() { - //TODO// return (motor1.getOutputCurrent() + motor2.getOutputCurrent()) / 2; + return (motor1.getOutputCurrent() + motor2.getOutputCurrent()) / 2; } public synchronized boolean isFinished() { @@ -439,18 +436,58 @@ public class Arm extends Subsystem implements ControlLoopable return periodMs; } + /* public void setPositionMM(double targetPositionInches){ + //TODO// motor1.set(ControlMode.MotionMagic, targetPositionInches); + //System.err.println(motor1.getControlMode()); + //TODO// motor1.selectProfileSlot(MM_SLOT, 0); + setArmControlMode(ArmControlMode.MOTION_MAGIC); + updatePositionMM(targetPositionInches); + setFinished(false); + } */ + + /* public void setPositionMP(double targetPositionInches) { + double startPositionInches = motorEncoder.getPosition(); + // mpController.setMPTarget(startPositionInches, limitPosition(targetPositionInches), MP_MAX_VELOCITY_INCHES_PER_SEC, MP_T1, MP_T2); + setFinished(false); + firstMpPoint = true; + setArmControlMode(ArmControlMode.MOTION_PROFILE); + } */ + + /* public void updatePositionMM(double targetPositionInches){ + targetPositionInchesMM = limitPosition(targetPositionInches); + //double startPositionInches = motor1.getPositionWorld(); + double compensation = calcGravityCompensationAtCurrentPosition(); + //System.err.println("compensation = " + compensation); + // motor1.set(ControlMode.MotionMagic, targetPositionInches); + //TODO// motor1.set(ControlMode.MotionMagic, targetPositionInches, DemandType.ArbitraryFeedForward, compensation); + //System.err.println(motor1.getControlMode()); + //TODO// motor1.configMotionCruiseVelocity(CV_value, TalonSRXEncoder.TIMEOUT_MS); + //TODO// motor1.configMotionAcceleration(A_value, TalonSRXEncoder.TIMEOUT_MS); + + + }*/ + + /* private void controlMMWithJoystick() { + double joystickPosition = -Robot.oi.getOperatorController().getLeftYAxis(); + double deltaPosition = joystickPosition * joystickInchesPerMs; + targetPositionInchesMM = targetPositionInchesMM + deltaPosition; + updatePositionMM(targetPositionInchesMM); + //Robot.wrist.targetPositionInchesPID = targetPositionInchesPID - (deltaPosition/3); + //Robot.wrist.updatePositionPID(Robot.wrist.targetPositionInchesPID); + } */ + public void updateStatus(Robot.OperationMode operationMode) { //System.err.println("the encoder is right after this"); try { - //TODO// SmartDashboard.putNumber("Arm Ticks", motor1.getPositionWorld()); + SmartDashboard.putNumber("Arm Ticks", motorEncoder.getPosition()); //SmartDashboard.putNumber("Arm Motor 1 Amps", motor1.getOutputCurrent()); //SmartDashboard.putNumber("Arm Motor 2 Amps", motor2.getOutputCurrent()); //SmartDashboard.putNumber("sensor vel", motor1.getSelectedSensorVelocity()); //SmartDashboard.putNumber("Arm Average Amps", getAverageMotorCurrent()); - //TODO// SmartDashboard.putNumber("Arm Error", motor1.getClosedLoopError()); + //SmartDashboard.putNumber("Arm Error", motor1.getClosedLoopError()); SmartDashboard.putNumber("Arm Amps", getAverageMotorCurrent()); - SmartDashboard.putNumber("Arm Target MM", targetPositionInchesMM); + SmartDashboard.putNumber("Arm Target SM", targetPositionInchesSM); //SmartDashboard.putNumber("arm output", motor1.getMotorOutputPercent()); //SmartDashboard.putNumber("Elevator Motor 1 Amps PDP", Robot.pdp.getCurrent(RobotMap.ELEVATOR_MOTOR_1_CAN_ID)); //SmartDashboard.putNumber("Elevator Motor 2 Amps PDP", Robot.pdp.getCurrent(RobotMap.ELEVATOR_MOTOR_2_CAN_ID));