From 860fb33c5a177b586b4b69280cff7f6aadd4ddf9 Mon Sep 17 00:00:00 2001 From: lukesta182 <33330639+lukesta182@users.noreply.github.com> Date: Fri, 15 Feb 2019 17:12:24 -0700 Subject: [PATCH] Revert "Revert "Merge branch 'Wrist' of https://github.com/Team4388/2019-Hit-or-Miss into Wrist"" This reverts commit 1dab4878f04d8619912769db46b2fd99fc5c8139. --- .../java/org/usfirst/frc4388/robot/Robot.java | 14 +- .../org/usfirst/frc4388/robot/RobotMap.java | 12 +- .../frc4388/robot/commands/FlipIntake.java | 53 ++++++ .../robot/commands/GrabBallOutOfRobot.java | 44 ++--- .../robot/commands/GrabHatchOffWall.java | 36 ++++ .../commands/GrabHatchOutOfFloorIntake.java | 37 ++++ .../robot/commands/RunWristMotorJumpBar.java | 53 ++++++ .../robot/commands/StopWristMotor.java | 52 ++++++ .../usfirst/frc4388/robot/subsystems/Arm.java | 161 +++++++++--------- .../frc4388/robot/subsystems/Drive.java | 111 ++++++++++++ .../frc4388/robot/subsystems/Wrist.java | 84 +++++++-- .../frc4388/utility/MMTalonPIDController.java | 2 +- .../frc4388/utility/PathGenerator.java | 2 +- 13 files changed, 534 insertions(+), 127 deletions(-) create mode 100644 2019robot/src/main/java/org/usfirst/frc4388/robot/commands/FlipIntake.java create mode 100644 2019robot/src/main/java/org/usfirst/frc4388/robot/commands/GrabHatchOffWall.java create mode 100644 2019robot/src/main/java/org/usfirst/frc4388/robot/commands/GrabHatchOutOfFloorIntake.java create mode 100644 2019robot/src/main/java/org/usfirst/frc4388/robot/commands/RunWristMotorJumpBar.java create mode 100644 2019robot/src/main/java/org/usfirst/frc4388/robot/commands/StopWristMotor.java 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 e681d58..e889ff5 100644 --- a/2019robot/src/main/java/org/usfirst/frc4388/robot/Robot.java +++ b/2019robot/src/main/java/org/usfirst/frc4388/robot/Robot.java @@ -12,7 +12,7 @@ import org.usfirst.frc4388.robot.subsystems.Pnumatics; import org.usfirst.frc4388.robot.subsystems.Wrist; //import org.usfirst.frc4388.utility.ControlLooper; import org.usfirst.frc4388.utility.Looper; -import org.usfirst.frc4388.utility.control.RobotStateEstimator; +//import org.usfirst.frc4388.utility.control.RobotStateEstimator; import org.usfirst.frc4388.utility.math.RigidTransform2d; import org.usfirst.frc4388.utility.control.RobotState; @@ -32,7 +32,7 @@ public class Robot extends TimedRobot public static OI oi; - public static final Drive drive = Drive.getInstance(); + //public static final Drive drive = Drive.getInstance(); public static final Arm arm = Arm.getInstance(); @@ -71,9 +71,9 @@ public class Robot extends TimedRobot setPeriod(Constants.kLooperDt * 2); System.out.println("Main loop period = " + getPeriod()); oi = OI.getInstance(); - controlLoop.register(drive); + //controlLoop.register(drive); controlLoop.register(arm); - controlLoop.register(RobotStateEstimator.getInstance()); + //controlLoop.register(RobotStateEstimator.getInstance()); operationModeChooser = new SendableChooser(); operationModeChooser.addDefault("Competition", OperationMode.COMPETITION); @@ -93,7 +93,7 @@ public class Robot extends TimedRobot @Override public void disabledInit() { - drive.setLimeLED(false); + //drive.setLimeLED(false); } @Override public void disabledPeriodic() { @@ -103,7 +103,7 @@ public class Robot extends TimedRobot @Override public void autonomousInit() { controlLoop.start(); - drive.setIsRed(getAlliance().equals(Alliance.Red)); + //drive.setIsRed(getAlliance().equals(Alliance.Red)); arm.resetZeroPosition(Arm.ZERO_POSITION_INCHES); updateChoosers(); drive.endGyroCalibration(); @@ -171,7 +171,7 @@ public class Robot extends TimedRobot public void updateStatus() { - drive.updateStatus(operationMode); + //drive.updateStatus(operationMode); arm.updateStatus(operationMode); robotState.updateStatus(operationMode); diff --git a/2019robot/src/main/java/org/usfirst/frc4388/robot/RobotMap.java b/2019robot/src/main/java/org/usfirst/frc4388/robot/RobotMap.java index efa00b7..d925b57 100644 --- a/2019robot/src/main/java/org/usfirst/frc4388/robot/RobotMap.java +++ b/2019robot/src/main/java/org/usfirst/frc4388/robot/RobotMap.java @@ -2,7 +2,8 @@ package org.usfirst.frc4388.robot; -public class RobotMap { +public class RobotMap +{ // USB Port IDs public static final int DRIVER_JOYSTICK_1_USB_ID = 0; public static final int OPERATOR_JOYSTICK_1_USB_ID = 1; @@ -11,16 +12,14 @@ public class RobotMap { public static final int copilot = 1; // MOTORS - public static final int DRIVETRAIN_LEFT_MOTOR1_CAN_ID = 2; public static final int DRIVETRAIN_LEFT_MOTOR2_CAN_ID = 3; - public static final int DRIVETRAIN_RIGHT_MOTOR1_CAN_ID = 4; public static final int DRIVETRAIN_RIGHT_MOTOR2_CAN_ID = 5; //carriage motors - public static final int WRIST_LEFT_MOTOR_CAN_ID = 6; + public static final int WRIST_RIGHT_MOTOR_CAN_ID = 6; public static final int INTAKE_MOTOR = 7; //Arm Motors @@ -31,8 +30,6 @@ public class RobotMap { public static final int CLIMBER_LEFT = 12; public static final int CLIMBER_RIGHT = 13; - - // Pneumatics public static final int DRIVETRAIN_SPEEDSHIFT_PCM_ID = 0; public static final int DRIVETRAIN_SPEEDSHIFT2_PCM_ID = 1; @@ -42,7 +39,4 @@ public class RobotMap { public static final int LOWER_HATCH_INTAKE_PCM_ID = 5; public static final int END_EFECTOR_EXPAND_PCM_ID = 6; public static final int END_EFECTOR_CONTRACT_PCM_ID = 7; - - - } diff --git a/2019robot/src/main/java/org/usfirst/frc4388/robot/commands/FlipIntake.java b/2019robot/src/main/java/org/usfirst/frc4388/robot/commands/FlipIntake.java new file mode 100644 index 0000000..fc232a9 --- /dev/null +++ b/2019robot/src/main/java/org/usfirst/frc4388/robot/commands/FlipIntake.java @@ -0,0 +1,53 @@ +/*----------------------------------------------------------------------------*/ +/* 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; + +import org.usfirst.frc4388.robot.*; + +import edu.wpi.first.wpilibj.command.Command; + +public class FlipIntake extends Command +{ + public FlipIntake() + { + requires(Robot.wrist); + } + + // Called just before this Command runs the first time + @Override + protected void initialize() + { + Robot.wrist.controlPIDFlipIntake(); + } + + // Called repeatedly when this Command is scheduled to run + @Override + protected void execute() + { + } + + // Make this return true when this Command no longer needs to run execute() + @Override + protected boolean isFinished() + { + return false; + } + + // Called once after isFinished returns true + @Override + protected void end() + { + } + + // Called when another command which requires one or more of the same + // subsystems is scheduled to run + @Override + protected void interrupted() + { + } +} diff --git a/2019robot/src/main/java/org/usfirst/frc4388/robot/commands/GrabBallOutOfRobot.java b/2019robot/src/main/java/org/usfirst/frc4388/robot/commands/GrabBallOutOfRobot.java index 2ab8874..6aa12c0 100644 --- a/2019robot/src/main/java/org/usfirst/frc4388/robot/commands/GrabBallOutOfRobot.java +++ b/2019robot/src/main/java/org/usfirst/frc4388/robot/commands/GrabBallOutOfRobot.java @@ -7,29 +7,35 @@ package org.usfirst.frc4388.robot.commands; -import edu.wpi.first.wpilibj.command.CommandGroup; +import org.usfirst.frc4388.robot.Robot; -public class GrabBallOutOfRobot extends CommandGroup { - /** - * Add your docs here. - */ +import edu.wpi.first.wpilibj.command.CommandGroup; +import edu.wpi.first.wpilibj.command.WaitCommand; + +/** +* Add your docs here. +*/ + +public class GrabBallOutOfRobot extends CommandGroup +{ public GrabBallOutOfRobot() { - // Add Commands here: - // e.g. addSequential(new Command1()); - // addSequential(new Command2()); - // these will run in order. + //Add rest of sequentials for this command group - // 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. + addSequential(new FlipIntake()); + //Move Arm until bar jump angle - // 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. + //Jump bar in robot + if(Robot.wrist.armAngleDegrees <= Robot.wrist.jumpBarArmAngle && Robot.wrist.armAngleDegrees >= Robot.wrist.jumpBarArmAngle) + { + addParallel(new WaitCommand(2)); + addSequential(new RunWristMotorJumpBar()); + addSequential(new StopWristMotor()); + } + + //continue arm movement until ball is in intake ///SENSOR for ball in robot + + //Move ball out of robot to 360 degrees + //move arm and wrist in parallel } } diff --git a/2019robot/src/main/java/org/usfirst/frc4388/robot/commands/GrabHatchOffWall.java b/2019robot/src/main/java/org/usfirst/frc4388/robot/commands/GrabHatchOffWall.java new file mode 100644 index 0000000..d7f0377 --- /dev/null +++ b/2019robot/src/main/java/org/usfirst/frc4388/robot/commands/GrabHatchOffWall.java @@ -0,0 +1,36 @@ +/*----------------------------------------------------------------------------*/ +/* 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; + +import edu.wpi.first.wpilibj.command.CommandGroup; + +/** +* Add your docs here. +*/ + +public class GrabHatchOffWall extends CommandGroup +{ + public GrabHatchOffWall() { + // 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/GrabHatchOutOfFloorIntake.java b/2019robot/src/main/java/org/usfirst/frc4388/robot/commands/GrabHatchOutOfFloorIntake.java new file mode 100644 index 0000000..b6c100c --- /dev/null +++ b/2019robot/src/main/java/org/usfirst/frc4388/robot/commands/GrabHatchOutOfFloorIntake.java @@ -0,0 +1,37 @@ +/*----------------------------------------------------------------------------*/ +/* 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; + +import edu.wpi.first.wpilibj.command.CommandGroup; + +/** +* Add your docs here. +*/ + +public class GrabHatchOutOfFloorIntake extends CommandGroup +{ + public GrabHatchOutOfFloorIntake() + { + // 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/RunWristMotorJumpBar.java b/2019robot/src/main/java/org/usfirst/frc4388/robot/commands/RunWristMotorJumpBar.java new file mode 100644 index 0000000..6fe8b19 --- /dev/null +++ b/2019robot/src/main/java/org/usfirst/frc4388/robot/commands/RunWristMotorJumpBar.java @@ -0,0 +1,53 @@ +/*----------------------------------------------------------------------------*/ +/* 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; + +import org.usfirst.frc4388.robot.*; + +import edu.wpi.first.wpilibj.command.Command; + +public class RunWristMotorJumpBar extends Command +{ + public RunWristMotorJumpBar() + { + requires(Robot.wrist); + } + + // Called just before this Command runs the first time + @Override + protected void initialize() + { + Robot.wrist.jumpBar(); + } + + // Called repeatedly when this Command is scheduled to run + @Override + protected void execute() + { + } + + // Make this return true when this Command no longer needs to run execute() + @Override + protected boolean isFinished() + { + return false; + } + + // Called once after isFinished returns true + @Override + protected void end() + { + } + + // Called when another command which requires one or more of the same + // subsystems is scheduled to run + @Override + protected void interrupted() + { + } +} diff --git a/2019robot/src/main/java/org/usfirst/frc4388/robot/commands/StopWristMotor.java b/2019robot/src/main/java/org/usfirst/frc4388/robot/commands/StopWristMotor.java new file mode 100644 index 0000000..164e14f --- /dev/null +++ b/2019robot/src/main/java/org/usfirst/frc4388/robot/commands/StopWristMotor.java @@ -0,0 +1,52 @@ +/*----------------------------------------------------------------------------*/ +/* 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; + +import org.usfirst.frc4388.robot.*; + +import edu.wpi.first.wpilibj.command.Command; + +public class StopWristMotor extends Command +{ + public StopWristMotor() + { + requires(Robot.wrist); + } + + // Called just before this Command runs the first time + @Override + protected void initialize() + { + Robot.wrist.stopMotor(); + } + + // Called repeatedly when this Command is scheduled to run + @Override + protected void execute() + { + } + + // Make this return true when this Command no longer needs to run execute() + @Override + protected boolean isFinished() + { + return false; + } + + // Called once after isFinished returns true + @Override + protected void end() + { + } + + // Called when another command which requires one or more of the same + // subsystems is scheduled to run + @Override + protected void interrupted() { + } +} 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 4b992c4..9a97637 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 @@ -1,6 +1,7 @@ package org.usfirst.frc4388.robot.subsystems; import java.util.ArrayList; +import org.usfirst.frc4388.controller.XboxController; import org.usfirst.frc4388.robot.Constants; import org.usfirst.frc4388.robot.Robot; import org.usfirst.frc4388.robot.RobotMap; @@ -11,6 +12,15 @@ import org.usfirst.frc4388.utility.TalonSRXEncoder; import org.usfirst.frc4388.utility.TalonSRXFactory; import com.ctre.phoenix.motorcontrol.ControlMode; + +import edu.wpi.first.wpilibj.DigitalInput; +import edu.wpi.first.wpilibj.Encoder; +import edu.wpi.first.wpilibj.PIDController; +import edu.wpi.first.wpilibj.SpeedControllerGroup; +import edu.wpi.first.wpilibj.command.Subsystem; +import edu.wpi.first.wpilibj.drive.DifferentialDrive; +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; + import com.ctre.phoenix.motorcontrol.FeedbackDevice; import com.ctre.phoenix.motorcontrol.can.TalonSRX; @@ -20,7 +30,9 @@ import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; public class Arm extends Subsystem implements Loop { - private static Arm instance; + //PID encoder and motor + private CANTalonEncoder armMotor; + //private WPI_TalonSRX ArmLeft; public static enum ArmControlMode { MOTION_PROFILE, JOYSTICK_PID, JOYSTICK_MANUAL, MANUAL }; @@ -58,10 +70,11 @@ public class Arm extends Subsystem implements Loop public static final double MP_T1 = 400; // Fast = 300 public static final double MP_T2 = 150; // Fast = 150 - // Motor controllers - private ArrayList motorControllers = new ArrayList(); + //PID controller Max Scale + private SoftwarePIDPositionController pidPositionControllerLowest; + //private PIDParams PositionPIDParamsLowest = new PIDParams(2.0, 0.0, 0.0); + private PIDParams PositionPLowest; - private TalonSRXEncoder motor1; private TalonSRX motor2; // PID controller and params @@ -105,67 +118,51 @@ public class Arm extends Subsystem implements Loop } - catch (Exception e) { - System.err.println("An error occurred in the DriveTrain constructor"); - } - } - - @Override - public void initDefaultCommand() { - } - - public void resetZeroPosition(double position) { - mpController.resetZeroPosition(position); - } - - private synchronized void setElevatorControlMode(ArmControlMode controlMode) { - this.elevatorControlMode = controlMode; - } - - private synchronized ArmControlMode getElevatorControlMode() { - return this.elevatorControlMode; - } - - public void setSpeed(double speed) { - motor1.set(ControlMode.PercentOutput, speed); - setElevatorControlMode(ArmControlMode.MANUAL); - } - - public void setSpeedJoystick(double speed) { - motor1.set(ControlMode.PercentOutput, speed); - setElevatorControlMode(ArmControlMode.JOYSTICK_MANUAL); - } - - public void setPositionPID(double targetPositionInches) { - mpController.setPIDSlot(PID_SLOT); - updatePositionPID(targetPositionInches); - setElevatorControlMode(ArmControlMode.JOYSTICK_PID); - setFinished(false); - } - - public void updatePositionPID(double targetPositionInches) { - targetPositionInchesPID = limitPosition(targetPositionInches); - double startPositionInches = motor1.getPositionWorld(); - mpController.setTarget(targetPositionInchesPID, targetPositionInchesPID > startPositionInches ? KF_UP : KF_DOWN); - } - - public void setPositionMP(double targetPositionInches) { - double startPositionInches = motor1.getPositionWorld(); - mpController.setMPTarget(startPositionInches, limitPosition(targetPositionInches), MP_MAX_VELOCITY_INCHES_PER_SEC, MP_T1, MP_T2); - setFinished(false); firstMpPoint = true; setElevatorControlMode(ArmControlMode.MOTION_PROFILE); } + + + //PID encoder position + public double getEncoderArmPosition() + { + return armMotor.getPositionWorld(); + } - private double limitPosition(double targetPosition) { - if (targetPosition < MIN_POSITION_INCHES) { - return MIN_POSITION_INCHES; - } - else if (targetPosition > MAX_POSITION_INCHES) { - return MAX_POSITION_INCHES; - } - - return targetPosition; + public double getArmHeightInchesAboveFloor() + { + return armMotor.getPositionWorld(); + } + + public synchronized void setControlMode(DriveControlMode controlMode) + { + this.controlMode = controlMode; + + isFinished = false; + } + /* + public void setArmPIDMaxScale(double ArmPosition, double maxError, double minError) + { + double ArmTargetPos = 0; + this.targetPPosition = ArmTargetPos; + pidPositionControllerMaxScale.setPIDPositionTarget(ArmTargetPos, maxError, minError); ///////TARGET POSITION WHERE?? + Robot.Arm.setControlMode(DriveControlMode.MOVE_POSITION_MAX_SCALE); + } + + public void setArmPIDLowScale(double ArmPosition, double maxError, double minError) + { + double ArmTargetPos = 0; + this.targetPPosition = ArmTargetPos; + pidPositionControllerMaxScale.setPIDPositionTarget(ArmTargetPos, maxError, minError); + Robot.Arm.setControlMode(DriveControlMode.MOVE_POSITION_LOW_SCALE); + } + + public void setArmPIDSwitch(double ArmPosition, double maxError, double minError) + { + double ArmTargetPos = 0; + this.targetPPosition = ArmTargetPos; + pidPositionControllerMaxScale.setPIDPositionTarget(ArmTargetPos, maxError, minError); + Robot.Arm.setControlMode(DriveControlMode.MOVE_POSITION_SWITCH); } @Override @@ -177,11 +174,9 @@ public class Arm extends Subsystem implements Loop mpController.setPID(pidPIDParamsHiGear, PID_SLOT); mpController.setPIDSlot(PID_SLOT); } - - @Override - public void onStop(double timestamp) { - // TODO Auto-generated method stub - + */ + public void rawSetOutput(double output){ + armMotor.set(/*ControlMode.PercentOutput,*/ output); } @Override @@ -209,8 +204,24 @@ public class Arm extends Subsystem implements Loop default: break; } + pressed = true; } + else + { + if (controlMode == DriveControlMode.STOP_MOTORS){ + { + Robot.arm.setControlMode(DriveControlMode.JOYSTICK); + } + + pressed = false; + } + } + } + + //pressed = (isPressed.isFwdLimitSwitchClosed() == true) ? true : false; + + private void controlPidWithJoystick() { double joystickPosition = -Robot.oi.getOperatorController().getLeftYAxis(); @@ -258,11 +269,8 @@ public class Arm extends Subsystem implements Loop return isFinished; } - public synchronized void setFinished(boolean isFinished) { - this.isFinished = isFinished; - } - - public double getPeriodMs() { + public double getPeriodMs() + { return periodMs; } @@ -278,15 +286,10 @@ public class Arm extends Subsystem implements Loop // SmartDashboard.putNumber("Elevator Motor 3 Amps PDP", Robot.pdp.getCurrent(RobotMap.ELEVATOR_MOTOR_3_CAN_ID)); SmartDashboard.putNumber("Elevator Target PID Position", targetPositionInchesPID); } - catch (Exception e) { + catch (Exception e) + { + System.err.println("Drivetrain update status error"); } } - } - - public static Arm getInstance() { - if(instance == null) { - instance = new Arm(); - } - return instance; } } \ No newline at end of file diff --git a/2019robot/src/main/java/org/usfirst/frc4388/robot/subsystems/Drive.java b/2019robot/src/main/java/org/usfirst/frc4388/robot/subsystems/Drive.java index 058869a..09fca73 100644 --- a/2019robot/src/main/java/org/usfirst/frc4388/robot/subsystems/Drive.java +++ b/2019robot/src/main/java/org/usfirst/frc4388/robot/subsystems/Drive.java @@ -930,3 +930,114 @@ public class Drive extends Subsystem implements Loop { } } + + + + + + + + + + + + + + + + + + + + +/*----------------------------------------------------------------------------*/ +/* 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. */ +/*----------------------------------------------------------------------------*/ + + + +import frc.robot.commands.DriveCommand; +import com.revrobotics.CANSparkMax; +import com.revrobotics.ControlType; +import com.revrobotics.CANPIDController; +import com.revrobotics.CANEncoder; + +import com.revrobotics.CANSparkMaxLowLevel.MotorType; + +import edu.wpi.first.wpilibj.command.Subsystem; + + +/** + * Add your docs here. + */ +public class Drive extends Subsystem { + + static double LOW_GEAR_RATIO = 4.821;//6.57992 + static double HIGH_GEAR_RATIO = ; + static double CIRCUMFERENCE = 15.221; + double HIGH_INCHES_PER_REV = CIRCUMFERENCE/HIGH_GEAR_RATIO; + double LOW_INCHES_PER_REV = CIRCUMFERENCE/LOW_GEAR_RATIO; + int timeoutMs = 10; + + // Front is the follow motor, and it is based on following the primary motor of its side. + public CANSparkMax leftDrivePrimary = new CANSparkMax(RobotMap.DRIVETRAIN_LEFT_MOTOR1_CAN_ID, MotorType.kBrushless), + leftDriveFront = new CANSparkMax(RobotMap.DRIVETRAIN_LEFT_MOTOR2_CAN_ID, MotorType.kBrushless), + rightDriveFront = new CANSparkMax(RobotMap.DRIVETRAIN_RIGHT_MOTOR1_CAN_ID, MotorType.kBrushless), + rightDrivePrimary = new CANSparkMax(RobotMap.DRIVETRAIN_RIGHT_MOTOR2_CAN_ID, MotorType.kBrushless); + + + public CANPIDController leftPID = new CANPIDController(leftDrivePrimary); + public CANPIDController rightPID = new CANPIDController(rightDrivePrimary); + + public CANEncoder leftEncoder = new CANEncoder(leftDrivePrimary); + public CANEncoder rightEncoder = new CANEncoder(rightDrivePrimary); + + + @Override + public void initDefaultCommand() { + setDefaultCommand(new DriveCommand()); + + leftDriveFront.follow(leftDrivePrimary); + rightDriveFront.follow(rightDrivePrimary); + + leftDrivePrimary.setCANTimeout(timeoutMs); + rightDrivePrimary.setCANTimeout(timeoutMs); + leftDrivePrimary.setMotorType(MotorType.kBrushless); + rightDrivePrimary.setMotorType(MotorType.kBrushless); + + } + + public void set(double left, double right) { + leftDrivePrimary.set(left); + rightDrivePrimary.set(right); + } + + public double getLeftRevs() { + return leftEncoder.getPosition(); + } + + public double getRightRevs() { + return rightEncoder.getPosition(); + } + + public double lowinchesToRevs(double INCHES) { + return INCHES/LOW_INCHES_PER_REV; + } + public double highinchesToRevs(double INCHES) { + return INCHES/HIGH_INCHES_PER_REV; + } + + // Takes an input of ticks + public void setPID(double left, double right) { + leftPID.setReference(left, ControlType.kPosition); + rightPID.setReference(right, ControlType.kPosition); + } + + public void stop() { + leftDrivePrimary.set(0); + rightDrivePrimary.set(0); + } +} 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 c936377..dfd4bb9 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 @@ -26,6 +26,11 @@ import com.ctre.phoenix.motorcontrol.LimitSwitchSource; import com.ctre.phoenix.motorcontrol.NeutralMode; import com.ctre.phoenix.motorcontrol.SensorCollection; import com.ctre.phoenix.motorcontrol.can.WPI_TalonSRX; +import edu.wpi.first.wpilibj.command.WaitCommand; + +import com.ctre.phoenix.motorcontrol.LimitSwitchNormal; +import com.ctre.phoenix.motorcontrol.RemoteLimitSwitchSource; +import com.ctre.phoenix.motorcontrol.LimitSwitchSource; import com.ctre.phoenix.motorcontrol.ControlMode; @@ -36,7 +41,7 @@ import com.ctre.phoenix.motorcontrol.ControlMode; public class Wrist extends Subsystem { //Control Mode Array - public static enum WristControlMode {PID, JOYSTICK_MANUAL, FLIP_INTAKE, GRAB_BALL}; + public static enum WristControlMode {PID, JOYSTICK_MANUAL, GRAB_BALL}; //Motor Controllers private ArrayList motorControllers = new ArrayList(); @@ -72,13 +77,19 @@ public class Wrist extends Subsystem public double armAngleDegrees = 90; - public static final double targetAngleDegreesBallIn = -45; ///Change values + public static final double targetAngleDegreesBallIn = 180; //only have to flip intake and go down to get ball + ///Change values public static final double targetAngleDegreesBallOut = 360; - public static final double targetAngleDegreesHatchIn = 55; + public static final double targetAngleDegreesHatchIn = 130; public static final double targetAngleDegreesHatchOut = 0; - public static final double jumpBarAngle = 50; //hard limit switch? + public final double jumpBarArmAngle = -50; public static final double armAngleForPIDSwitch = -45; ///Change values + + public static final boolean ballIntakeOut = true; + + //control mode for joystick control + private DriveControlMode controlMode = DriveControlMode.JOYSTICK; //Misc private WristControlMode wristControlMode = WristControlMode.JOYSTICK_MANUAL; @@ -89,12 +100,18 @@ public class Wrist extends Subsystem private double joystickInchesPerMs = JOYSTICK_INCHES_PER_MS_LO; private double joystickDegreesPerMs = JOYSTICK_Degrees_PER_MS_LO; + LimitSwitchSource limitSwitchSource; + public Wrist() { try { //PID wrist encoder and talon - wristRight = new CANTalonEncoder(RobotMap.WRIST_LEFT_MOTOR_CAN_ID, WRIST_ENCODER_TICKS_TO_DEGREES, FeedbackDevice.QuadEncoder); + wristRight = new CANTalonEncoder(RobotMap.WRIST_RIGHT_MOTOR_CAN_ID, WRIST_ENCODER_TICKS_TO_DEGREES, FeedbackDevice.QuadEncoder); + + //Limit Switch + wristRight.configForwardLimitSwitchSource(limitSwitchSource, LimitSwitchNormal.NormallyOpen, 0); + wristRight.configReverseLimitSwitchSource(limitSwitchSource, LimitSwitchNormal.NormallyOpen, 0); } catch(Exception e) { @@ -102,12 +119,17 @@ public class Wrist extends Subsystem } } - //Flipping the intake to the other side - public void flipIntake() + //Jump bar by putting power to the motors for a specific amount of time + //Jump bar output + public void jumpBar() { - double currentWristAngle = wristRight.getPositionWorld(); - + wristRight.set(0.8); + } + //Stop wrist motor + public void stopMotor() + { + wristRight.set(0); } //Method for setting the control mode for the wrist @@ -191,8 +213,29 @@ public class Wrist extends Subsystem } else if(armAngleDegrees <= armAngleForPIDSwitch) { + if(ballIntakeOut) + { + if(armAngleDegrees > targetAngleDegreesBallIn) + { + controlPIDBallIn(); + } + else + { + controlPIDBallOut(); + } + } + else + { + if(armAngleDegrees > targetAngleDegreesHatchIn) + { + controlPIDBallIn(); + } + else + { + controlPIDHatchOut(); + } + } } - controlPID(); break; case JOYSTICK_MANUAL: controlManualWithJoystick(); @@ -222,7 +265,7 @@ public class Wrist extends Subsystem { double joystickSpeed; - joystickSpeed = -Robot.oi.getOperatorController().getLeftYAxis(); + joystickSpeed = -Robot.oi.getOperatorController().getRightYAxis(); setSpeedJoystick(joystickSpeed); } @@ -240,6 +283,25 @@ public class Wrist extends Subsystem public void controlPIDBallIn() { updatePositionPID(targetAngleDegreesBallIn); + //Needs limit to lift up (so it can get over bar) in command group + } + + public void controlPIDBallOut() + { + updatePositionPID(targetAngleDegreesBallOut); + //Needs to be tuned a lot + } + + public void controlPIDHatchIn() + { + updatePositionPID(targetAngleDegreesHatchIn); + //Needs limit to lift up (so it can get over bar) in command group + } + + public void controlPIDHatchOut() + { + updatePositionPID(targetAngleDegreesHatchOut); + //Needs to be tuned a lot } public synchronized boolean isFinished() diff --git a/2019robot/src/main/java/org/usfirst/frc4388/utility/MMTalonPIDController.java b/2019robot/src/main/java/org/usfirst/frc4388/utility/MMTalonPIDController.java index c3be5a6..5f96efe 100644 --- a/2019robot/src/main/java/org/usfirst/frc4388/utility/MMTalonPIDController.java +++ b/2019robot/src/main/java/org/usfirst/frc4388/utility/MMTalonPIDController.java @@ -96,7 +96,7 @@ public class MMTalonPIDController // Update the set points if (controlMode == MMControlMode.STRAIGHT) { double gyroDelta = useGyroLock ? startGyroAngle - currentGyroAngle: 0; - double deltaDistance = calcTrackDistance(gyroDelta, MMTalonTurnType.TANK, Drive.TRACK_WIDTH_INCHES); + double deltaDistance = calcTrackDistance(gyroDelta, MMTalonTurnType.TANK, 1); rightTarget = targetValue + deltaDistance; leftTarget = targetValue - deltaDistance; diff --git a/2019robot/src/main/java/org/usfirst/frc4388/utility/PathGenerator.java b/2019robot/src/main/java/org/usfirst/frc4388/utility/PathGenerator.java index f99df73..e39719d 100644 --- a/2019robot/src/main/java/org/usfirst/frc4388/utility/PathGenerator.java +++ b/2019robot/src/main/java/org/usfirst/frc4388/utility/PathGenerator.java @@ -35,7 +35,7 @@ public class PathGenerator { Trajectory trajectory = Pathfinder.generate(points, config); centerPoints = trajectory.segments; - TankModifier modifier = new TankModifier(trajectory).modify(Drive.TRACK_WIDTH_INCHES); + TankModifier modifier = new TankModifier(trajectory).modify(1); leftPoints = modifier.getLeftTrajectory().segments; rightPoints = modifier.getRightTrajectory().segments;