diff --git a/2019robot/src/main/java/org/usfirst/frc4388/robot/OI.java b/2019robot/src/main/java/org/usfirst/frc4388/robot/OI.java index 914ee9d..efbe2c8 100644 --- a/2019robot/src/main/java/org/usfirst/frc4388/robot/OI.java +++ b/2019robot/src/main/java/org/usfirst/frc4388/robot/OI.java @@ -7,7 +7,7 @@ import buttons.XBoxTriggerButton; import org.usfirst.frc4388.controller.IHandController; import org.usfirst.frc4388.controller.XboxController; import org.usfirst.frc4388.robot.commands.*; -import org.usfirst.frc4388.robot.commands.presets.SetPositionArmWrist; +import org.usfirst.frc4388.robot.commands.presets.StowArm; import org.usfirst.frc4388.robot.constants.LEDPatterns; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; @@ -16,6 +16,7 @@ import edu.wpi.first.wpilibj.Joystick; import edu.wpi.first.wpilibj.buttons.*; import org.usfirst.frc4388.robot.subsystems.*; import org.usfirst.frc4388.robot.subsystems.Arm.ArmControlMode; +import org.usfirst.frc4388.robot.subsystems.Arm.PlaceMode; import org.usfirst.frc4388.robot.subsystems.Wrist.WristControlMode; import org.usfirst.frc4388.utility.MPSoftwarePIDController.MPSoftwareTurnType; import org.usfirst.frc4388.utility.MPSoftwarePIDController.MPSoftwareTurnType; @@ -68,6 +69,10 @@ public class OI JoystickButton liftBothIntake = new JoystickButton(m_operatorXbox.getJoyStick(), XboxController.X_BUTTON); liftBothIntake.whenPressed(new HatchAndBallUp()); + + JoystickButton cargoPlaceMode = new JoystickButton(m_operatorXbox.getJoyStick(), XboxController.B_BUTTON); + cargoPlaceMode.whenPressed(new SwitchArmPlaceMode(PlaceMode.CARGO)); + cargoPlaceMode.whenReleased(new SwitchArmPlaceMode(PlaceMode.HATCH)); JoystickButton liftHatchIntake = new JoystickButton(m_operatorXbox.getJoyStick(), XboxController.RIGHT_BUMPER_BUTTON); liftHatchIntake.whenPressed(new LiftHatchDropBall()); @@ -87,9 +92,9 @@ public class OI JoystickButton lowHeight = new JoystickButton(m_operatorXbox.getJoyStick(),XboxController.LEFT_JOYSTICK_BUTTON); lowHeight.whenPressed(new GrabFromLoadingSatation()); - JoystickButton stow = new JoystickButton(m_operatorXbox.getJoyStick(), XboxController.B_BUTTON); - stow.whenPressed(new StowArm()); - stow.whenPressed(new setLEDPattern(LEDPatterns.SOLID_GREEN)); + //JoystickButton stow = new JoystickButton(m_operatorXbox.getJoyStick(), XboxController.B_BUTTON); + //stow.whenPressed(new StowArm()); + //stow.whenPressed(new setLEDPattern(LEDPatterns.SOLID_GREEN)); /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// @@ -122,8 +127,10 @@ public class OI SmartDashboard.putData("switch to manuel", new SetManual()); - SmartDashboard.putData("grab from station", new GrabFromLoadingSatation()); - SmartDashboard.putData("wrist test", new wristTest()); + SmartDashboard.putData("WristTest", new wristTest()); + SmartDashboard.putData("arm test", new ArmTest()); + //SmartDashboard.putData("arm test2", new ArmTest2()); + } catch (Exception e) { diff --git a/2019robot/src/main/java/org/usfirst/frc4388/robot/Robot.java b/2019robot/src/main/java/org/usfirst/frc4388/robot/Robot.java index 896a60f..fe67815 100644 --- a/2019robot/src/main/java/org/usfirst/frc4388/robot/Robot.java +++ b/2019robot/src/main/java/org/usfirst/frc4388/robot/Robot.java @@ -49,8 +49,8 @@ public class Robot extends TimedRobot //Printing game data to riolog String gameData = DriverStation.getInstance().getGameSpecificMessage(); System.out.println(gameData); - CameraServer.getInstance().startAutomaticCapture(); - CameraServer.getInstance().putVideo("res", 300, 220); + //CameraServer.getInstance().startAutomaticCapture(); + //CameraServer.getInstance().putVideo("res", 300, 220); try { oi = OI.getInstance(); @@ -89,38 +89,30 @@ public class Robot extends TimedRobot public void autonomousInit() { updateChoosers(); - controlLoop.start(); - drive.endGyroCalibration(); - drive.resetEncoders(); - drive.resetGyro(); - drive.setIsRed(getAlliance().equals(Alliance.Red)); - arm.resetEncoder(); - + drive.endGyroCalibration(); + arm.targetPositionInchesMM = arm.motor1.getPositionWorld(); + wrist.targetPositionInchesMM = wrist.wristMotor1.getPositionWorld(); + //drive.resetEncoders(); + //drive.resetGyro(); + //drive.setIsRed(getAlliance().equals(Alliance.Red)); + //arm.resetEncoder(); + updateStatus(); } - - - + public void autonomousPeriodic() { - Scheduler.getInstance().run(); + Scheduler.getInstance().run(); updateStatus(); } public void teleopInit() { - System.err.println("Beginning of teleopInit."); - - drive.setToBrakeOnNeutral(false); +// drive.setToBrakeOnNeutral(false); updateChoosers(); - System.err.println("TeleopInit after UpdateChoosers"); controlLoop.start(); - System.err.println("TeleopInit after controlLoop"); //drive.resetEncoders(); - //System.err.println("TeleopInit after resetEncoders"); drive.endGyroCalibration(); - System.err.println("TeleopInit after endGyroCalibrations"); arm.targetPositionInchesMM = arm.motor1.getPositionWorld(); wrist.targetPositionInchesPID = wrist.wristMotor1.getPositionWorld(); - updateStatus(); } 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 3cb7813..9373add 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(1200)); + addSequential(new ArmSetPositionMM(3600)); // Add Commands here: // e.g. addSequential(new Command1()); // addSequential(new Command2()); diff --git a/2019robot/src/main/java/org/usfirst/frc4388/robot/commands/DriveStraightBasic.java b/2019robot/src/main/java/org/usfirst/frc4388/robot/commands/DriveStraightBasic.java index a477dc7..abd3ccf 100644 --- a/2019robot/src/main/java/org/usfirst/frc4388/robot/commands/DriveStraightBasic.java +++ b/2019robot/src/main/java/org/usfirst/frc4388/robot/commands/DriveStraightBasic.java @@ -80,9 +80,9 @@ public class DriveStraightBasic extends Command { } if (!finished) { m_speed = velocityToMoveSpeed(velocity, m_goingBackwards); - SmartDashboard.putNumber("DSB Dist", position); + //SmartDashboard.putNumber("DSB Dist", position); } else { - SmartDashboard.putNumber("DSB finDist", position); + //SmartDashboard.putNumber("DSB finDist", position); } return finished; } @@ -90,7 +90,7 @@ public class DriveStraightBasic extends Command { // Called once after isFinished returns true protected void end() { double currentTimestamp = Timer.getFPGATimestamp(); - SmartDashboard.putNumber("DSB Runtime", currentTimestamp - m_commandInitTimestamp); + //SmartDashboard.putNumber("DSB Runtime", currentTimestamp - m_commandInitTimestamp); Robot.drive.rawMoveSteer(0.0, 0.0); Robot.drive.setControlMode(DriveControlMode.JOYSTICK); } diff --git a/2019robot/src/main/java/org/usfirst/frc4388/robot/commands/DriveStraightBasicAbs.java b/2019robot/src/main/java/org/usfirst/frc4388/robot/commands/DriveStraightBasicAbs.java index 21fede7..9ed2fb6 100644 --- a/2019robot/src/main/java/org/usfirst/frc4388/robot/commands/DriveStraightBasicAbs.java +++ b/2019robot/src/main/java/org/usfirst/frc4388/robot/commands/DriveStraightBasicAbs.java @@ -92,9 +92,9 @@ public class DriveStraightBasicAbs extends Command { } if (!finished) { m_speed = velocityToMoveSpeed(velocity, m_goingBackwards); - SmartDashboard.putNumber("DSB Dist", position); + //SmartDashboard.putNumber("DSB Dist", position); } else { - SmartDashboard.putNumber("DSB finDist", position); + //SmartDashboard.putNumber("DSB finDist", position); } return finished; } diff --git a/2019robot/src/main/java/org/usfirst/frc4388/robot/commands/WristSetMode.java b/2019robot/src/main/java/org/usfirst/frc4388/robot/commands/WristSetMode.java index e0951ec..366375c 100644 --- a/2019robot/src/main/java/org/usfirst/frc4388/robot/commands/WristSetMode.java +++ b/2019robot/src/main/java/org/usfirst/frc4388/robot/commands/WristSetMode.java @@ -24,7 +24,11 @@ public class WristSetMode extends Command { } else if (controlMode == WristControlMode.JOYSTICK_MANUAL) { Robot.wrist.setSpeedJoystick(0); - } + } + else if (controlMode == WristControlMode.MOTION_MAGIC){ + Robot.wrist.setPositionMM(Robot.wrist.getPositionInches()); + } + else { Robot.wrist.setSpeed(0.0); } diff --git a/2019robot/src/main/java/org/usfirst/frc4388/robot/commands/WristSetPositionMM.java b/2019robot/src/main/java/org/usfirst/frc4388/robot/commands/WristSetPositionMM.java new file mode 100644 index 0000000..ac0183b --- /dev/null +++ b/2019robot/src/main/java/org/usfirst/frc4388/robot/commands/WristSetPositionMM.java @@ -0,0 +1,57 @@ +package org.usfirst.frc4388.robot.commands; + +import org.usfirst.frc4388.robot.Robot; + +import edu.wpi.first.wpilibj.command.Command; + +/** + * + */ +public class WristSetPositionMM extends Command { + + private double targetPositionInches; + private boolean isAtTarget; + private static final double MIN_DELTA_TARGET = 20; + + public WristSetPositionMM(double targetPositionInches) { + this.targetPositionInches = targetPositionInches; + requires(Robot.wrist); + } + + // Called just before this Command runs the first time + protected void initialize() { + + if (Math.abs(targetPositionInches - Robot.wrist.getPositionInches()) < MIN_DELTA_TARGET) { + isAtTarget = true; + System.out.println("Wrist is at target"); + } + else { + isAtTarget = false; + Robot.wrist.setPositionMM(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.wrist.isFinished(); + } + + // Called once after isFinished returns true + protected void end() { + Robot.wrist.setPositionMM(Robot.wrist.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.wrist.setFinished(true); + Robot.wrist.setPositionMM(Robot.wrist.getPositionInches()); + } +} 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 920b755..57996b9 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 @@ -8,6 +8,7 @@ package org.usfirst.frc4388.robot.commands.presets; import org.usfirst.frc4388.robot.commands.ArmSetPositionMM; +import org.usfirst.frc4388.robot.commands.HatchFlip; import org.usfirst.frc4388.robot.commands.WristSetPositionPID; import edu.wpi.first.wpilibj.command.CommandGroup; @@ -17,9 +18,10 @@ public class CargoHigh extends CommandGroup { * Add your docs here. */ public CargoHigh() { - - addParallel(new WristSetPositionPID(3243)); - addSequential(new ArmSetPositionMM(4298)); + addSequential(new HatchFlip(false)); + addParallel(new setWrist(2781)); + addParallel(new DelayHatch()); + addSequential(new ArmSetPositionMM(3812)); // 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 d636586..58dd417 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 @@ -18,8 +18,8 @@ public class CargoLow extends CommandGroup { */ public CargoLow() { - addParallel(new WristSetPositionPID(2500)); - addSequential(new ArmSetPositionMM(1388)); + addParallel(new setWrist(2300)); + addSequential(new ArmSetPositionMM(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 ec71323..2a48ef7 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 @@ -18,8 +18,8 @@ public class CargoMid extends CommandGroup { */ public CargoMid() { - addParallel(new WristSetPositionPID(2830)); - addSequential(new ArmSetPositionMM(2830)); + addParallel(new setWrist(2430)); + addSequential(new ArmSetPositionMM(2630)); // Add Commands here: // e.g. addSequential(new Command1()); diff --git a/2019robot/src/main/java/org/usfirst/frc4388/robot/commands/presets/DelayHatch.java b/2019robot/src/main/java/org/usfirst/frc4388/robot/commands/presets/DelayHatch.java new file mode 100644 index 0000000..e10b72f --- /dev/null +++ b/2019robot/src/main/java/org/usfirst/frc4388/robot/commands/presets/DelayHatch.java @@ -0,0 +1,39 @@ +/*----------------------------------------------------------------------------*/ +/* Copyright (c) 2018 FIRST. All Rights Reserved. */ +/* Open Source Software - may be modified and shared by FRC teams. The code */ +/* must be accompanied by the FIRST BSD license file in the root directory of */ +/* the project. */ +/*----------------------------------------------------------------------------*/ + +package org.usfirst.frc4388.robot.commands.presets; + +import org.usfirst.frc4388.robot.commands.HatchFlip; + +import edu.wpi.first.wpilibj.command.CommandGroup; +import edu.wpi.first.wpilibj.command.WaitCommand; + +public class DelayHatch extends CommandGroup { + /** + * Add your docs here. + */ + public DelayHatch() { + addSequential(new WaitCommand(1)); + addSequential(new HatchFlip (true)); + // Add Commands here: + // e.g. addSequential(new Command1()); + // addSequential(new Command2()); + // these will run in order. + + // To run multiple commands at the same time, + // use addParallel() + // e.g. addParallel(new Command1()); + // addSequential(new Command2()); + // Command1 and Command2 will run in parallel. + + // A command group will require all of the subsystems that each member + // would require. + // e.g. if Command1 requires chassis, and Command2 requires arm, + // a CommandGroup containing them would require both the chassis and the + // arm. + } +} 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 a78c8f3..b9bc0e8 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 @@ -8,6 +8,7 @@ package org.usfirst.frc4388.robot.commands.presets; import org.usfirst.frc4388.robot.commands.ArmSetPositionMM; +import org.usfirst.frc4388.robot.commands.HatchFlip; import org.usfirst.frc4388.robot.commands.WristSetPositionPID; import edu.wpi.first.wpilibj.command.CommandGroup; @@ -17,8 +18,9 @@ public class HatchHigh extends CommandGroup { * Add your docs here. */ public HatchHigh() { - - addParallel(new WristSetPositionPID(1144)); + addSequential(new HatchFlip(false)); + addParallel(new setWrist(1144)); + addParallel(new DelayHatch()); addSequential(new ArmSetPositionMM(3605)); // Add Commands here: 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 fd85278..da1b0ba 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 @@ -18,8 +18,8 @@ public class HatchLow extends CommandGroup { */ public HatchLow() { - addParallel(new WristSetPositionPID(450)); - addSequential(new ArmSetPositionMM(590)); + addParallel(new setWrist(350)); + addSequential(new ArmSetPositionMM(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 b5660ac..bbf9813 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 @@ -18,7 +18,7 @@ public class HatchMid extends CommandGroup { */ public HatchMid() { - addParallel(new WristSetPositionPID(750)); + addParallel(new setWrist(750)); addSequential(new ArmSetPositionMM(2000)); // Add Commands here: 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 544ba95..3b1e20e 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 @@ -11,6 +11,8 @@ import org.usfirst.frc4388.robot.commands.ArmSetPositionMM; import org.usfirst.frc4388.robot.commands.HatchFlip; import org.usfirst.frc4388.robot.commands.WristPlacement; import org.usfirst.frc4388.robot.commands.WristSetPositionPID; +import org.usfirst.frc4388.robot.commands.setLEDPattern; +import org.usfirst.frc4388.robot.constants.LEDPatterns; import edu.wpi.first.wpilibj.command.CommandGroup; @@ -22,6 +24,7 @@ public class StowArm extends CommandGroup { public StowArm() { addSequential(new HatchFlip(false)); addParallel(new WristPlacement(true)); + addParallel(new setLEDPattern(LEDPatterns.SOLID_GREEN)); addParallel(new WristSetPositionPID(110), 2); addSequential(new ArmSetPositionMM(10)); // Add Commands here: diff --git a/2019robot/src/main/java/org/usfirst/frc4388/robot/commands/presets/setWrist.java b/2019robot/src/main/java/org/usfirst/frc4388/robot/commands/presets/setWrist.java new file mode 100644 index 0000000..b4fba0c --- /dev/null +++ b/2019robot/src/main/java/org/usfirst/frc4388/robot/commands/presets/setWrist.java @@ -0,0 +1,38 @@ +/*----------------------------------------------------------------------------*/ +/* Copyright (c) 2018 FIRST. All Rights Reserved. */ +/* Open Source Software - may be modified and shared by FRC teams. The code */ +/* must be accompanied by the FIRST BSD license file in the root directory of */ +/* the project. */ +/*----------------------------------------------------------------------------*/ + +package org.usfirst.frc4388.robot.commands.presets; + +import org.usfirst.frc4388.robot.commands.WristSetPositionPID; + +import edu.wpi.first.wpilibj.command.CommandGroup; +import edu.wpi.first.wpilibj.command.WaitCommand; + +public class setWrist extends CommandGroup { + /** + * Add your docs here. + */ + public setWrist(double wrist) { + addSequential(new WaitCommand(1)); + addSequential(new WristSetPositionPID(wrist)); + // Add Commands here: + // addSequential(new Command2()); + // these will run in order. + + // To run multiple commands at the same time, + // use addParallel() + // e.g. addParallel(new Command1()); + // addSequential(new Command2()); + // Command1 and Command2 will run in parallel. + + // A command group will require all of the subsystems that each member + // would require. + // e.g. if Command1 requires chassis, and Command2 requires arm, + // a CommandGroup containing them would require both the chassis and the + // arm. + } +} diff --git a/2019robot/src/main/java/org/usfirst/frc4388/robot/commands/wristTest.java b/2019robot/src/main/java/org/usfirst/frc4388/robot/commands/wristTest.java index ee1fb58..c278e1a 100644 --- a/2019robot/src/main/java/org/usfirst/frc4388/robot/commands/wristTest.java +++ b/2019robot/src/main/java/org/usfirst/frc4388/robot/commands/wristTest.java @@ -14,7 +14,7 @@ public class wristTest extends CommandGroup { * Add your docs here. */ public wristTest() { - addSequential(new WristSetPositionPID(500)); + addSequential(new WristSetPositionMM(1000)); // 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 fc151f4..cf9aab3 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 @@ -64,18 +64,9 @@ public class Arm extends Subsystem implements ControlLoopable public static final double ZERO_POSITION_INCHES = -0.25; public static final double NEAR_ZERO_POSITION_INCHES = 0.0; public static final double MIN_POSITION_INCHES = -25; - public static final double MAX_POSITION_INCHES = 4400; + public static final double MAX_POSITION_INCHES = 4200; public static final double AFTER_INTAKE_POSITION_INCHES = 4.0; - public static final double SWITCH_POSITION_INCHES = 24.0; - public static final double SWITCH_POSITION_HIGH_INCHES = 36.0; //Switch Position for First Cube APR - public static final double SCALE_LOW_POSITION_INCHES = 56.0; - public static final double SCALE_FIRST_CUBE_POSITION_INCHES = 78.0; //72.0 - public static final double SCALE_SECOND_CUBE_POSITION_INCHES = 77.0; - public static final double SCALE_HIGH_POSITION_INCHES = MAX_POSITION_INCHES; - public static final double CLIMB_BAR_POSITION_INCHES = 70.0; - public static final double CLIMB_HIGH_POSITION_INCHES = 10.0; - public static final double CLIMB_ASSIST_POSITION_INCHES = 50.0; // Motion profile max velocities and accel times public static final double MP_MAX_VELOCITY_INCHES_PER_SEC = 60; @@ -99,20 +90,20 @@ public class Arm extends Subsystem implements ControlLoopable 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 = 0.5;// 2; - public static final double I_Value = 0.0008;// 0.00030; - public static final double D_Value = 100;// 200; + 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.08; + public static final double maxGravityComp = 0.01; public static final double RampRate = 0;// 0.0; - public static final int A_value = 450; - public static final int CV_value = 740; + public static final int A_value = 400; + public static final int CV_value = 500; private PIDParams armPIDParams = new PIDParams(P_Value, I_Value, D_Value, KF_DOWN); // KF gets updated later - public static final double PID_ERROR_INCHES = 5; + public static final double PID_ERROR_INCHES = 50; LimitSwitchSource limitSwitchSource; // Pneumatics @@ -149,7 +140,7 @@ public class Arm extends Subsystem implements ControlLoopable motor1.configPeakOutputReverse(-1, TalonSRXEncoder.TIMEOUT_MS); motor1.selectProfileSlot(MM_SLOT, 0); - motor1.config_kF(MM_SLOT, F_Value, TalonSRXEncoder.TIMEOUT_MS); + //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); @@ -179,8 +170,8 @@ public class Arm extends Subsystem implements ControlLoopable public void resetEncoder(){ motor1.setPosition(0); - targetPositionInchesMM = 0; - targetPositionInchesPID = 0; + //targetPositionInchesMM = 0; + //targetPositionInchesPID = 0; } private synchronized void setArmControlMode(ArmControlMode controlMode) { @@ -266,9 +257,9 @@ public class Arm extends Subsystem implements ControlLoopable } private double limitPosition(double targetPosition) { - /*if (targetPosition < MIN_POSITION_INCHES) { + if (targetPosition < MIN_POSITION_INCHES) { return MIN_POSITION_INCHES; - }*/ + } if (targetPosition > MAX_POSITION_INCHES) { return MAX_POSITION_INCHES; } @@ -334,7 +325,7 @@ public class Arm extends Subsystem implements ControlLoopable // Do the update if (armControlMode == ArmControlMode.JOYSTICK_MANUAL) { controlManualWithJoystick(); - System.err.println(motor1.getControlMode()); + //System.err.println(motor1.getControlMode()); } else if (!isFinished) { if (armControlMode == ArmControlMode.MOTION_PROFILE) { @@ -343,11 +334,11 @@ public class Arm extends Subsystem implements ControlLoopable } if (armControlMode == ArmControlMode.JOYSTICK_PID){ controlPidWithJoystick(); - System.err.println(motor1.getControlMode()); + //System.err.println(motor1.getControlMode()); } if (armControlMode == ArmControlMode.MOTION_MAGIC){ controlMMWithJoystick(); - System.err.println(motor1.getControlMode()); + //System.err.println(motor1.getControlMode()); } /*else if (armControlMode == ArmControlMode.MP_PATH_VELOCITY) { @@ -378,7 +369,7 @@ public class Arm extends Subsystem implements ControlLoopable targetPositionInchesMM = targetPositionInchesMM + deltaPosition; updatePositionMM(targetPositionInchesMM); //Robot.wrist.targetPositionInchesPID = targetPositionInchesPID - (deltaPosition/3); - Robot.wrist.updatePositionPID(Robot.wrist.targetPositionInchesPID); + //Robot.wrist.updatePositionPID(Robot.wrist.targetPositionInchesPID); } @@ -391,7 +382,7 @@ public class Arm extends Subsystem implements ControlLoopable private void controlManualWithJoystick() { double joyStickSpeed = -Robot.oi.getOperatorController().getLeftYAxis(); - setSpeedJoystick((joyStickSpeed*.3)+.08); + setSpeedJoystick(joyStickSpeed); } /* public void setShiftState(ElevatorSpeedShiftState state) { @@ -439,19 +430,19 @@ public class Arm extends Subsystem implements ControlLoopable //System.err.println("the encoder is right after this"); try { - SmartDashboard.putNumber("Arm Position Ticks", motor1.getPositionWorld()); + SmartDashboard.putNumber("Arm Ticks", motor1.getPositionWorld()); //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()); - SmartDashboard.putNumber("arm pid error", motor1.getClosedLoopError()); - SmartDashboard.putNumber("arm motor output", motor1.getMotorOutputPercent()); - SmartDashboard.putNumber("Arm Target MM Position", targetPositionInchesMM); + //SmartDashboard.putNumber("sensor vel", motor1.getSelectedSensorVelocity()); + //SmartDashboard.putNumber("Arm Average Amps", getAverageMotorCurrent()); + SmartDashboard.putNumber("Arm Error", motor1.getClosedLoopError()); + SmartDashboard.putNumber("Arm Amps", getAverageMotorCurrent()); + SmartDashboard.putNumber("Arm Target MM", targetPositionInchesMM); //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)); -// SmartDashboard.putNumber("Elevator Motor 3 Amps PDP", Robot.pdp.getCurrent(RobotMap.ELEVATOR_MOTOR_3_CAN_ID)); - SmartDashboard.putNumber("Arm Target PID Position", targetPositionInchesPID); + //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)); + //SmartDashboard.putNumber("Elevator Motor 3 Amps PDP", Robot.pdp.getCurrent(RobotMap.ELEVATOR_MOTOR_3_CAN_ID)); + //SmartDashboard.putNumber("Arm Target PID Position", targetPositionInchesPID); } catch (Exception e) { System.err.println("Arm update status error" +e.getMessage()); diff --git a/2019robot/src/main/java/org/usfirst/frc4388/robot/subsystems/Wrist.java b/2019robot/src/main/java/org/usfirst/frc4388/robot/subsystems/Wrist.java index b28e064..6d23b60 100644 --- a/2019robot/src/main/java/org/usfirst/frc4388/robot/subsystems/Wrist.java +++ b/2019robot/src/main/java/org/usfirst/frc4388/robot/subsystems/Wrist.java @@ -12,6 +12,7 @@ import org.usfirst.frc4388.utility.TalonSRXEncoder; import org.usfirst.frc4388.utility.TalonSRXFactory; import com.ctre.phoenix.motorcontrol.ControlMode; +import com.ctre.phoenix.motorcontrol.DemandType; import com.ctre.phoenix.motorcontrol.FeedbackDevice; import com.ctre.phoenix.motorcontrol.can.TalonSRX; import com.ctre.phoenix.motorcontrol.LimitSwitchSource; @@ -30,7 +31,7 @@ public class Wrist extends Subsystem implements ControlLoopable { private static Wrist instance; - public static enum WristControlMode { MOTION_PROFILE, JOYSTICK_PID, JOYSTICK_MANUAL, MANUAL }; + public static enum WristControlMode { MOTION_PROFILE, JOYSTICK_PID, JOYSTICK_MANUAL, MANUAL, MOTION_MAGIC }; // 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 public static final double ENCODER_TICKS_TO_INCHES = (1); @@ -79,16 +80,21 @@ public class Wrist extends Subsystem implements ControlLoopable private MPTalonPIDController mpController; public static int PID_SLOT = 0; - public static int MP_SLOT = 1; + public static int MM_SLOT = 1; + public static int MP_SLOT = 2; 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 = 0.01; public static final double KF_DOWN = 0.0; - public static final double P_Value = 1; - public static final double I_Value = 0.0001; - public static final double D_Value = 100; + public static final double P_Value = 4; + public static final double I_Value = 0.0000; + public static final double D_Value = 000; + public static final double F_Value = 1; + public static final int CV_value = 100; + public static final int A_value = 100; public static final double RampRate = 0.0; + public static final double maxGravityComp = 0; private PIDParams wristPIDParams = new PIDParams(P_Value, I_Value, D_Value, KF_DOWN); // KF gets updated later public static final double PID_ERROR_INCHES = 150; LimitSwitchSource limitSwitchSource; @@ -98,8 +104,9 @@ public class Wrist extends Subsystem implements ControlLoopable // Misc public static final double AUTO_ZERO_MOTOR_CURRENT = 4.0; private boolean isFinished; - private WristControlMode wristControlMode = WristControlMode.JOYSTICK_PID; + private WristControlMode wristControlMode = WristControlMode.MOTION_MAGIC; public double targetPositionInchesPID = 0; + public double targetPositionInchesMM = 0; private boolean firstMpPoint; private double joystickInchesPerMs = JOYSTICK_INCHES_PER_MS_LO; private double p = 0; @@ -120,9 +127,18 @@ public class Wrist extends Subsystem implements ControlLoopable wristMotor1.setNeutralMode(NeutralMode.Brake); wristMotor1.enableCurrentLimit(true); //wristMotor1.setSensorPhase(true); + + wristMotor1.configNominalOutputForward(0, TalonSRXEncoder.TIMEOUT_MS); + wristMotor1.configNominalOutputReverse(0, TalonSRXEncoder.TIMEOUT_MS); + wristMotor1.configPeakOutputForward(1, TalonSRXEncoder.TIMEOUT_MS); + wristMotor1.configPeakOutputReverse(-1, TalonSRXEncoder.TIMEOUT_MS); + + wristMotor1.selectProfileSlot(MM_SLOT, 0); + wristMotor1.config_kF(MM_SLOT, F_Value, TalonSRXEncoder.TIMEOUT_MS); + wristMotor1.config_kP(MM_SLOT, P_Value, TalonSRXEncoder.TIMEOUT_MS); + wristMotor1.config_kI(MM_SLOT, I_Value, TalonSRXEncoder.TIMEOUT_MS); + wristMotor1.config_kD(MM_SLOT, D_Value, TalonSRXEncoder.TIMEOUT_MS); motorControllers.add(wristMotor1); - - } catch (Exception e) { System.err.println("An error occurred in the Wrist constructor"); @@ -160,6 +176,37 @@ public class Wrist extends Subsystem implements ControlLoopable setWristControlMode(WristControlMode.JOYSTICK_MANUAL); } + public void setPositionMM(double targetPositionInches){ + wristMotor1.set(ControlMode.MotionMagic, targetPositionInches); + //System.err.println(wristMotor1.getControlMode()); + wristMotor1.selectProfileSlot(MM_SLOT, 0); + setWristControlMode(WristControlMode.MOTION_MAGIC); + updatePositionMM(targetPositionInches); + setFinished(false); + } + + public double calcGravityCompensationAtCurrentPosition() { + int ticks = wristMotor1.getSelectedSensorPosition(); + double degreesFromDown = (ticks+920)*(360.0/(4096*3)); + double compensation = maxGravityComp * Math.sin(degreesFromDown*Math.PI/180); + //System.err.println("comp(" + degreesFromDown + "^) = " + compensation); + return compensation; + } + public void updatePositionMM(double targetPositionInches){ + targetPositionInchesMM = limitPosition(targetPositionInches); + System.err.println("we are here"); + //double startPositionInches = motor1.getPositionWorld(); + //System.err.println("compensation = " + compensation); + wristMotor1.set(ControlMode.MotionMagic, targetPositionInches); + //wristMotor1.set(ControlMode.MotionMagic, targetPositionInchesMM, DemandType.ArbitraryFeedForward, 0); + //System.err.println(motor1.getControlMode()); + wristMotor1.configMotionCruiseVelocity(CV_value, TalonSRXEncoder.TIMEOUT_MS); + wristMotor1.configMotionAcceleration(A_value, TalonSRXEncoder.TIMEOUT_MS); + + + } + + public void setPositionPID(double targetPositionInches) { wristMotor1.set(ControlMode.Position, targetPositionInches); mpController.setPIDSlot(PID_SLOT); //TODO: verify that motor's selectProfileSlot() should be called AFTER its control mode is set @@ -279,6 +326,10 @@ public class Wrist extends Subsystem implements ControlLoopable isFinished = mpController.controlLoopUpdate(getPositionInches()); } + if (wristControlMode == WristControlMode.MOTION_MAGIC){ + controlMMWithJoystick(); + //System.err.println(wristMotor1.getControlMode()); + } if (wristControlMode == WristControlMode.JOYSTICK_PID){ //System.err.println(wristMotor1.getControlMode()); controlPidWithJoystick(); @@ -298,7 +349,14 @@ public class Wrist extends Subsystem implements ControlLoopable - + private void controlMMWithJoystick() { + double joystickPosition = -Robot.oi.getOperatorController().getRightYAxis(); + double deltaPosition = joystickPosition * joystickInchesPerMs; + targetPositionInchesMM = targetPositionInchesMM + deltaPosition; + updatePositionMM(targetPositionInchesMM); + //Robot.wrist.targetPositionInchesPID = targetPositionInchesPID - (deltaPosition/3); + + } @@ -319,7 +377,7 @@ public class Wrist extends Subsystem implements ControlLoopable private void controlManualWithJoystick() { double joyStickSpeed = -Robot.oi.getOperatorController().getRightYAxis(); - setSpeedJoystick(joyStickSpeed*.5); + setSpeedJoystick(joyStickSpeed); } /* public void setShiftState(ElevatorSpeedShiftState state) { @@ -365,15 +423,16 @@ public class Wrist extends Subsystem implements ControlLoopable //System.err.println("the encoder is right after this"); try { - SmartDashboard.putNumber("Wrist Position Ticks", wristMotor1.getPositionWorld()); - SmartDashboard.putNumber("Wrist Motor 1 Amps", wristMotor1.getOutputCurrent()); - SmartDashboard.putNumber("wrist pid error", wristMotor1.getClosedLoopError()); + SmartDashboard.putNumber("Wrist Ticks", wristMotor1.getPositionWorld()); + SmartDashboard.putNumber("Wrist Amps", wristMotor1.getOutputCurrent()); + SmartDashboard.putNumber("wrist error", wristMotor1.getClosedLoopError()); SmartDashboard.putNumber("wrist motor output", wristMotor1.getMotorOutputPercent()); - + SmartDashboard.putNumber("wrist velocity", wristMotor1.getSelectedSensorVelocity()); + SmartDashboard.putNumber("Wrist Target MM", targetPositionInchesMM); // 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)); // SmartDashboard.putNumber("Elevator Motor 3 Amps PDP", Robot.pdp.getCurrent(RobotMap.ELEVATOR_MOTOR_3_CAN_ID)); - SmartDashboard.putNumber("Wrist Target PID Position", targetPositionInchesPID); + SmartDashboard.putNumber("Wrist Target PID", targetPositionInchesPID); } catch (Exception e) { System.err.println("Drivetrain update status error" +e.getMessage()); diff --git a/2019robot/src/main/java/org/usfirst/frc4388/utility/control/RobotState.java b/2019robot/src/main/java/org/usfirst/frc4388/utility/control/RobotState.java index 690c1d0..d9edd5c 100644 --- a/2019robot/src/main/java/org/usfirst/frc4388/utility/control/RobotState.java +++ b/2019robot/src/main/java/org/usfirst/frc4388/utility/control/RobotState.java @@ -124,10 +124,10 @@ public class RobotState { public void updateStatus(OperationMode operationMode) { if (operationMode == OperationMode.TEST) { RigidTransform2d odometry = getLatestFieldToVehicle().getValue(); - SmartDashboard.putNumber("robot_pose_x", odometry.getTranslation().x()); - SmartDashboard.putNumber("robot_pose_y", odometry.getTranslation().y()); - SmartDashboard.putNumber("robot_pose_theta", odometry.getRotation().getDegrees()); - SmartDashboard.putNumber("robot velocity", vehicle_velocity_measured_.dx); + //SmartDashboard.putNumber("robot_pose_x", odometry.getTranslation().x()); + //SmartDashboard.putNumber("robot_pose_y", odometry.getTranslation().y()); + //SmartDashboard.putNumber("robot_pose_theta", odometry.getRotation().getDegrees()); + // SmartDashboard.putNumber("robot velocity", vehicle_velocity_measured_.dx); } } }