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 c2c6a02..49bb45c 100644 --- a/2019robot/src/main/java/org/usfirst/frc4388/robot/Robot.java +++ b/2019robot/src/main/java/org/usfirst/frc4388/robot/Robot.java @@ -20,6 +20,7 @@ import org.usfirst.frc4388.robot.OI; import org.usfirst.frc4388.robot.subsystems.*; import org.usfirst.frc4388.utility.ControlLooper; import org.usfirst.frc4388.robot.subsystems.Drive; +import org.usfirst.frc4388.robot.subsystems.Wrist; import org.usfirst.frc4388.robot.subsystems.Drive.DriveControlMode;; @@ -30,6 +31,7 @@ public class Robot extends IterativeRobot public static final Drive drive = new Drive(); public static final Arm arm = new Arm(); + public static final Wrist wrist = new Wrist(); 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 e7a1a21..9fa1d12 100644 --- a/2019robot/src/main/java/org/usfirst/frc4388/robot/RobotMap.java +++ b/2019robot/src/main/java/org/usfirst/frc4388/robot/RobotMap.java @@ -17,8 +17,9 @@ public class RobotMap { public static final int DRIVETRAIN_RIGHT_MOTOR1_CAN_ID = 4; public static final int DRIVETRAIN_RIGHT_MOTOR2_CAN_ID = 5; - //Intake motors - public static final int INTAKE_BELT_DRIVE_CAN_ID = 8; + //carriage motors + public static final int WRIST_RIGHT_MOTOR_CAN_ID = 6; + public static final int INTAKE_BELT_DRIVE_CAN_ID = 7; //Elevator Motors public static final int ARM_MOTOR1_ID= 6; 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 new file mode 100644 index 0000000..6aa12c0 --- /dev/null +++ b/2019robot/src/main/java/org/usfirst/frc4388/robot/commands/GrabBallOutOfRobot.java @@ -0,0 +1,41 @@ +/*----------------------------------------------------------------------------*/ +/* 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.Robot; + +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 rest of sequentials for this command group + + addSequential(new FlipIntake()); + //Move Arm until bar jump angle + + //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/commands/WristSetMode.java b/2019robot/src/main/java/org/usfirst/frc4388/robot/commands/WristSetMode.java new file mode 100644 index 0000000..b86576a --- /dev/null +++ b/2019robot/src/main/java/org/usfirst/frc4388/robot/commands/WristSetMode.java @@ -0,0 +1,47 @@ +package org.usfirst.frc4388.robot.commands; + +import org.usfirst.frc4388.robot.Robot; +import org.usfirst.frc4388.robot.subsystems.Wrist.WristControlMode; + +import edu.wpi.first.wpilibj.command.Command; + +/** + * Description + */ +public class WristSetMode extends Command { + + private WristControlMode controlMode; + + public WristSetMode(WristControlMode controlMode) { + this.controlMode = controlMode; + requires(Robot.wrist); + } + + // Called just before this Command runs the first time + protected void initialize() { + if (controlMode == WristControlMode.PID) { + Robot.wrist.setPositionPID(Robot.wrist.getPositionDegrees()); + } + else if (controlMode == WristControlMode.JOYSTICK_MANUAL) { + Robot.wrist.setSpeedJoystick(0); + } + } + + // 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 true; + } + + // Called once after isFinished returns true + protected void end() { + } + + // Called when another command which requires one or more of the same + // subsystems is scheduled to run + protected void interrupted() { + } +} 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 bfb7817..50930b8 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 @@ -1,15 +1,23 @@ package org.usfirst.frc4388.robot.subsystems; + +import edu.wpi.first.wpilibj.command.Subsystem; + 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; -import org.usfirst.frc4388.utility.ControlLoopable; -import org.usfirst.frc4388.utility.Loop; import org.usfirst.frc4388.utility.MPTalonPIDController; +import org.usfirst.frc4388.robot.commands.*; +import org.usfirst.frc4388.robot.subsystems.Drive.DriveControlMode; +import org.usfirst.frc4388.utility.CANTalonEncoder; +import org.usfirst.frc4388.utility.ControlLoopable; import org.usfirst.frc4388.utility.PIDParams; -import org.usfirst.frc4388.utility.TalonSRXEncoder; -import org.usfirst.frc4388.utility.TalonSRXFactory; +import org.usfirst.frc4388.utility.SoftwarePIDPositionController; +import org.usfirst.frc4388.utility.MPTalonPIDPathController; +import org.usfirst.frc4388.utility.MPTalonPIDController; +import org.usfirst.frc4388.robot.subsystems.Arm; import com.ctre.phoenix.motorcontrol.ControlMode; import com.ctre.phoenix.motorcontrol.FeedbackDevice; @@ -20,329 +28,312 @@ 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 edu.wpi.first.wpilibj.command.WaitCommand; -import edu.wpi.first.wpilibj.Solenoid; -import edu.wpi.first.wpilibj.command.Subsystem; -import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; -import edu.wpi.first.wpilibj.Timer; +import com.ctre.phoenix.motorcontrol.LimitSwitchNormal; +import com.ctre.phoenix.motorcontrol.RemoteLimitSwitchSource; +import com.ctre.phoenix.motorcontrol.LimitSwitchSource; -public class Wrist extends Subsystem implements ControlLoopable +import com.ctre.phoenix.motorcontrol.ControlMode; + +public class Wrist extends Subsystem { private static Wrist instance; - public static enum WristControlMode { MOTION_PROFILE, JOYSTICK_PID, JOYSTICK_MANUAL, MANUAL }; + //Control Mode Array + public static enum WristControlMode {PID, JOYSTICK_MANUAL, GRAB_BALL}; - // 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 = ((360/4096)/(3))-60; - + //Motor Controllers + private ArrayList motorControllers = new ArrayList(); - private double periodMs; - private double lastControlLoopUpdatePeriod = 0.0; - private double lastControlLoopUpdateTimestamp = 0.0; - // Defined speeds - public static final double CLIMB_SPEED = -1.0; - public static final double TEST_SPEED_UP = 0.3; - public static final double TEST_SPEED_DOWN = -0.3; - public static final double AUTO_ZERO_SPEED = -0.3; - public static final double JOYSTICK_INCHES_PER_MS_HI = 0.75; - public static final double JOYSTICK_INCHES_PER_MS_LO = JOYSTICK_INCHES_PER_MS_HI/3.68 * 0.8; - - // Defined positions - public static final double ZERO_POSITION_AUTON_FORWARD_INCHES = 8.0; - 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 = 0.0; - public static final double MAX_POSITION_INCHES = 83.4; - public static final double AFTER_INTAKE_POSITION_INCHES = 4.0; + private CANTalonEncoder wristRight; - 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; - 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(); - - private TalonSRXEncoder wristmotor1; - - // PID controller and params + //Encoder ticks to inches for encoders + public static final double WRIST_ENCODER_TICKS_TO_DEGREES = ((4096/360)*(1/3)); + + // PID controller and params private MPTalonPIDController mpController; public static int PID_SLOT = 0; public static int MP_SLOT = 1; - 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); + ///private PIDParams mpPIDParams = new PIDParams(0.2, 0.0, 0.0, 0.0, 0.005, 0.0); + private PIDParams pidPIDParamsLevel = new PIDParams(0.075, 0.0, 0.0, 0.0, 0.0, 0.0); public static final double KF_UP = 0.005; public static final double KF_DOWN = 0.0; - public static final double PID_ERROR_INCHES = 1.0; - LimitSwitchSource limitSwitchSource; - // Pneumatics - private Solenoid speedShift; + public static final double PID_ERROR_INCHES = 1.0; + private long periodMs = (long)(Constants.kLooperDt * 1000.0); - private WristControlMode controlMode = WristControlMode.JOYSTICK_MANUAL; - // Misc - public static final double AUTO_ZERO_MOTOR_CURRENT = 4.0; - private boolean isFinished; - private WristControlMode wristControlMode = WristControlMode.JOYSTICK_MANUAL; - private double targetPositionInchesPID = 0; - private boolean firstMpPoint; - private double joystickInchesPerMs = JOYSTICK_INCHES_PER_MS_LO; - - public Wrist() { - try { - wristmotor1 = TalonSRXFactory.createTalonEncoder(RobotMap.WRIST_MOTOR_ID, (ENCODER_TICKS_TO_INCHES), false, FeedbackDevice.QuadEncoder); - - - wristmotor1.setInverted(true); - -// if (motor1.isSensorPresent(CANTalon.FeedbackDevice.QuadEncoder) != CANTalon.FeedbackDeviceStatus.FeedbackStatusPresent) { -// Driver.reportError("Could not detect elevator motor 1 encoder encoder!", false); -// } - wristmotor1.configForwardLimitSwitchSource(limitSwitchSource, LimitSwitchNormal.NormallyOpen, 0); - wristmotor1.configReverseLimitSwitchSource(limitSwitchSource, LimitSwitchNormal.NormallyOpen, 0); - wristmotor1.setNeutralMode(NeutralMode.Brake); - - motorControllers.add(wristmotor1); - - - } - catch (Exception e) { - System.err.println("An error occurred in the Wrist constructor"); - } - } + // Defined positions + public static final double MIN_POSITION_INCHES = 0.0; + public static final double MAX_POSITION_INCHES = 83.4; - @Override - public void initDefaultCommand() { - } - - public void resetZeroPosition(double position) { - mpController.resetZeroPosition(position); - } - - private synchronized void setWristControlMode(WristControlMode controlMode) { + public static final double MIN_ANGLE_DEGREES = -3; ////FIND ANGLE VALUES + public static final double MAX_ANGLE_DEGREES = 3; + + public static final double JOYSTICK_INCHES_PER_MS_HI = 0.75; + public static final double JOYSTICK_INCHES_PER_MS_LO = JOYSTICK_INCHES_PER_MS_HI/3.68 * 0.8; + public static final double JOYSTICK_Degrees_PER_MS_LO = JOYSTICK_INCHES_PER_MS_HI/3.68 * 0.8; + + public double armAngleDegrees = 90; + + 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 = 130; + public static final double targetAngleDegreesHatchOut = 0; + + 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; + private boolean isFinished; + private double targetPositionInchesPID = 0; + private double targetAngleDegreesPID = -(180 - armAngleDegrees); + + 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_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) + { + System.err.println("You thought the code would work, but it was me, error. An error occurred in the Wrist Construtor"); + } + } + + //Jump bar by putting power to the motors for a specific amount of time + //Jump bar output + public void jumpBar() + { + wristRight.set(0.8); + } + + //Stop wrist motor + public void stopMotor() + { + wristRight.set(0); + } + + //Method for setting the control mode for the wrist + private synchronized void setWristControlMode(WristControlMode controlMode) + { this.wristControlMode = controlMode; - } - - private synchronized WristControlMode getWristControlMode() { + } + + //Getting the control mode for the wrist + private synchronized WristControlMode getWristControlMode() + { return this.wristControlMode; - } + } - public void setSpeed(double speed) { - wristmotor1.set(ControlMode.PercentOutput, speed*0.3); - setWristControlMode(WristControlMode.MANUAL); - } - - public void setSpeedJoystick(double speed) { - wristmotor1.set(ControlMode.PercentOutput, speed); + //Setting the speed for the motor on the wrist along with setting the control mode to manual + public void setSpeed(double speed) + { + wristRight.set(ControlMode.PercentOutput, speed); setWristControlMode(WristControlMode.JOYSTICK_MANUAL); } - - public void setPositionPID(double targetPositionInches) { - mpController.setPIDSlot(PID_SLOT); - updatePositionPID(targetPositionInches); - setWristControlMode(WristControlMode.JOYSTICK_PID); + + //Setting the target position for the PID loop and set the control mode to PID + public void setPositionPID(double targetAngleInches) + { + mpController.setPIDSlot(PID_SLOT); + updatePositionPID(targetAngleInches); + setWristControlMode(WristControlMode.PID); setFinished(false); - } - - public void updatePositionPID(double targetPositionInches) { - targetPositionInchesPID = limitPosition(targetPositionInches); - double startPositionInches = wristmotor1.getPositionWorld(); - mpController.setTarget(targetPositionInchesPID, targetPositionInchesPID > startPositionInches ? KF_UP : KF_DOWN); - } - - public void setPositionMP(double targetPositionInches) { - double startPositionInches = wristmotor1.getPositionWorld(); - mpController.setMPTarget(startPositionInches, limitPosition(targetPositionInches), MP_MAX_VELOCITY_INCHES_PER_SEC, MP_T1, MP_T2); - setFinished(false); - firstMpPoint = true; - setWristControlMode(WristControlMode.MOTION_PROFILE); - } - - private double limitPosition(double targetPosition) { - if (targetPosition < MIN_POSITION_INCHES) { - return MIN_POSITION_INCHES; + } + + //Setting range for target position + private double limitPosition(double targetAngle) + { + if (targetAngle < MIN_ANGLE_DEGREES) { + return MIN_ANGLE_DEGREES; } - else if (targetPosition > MAX_POSITION_INCHES) { - return MAX_POSITION_INCHES; + else if (targetAngle > MAX_ANGLE_DEGREES) { + return MAX_ANGLE_DEGREES; } - return targetPosition; + return targetAngle; } - @Override - public void setPeriodMs(long periodMs) { - mpController = new MPTalonPIDController(periodMs, motorControllers); - mpController.setPID(mpPIDParams, MP_SLOT); - mpController.setPID(pidPIDParamsHiGear, PID_SLOT); - mpController.setPIDSlot(PID_SLOT); - mpController.setPID(pidPIDParamsHiGear, PID_SLOT); - mpController.setPIDSlot(PID_SLOT); - this.periodMs = periodMs; - } - /*@Override - public void onStart(double timestamp) { - mpController = new MPTalonPIDController(periodMs, motorControllers); - mpController.setPID(mpPIDParams, MP_SLOT); - mpController.setPID(pidPIDParamsHiGear, PID_SLOT); - mpController.setPIDSlot(PID_SLOT); - mpController.setPID(pidPIDParamsHiGear, PID_SLOT); + + //Method for updating the PID target position + public void updatePositionPID(double targetAngleDegrees) + { + targetAngleDegreesPID = limitPosition(targetAngleDegrees); + double startAngleDegrees = wristRight.getPositionWorld(); + mpController.setTarget(targetPositionInchesPID, targetAngleDegreesPID > startAngleDegrees ? KF_UP : KF_DOWN); + } + + //Getting the current encoder position + public double getPositionDegrees() + { + return wristRight.getPositionWorld(); + } + + //Setting the speed for the motors in manual mode + public void setSpeedJoystick(double speed) + { + wristRight.set(ControlMode.PercentOutput, speed); + setWristControlMode(wristControlMode.JOYSTICK_MANUAL); + } + + public void onStart(double timestamp) + { + //mpController.setPID(mpPIDParams); + // mpController.setPID(pidPIDParamsLevel); mpController.setPIDSlot(PID_SLOT); } - @Override - public void onStop(double timestamp) { - // TODO Auto-generated method stub - - } - - @Override - public void onLoop(double timestamp) { + //@Override + public void onLoop(double timestamp) + { synchronized (Wrist.this) { - switch( getElevatorControlMode() ) { - case JOYSTICK_PID: - controlPidWithJoystick(); + switch(getWristControlMode() ) { + case PID: + if(armAngleDegrees > armAngleForPIDSwitch) + { + controlPID(); + } + else if(armAngleDegrees <= armAngleForPIDSwitch) + { + if(ballIntakeOut) + { + if(armAngleDegrees > targetAngleDegreesBallIn) + { + controlPIDBallIn(); + } + else + { + controlPIDBallOut(); + } + } + else + { + if(armAngleDegrees > targetAngleDegreesHatchIn) + { + controlPIDBallIn(); + } + else + { + controlPIDHatchOut(); + } + } + } break; case JOYSTICK_MANUAL: controlManualWithJoystick(); - break; - case MOTION_PROFILE: - if (!isFinished()) { - if (firstMpPoint) { - mpController.setPIDSlot(MP_SLOT); - firstMpPoint = false; - } - setFinished(mpController.controlLoopUpdate()); - if (isFinished()) { - mpController.setPIDSlot(PID_SLOT); - } - } - break; + break; + /* + case FLIP_INTAKE: + controlPIDFlipIntake(); + break; + */ default: break; } } + } + + private void controlPID() + { + double joystickAngle = -Robot.oi.getOperatorController().getLeftYAxis(); + double deltaAngle = joystickAngle * joystickDegreesPerMs; + + targetAngleDegreesPID = targetAngleDegreesPID + deltaAngle; + updatePositionPID(targetAngleDegreesPID); } + + //Controlling the motor with the joystick + private void controlManualWithJoystick() + { + double joystickSpeed; + + joystickSpeed = -Robot.oi.getOperatorController().getRightYAxis(); + setSpeedJoystick(joystickSpeed); + } + + //Flip the intake from hatch to ball and visa versa + public void controlPIDFlipIntake() + { + double currentWristAngle = wristRight.getPositionWorld(); + double targetFlipAngle = currentWristAngle - 180; + //Flip angle may need to be adjusted if angle shouldn't be 180 -*/ + updatePositionPID(targetFlipAngle); + } + //Controlling the PID for the intake going into the robot with ball side facing in + 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 void controlLoopUpdate() { - // Measure *actual* update period - double currentTimestamp = Timer.getFPGATimestamp(); - if (lastControlLoopUpdateTimestamp > 0.001) { // ie, if this is NOT the first time - lastControlLoopUpdatePeriod = currentTimestamp - lastControlLoopUpdateTimestamp; - } - lastControlLoopUpdateTimestamp = currentTimestamp; - - // Do the update - if (controlMode == WristControlMode.JOYSTICK_MANUAL) { - controlManualWithJoystick(); - } - else if (!isFinished) { - if (controlMode == WristControlMode.MOTION_PROFILE) { - isFinished = mpController.controlLoopUpdate(getPositionInches()); - } - - /*else if (controlMode == WristControlMode.MP_PATH_VELOCITY) { - isFinished = mpPathVelocityController.controlLoopUpdate(getGyroAngleDeg()); - } - else if (controlMode == WristControlMode.ADAPTIVE_PURSUIT) { - updatePose(); - isFinished = adaptivePursuitController.controlLoopUpdate(currentPose); - }*/ - } - } - - - - - - - - private void controlPidWithJoystick() { - double joystickPosition = -Robot.oi.getOperatorController().getRightYAxis(); - double deltaPosition = joystickPosition * joystickInchesPerMs; - targetPositionInchesPID = targetPositionInchesPID + deltaPosition; - updatePositionPID(targetPositionInchesPID); - } - - private void ControlWithJoystickhold(){ - double holdPosition = wristmotor1.getPositionWorld(); - updatePositionPID(holdPosition); - - } - - private void controlManualWithJoystick() { - double joyStickSpeed = -Robot.oi.getOperatorController().getRightYAxis(); - setSpeedJoystick(joyStickSpeed*.10); - } - /* - public void setShiftState(ElevatorSpeedShiftState state) { - - joystickInchesPerMs = JOYSTICK_INCHES_PER_MS_HI; - speedShift.set(true); - mpController.setPID(pidPIDParamsHiGear, PID_SLOT); - } - else if(state == ElevatorSpeedShiftState.LO) { - joystickInchesPerMs = JOYSTICK_INCHES_PER_MS_LO; - speedShift.set(false); - mpController.setPID(pidPIDParamsLoGear, PID_SLOT); - } - } - - public ElevatorSpeedShiftState getShiftState() { - return shiftState; - } -*/ - public double getPositionInches() { - return wristmotor1.getPositionWorld(); - } - -// public double getAverageMotorCurrent() { -// return (Robot.pdp.getCurrent(RobotMap.ELEVATOR_MOTOR_1_CAN_ID) + Robot.pdp.getCurrent(RobotMap.ELEVATOR_MOTOR_2_CAN_ID) + Robot.pdp.getCurrent(RobotMap.ELEVATOR_MOTOR_3_CAN_ID)) / 3; -// } - - public double getAverageMotorCurrent() { - return (wristmotor1.getOutputCurrent()); - } - - public synchronized boolean isFinished() { + public synchronized boolean isFinished() + { return isFinished; } - - public synchronized void setFinished(boolean isFinished) { + + public synchronized void setFinished(boolean isFinished) + { this.isFinished = isFinished; } - - public double getPeriodMs() { + + public double getPeriodMs() + { return periodMs; } - - public void updateStatus(Robot.OperationMode operationMode) { - if (operationMode == Robot.OperationMode.TEST) { - try { - SmartDashboard.putNumber("Elevator Position Inches", wristmotor1.getPositionWorld()); - SmartDashboard.putNumber("Elevator Motor 1 Amps", wristmotor1.getOutputCurrent()); - SmartDashboard.putNumber("Elevator Average Amps", getAverageMotorCurrent()); -// 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("Elevator Target PID Position", targetPositionInchesPID); + + @Override + public void initDefaultCommand() + { + } + + public void updateStatus(Robot.OperationMode operationMode) + { + if (operationMode == Robot.OperationMode.TEST) + { + try + { } - catch (Exception e) { + catch (Exception e) + { + System.err.println("Wrist update status error"); } } } diff --git a/2019robot/src/main/java/org/usfirst/frc4388/utility/CANTalonEncoder.java b/2019robot/src/main/java/org/usfirst/frc4388/utility/CANTalonEncoder.java new file mode 100644 index 0000000..73373e8 --- /dev/null +++ b/2019robot/src/main/java/org/usfirst/frc4388/utility/CANTalonEncoder.java @@ -0,0 +1,80 @@ +package org.usfirst.frc4388.utility; + +import com.ctre.phoenix.motorcontrol.ControlMode; +import com.ctre.phoenix.motorcontrol.FeedbackDevice; +import com.ctre.phoenix.motorcontrol.can.WPI_TalonSRX; + +public class CANTalonEncoder extends WPI_TalonSRX +{ + public static int TIMEOUT_MS = 0; + + private double encoderTicksToWorld; + private boolean isRight = true; + + + + public CANTalonEncoder(int deviceNumber, double encoderTicksToWorld, FeedbackDevice feedbackDevice) { + this(deviceNumber, encoderTicksToWorld, false, feedbackDevice); + } + + public CANTalonEncoder(int deviceNumber, double encoderTicksToWorld, boolean isRight, FeedbackDevice feedbackDevice) { + super(deviceNumber); + //this.setFeedbackDevice(feedbackDevice); + this.configSelectedFeedbackSensor(feedbackDevice, 0, 0); + this.encoderTicksToWorld = encoderTicksToWorld; + this.isRight = isRight; + } + + public boolean isRight() { + return isRight; + } + + public void setRight(boolean isRight) { + this.isRight = isRight; + } + + public double convertEncoderTicksToWorld(double encoderTicks) + { + return encoderTicks / encoderTicksToWorld; + } + + public void setWorld(ControlMode mode, double worldValue) + { + this.set(mode, convertEncoderWorldToTicks(worldValue)); + } + + public double convertEncoderWorldToTicks(double worldValue) + { + return worldValue * encoderTicksToWorld; + } + + public void setWorld(double worldValue) + { + //this.set(convertEncoderWorldToTicks(worldValue)); + this.set(getControlMode(), convertEncoderWorldToTicks(worldValue)); //TODO: change to explicit mode set? + } + + public void setPositionWorld(double worldValue) + { + //this.setPosition(convertEncoderWorldToTicks(worldValue)); + this.setSelectedSensorPosition((int)convertEncoderWorldToTicks(worldValue), 0, 0); //TODO: verify + } + + public double getPositionWorld() + { + //return convertEncoderTicksToWorld(this.getPosition()); + return convertEncoderTicksToWorld(this.getSelectedSensorPosition(0)); //TODO: verify want 0="Primary closed-loop" + } + + public void setVelocityWorld(double worldValue) + { + //this.set(convertEncoderWorldToTicks(worldValue) * 0.1); + this.set(getControlMode(), convertEncoderWorldToTicks(worldValue) * 0.1); //TODO: change to explicit mode set? + } + + public double getVelocityWorld() + { + //return convertEncoderTicksToWorld(this.getSpeed() / 0.1); + return convertEncoderTicksToWorld(this.getSelectedSensorVelocity(0) / 0.1); //TODO: verify want 0="Primary closed-loop" + } +} diff --git a/2019robot/src/main/java/org/usfirst/frc4388/utility/SoftwarePIDPositionController.java b/2019robot/src/main/java/org/usfirst/frc4388/utility/SoftwarePIDPositionController.java new file mode 100644 index 0000000..383acd7 --- /dev/null +++ b/2019robot/src/main/java/org/usfirst/frc4388/utility/SoftwarePIDPositionController.java @@ -0,0 +1,85 @@ + +package org.usfirst.frc4388.utility; + +import java.util.ArrayList; + +import org.usfirst.frc4388.robot.Robot; +import org.usfirst.frc4388.utility.MPSoftwarePIDController.MPSoftwareTurnType; + +import com.ctre.phoenix.motorcontrol.ControlMode; +import com.ctre.phoenix.motorcontrol.can.WPI_TalonSRX; + +import edu.wpi.first.wpilibj.Timer; + +public class SoftwarePIDPositionController +{ + //protected ArrayList motorControllers; + protected WPI_TalonSRX motorController; + protected PIDParams pidParams; + + protected PIDParams PValue; + + protected double targetEncoderPosition; + + protected double minTurnOutput = 0.002; + protected double maxError; + protected double minError; + protected double maxPrevError; ///?? + protected double prevError = 0.0; // the prior error (used to compute velocity) + protected double totalError = 0.0; // the sum of the errors for use in the integral calc + + public SoftwarePIDPositionController(PIDParams PValue, WPI_TalonSRX elevatorLeft) + { + this.motorController = elevatorLeft; + setP(PValue); + } + + public void setP(PIDParams PValue) + { + this.PValue = PValue; + } + + public void setPIDPositionTarget(double targetPosition, double maxError, double minError) + { + this.targetEncoderPosition = targetPosition; + + this.maxError = maxError; + this.minError = minError; + //this.maxPrevError = maxPrevError; + + prevError = 0.0; + totalError = 0.0; + } + + public boolean controlLoopUpdate() + { + return controlLoopUpdate(0); + } + + public boolean controlLoopUpdate(double currentEncoderPosition) + { + // Calculate the motion profile feed forward and error feedback terms + double error = targetEncoderPosition - currentEncoderPosition; + //double deltaLastError = (error - prevError); + + //Updating the error + //totalError += error; + + // Check if we are finished + if (Math.abs(error) < maxError && Math.abs(error) > minError) + { + //Robot.elevator.holdInPos(); + + return true; + } + + double output = pidParams.kP * error; // + pidParams.kI * totalError + pidParams.kD * deltaLastError; + + prevError = error; + + // Update the controllers set point. + motorController.set(ControlMode.PercentOutput, output); + + return false; + } +} \ No newline at end of file