From f6426c93247eec84a346dcf02dae6c2c7e4a817c Mon Sep 17 00:00:00 2001 From: lukesta182 <33330639+lukesta182@users.noreply.github.com> Date: Thu, 31 Jan 2019 19:37:31 -0700 Subject: [PATCH] changes to arm, make it able to work and not time out times out becase of choosers in robot --- .../java/org/usfirst/frc4388/robot/Robot.java | 21 +------- .../frc4388/robot/commands/ArmSetMode.java | 3 +- .../robot/commands/ArmSetPositionPID.java | 42 ++++++++++++++++ .../usfirst/frc4388/robot/subsystems/Arm.java | 50 +++++++++++-------- 4 files changed, 74 insertions(+), 42 deletions(-) create mode 100644 2019robot/src/main/java/org/usfirst/frc4388/robot/commands/ArmSetPositionPID.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 de0d898..ac82e4a 100644 --- a/2019robot/src/main/java/org/usfirst/frc4388/robot/Robot.java +++ b/2019robot/src/main/java/org/usfirst/frc4388/robot/Robot.java @@ -41,15 +41,6 @@ public class Robot extends IterativeRobot public static OperationMode operationMode = OperationMode.TEST; private SendableChooser operationModeChooser; - private SendableChooser RRautonTaskChooser; - private SendableChooser RLautonTaskChooser; - private SendableChooser LRautonTaskChooser; - private SendableChooser LLautonTaskChooser; - - private Command RRautonomousCommand; - private Command RLautonomousCommand; - private Command LRautonomousCommand; - private Command LLautonomousCommand; public void robotInit() { @@ -71,8 +62,7 @@ public class Robot extends IterativeRobot operationModeChooser.addObject("Competition", OperationMode.COMPETITION); SmartDashboard.putData("Operation Mode", operationModeChooser); - //Initializing the angle of the arm to be - arm.setInitAngle(); + //ledLights.setAllLightsOn(false); } @@ -93,7 +83,6 @@ public class Robot extends IterativeRobot public void autonomousInit() { updateChoosers(); - controlLoop.start(); drive.endGyroCalibration(); drive.resetEncoders(); @@ -110,10 +99,6 @@ public class Robot extends IterativeRobot } public void teleopInit() { - if (RRautonomousCommand != null) RRautonomousCommand.cancel(); - if (RLautonomousCommand != null) RLautonomousCommand.cancel(); - if (LRautonomousCommand != null) LRautonomousCommand.cancel(); - if (LLautonomousCommand != null) LLautonomousCommand.cancel(); drive.setToBrakeOnNeutral(false); updateChoosers(); controlLoop.start(); @@ -137,10 +122,6 @@ public class Robot extends IterativeRobot private void updateChoosers() { operationMode = (OperationMode)operationModeChooser.getSelected(); - RRautonomousCommand = (Command)RRautonTaskChooser.getSelected(); - RLautonomousCommand = (Command)RLautonTaskChooser.getSelected(); - LRautonomousCommand = (Command)LRautonTaskChooser.getSelected(); - LLautonomousCommand = (Command)LLautonTaskChooser.getSelected(); } public Alliance getAlliance() diff --git a/2019robot/src/main/java/org/usfirst/frc4388/robot/commands/ArmSetMode.java b/2019robot/src/main/java/org/usfirst/frc4388/robot/commands/ArmSetMode.java index 63b1092..b8bf31a 100644 --- a/2019robot/src/main/java/org/usfirst/frc4388/robot/commands/ArmSetMode.java +++ b/2019robot/src/main/java/org/usfirst/frc4388/robot/commands/ArmSetMode.java @@ -2,6 +2,7 @@ package org.usfirst.frc4388.robot.commands; import org.usfirst.frc4388.robot.Robot; import org.usfirst.frc4388.robot.subsystems.Arm.ArmControlMode; +import java.lang.Math; import edu.wpi.first.wpilibj.command.Command; @@ -20,7 +21,7 @@ public class ArmSetMode extends Command { // Called just before this Command runs the first time protected void initialize() { if (controlMode == ArmControlMode.PID) { - Robot.wrist.setPositionPID(27); + Robot.wrist.setPositionPID(Robot.arm.getYPositionInches()); } else if (controlMode == ArmControlMode.JOYSTICK_MANUAL) { Robot.wrist.setSpeedJoystick(0); diff --git a/2019robot/src/main/java/org/usfirst/frc4388/robot/commands/ArmSetPositionPID.java b/2019robot/src/main/java/org/usfirst/frc4388/robot/commands/ArmSetPositionPID.java new file mode 100644 index 0000000..5c33de3 --- /dev/null +++ b/2019robot/src/main/java/org/usfirst/frc4388/robot/commands/ArmSetPositionPID.java @@ -0,0 +1,42 @@ +package org.usfirst.frc4388.robot.commands; + +import org.usfirst.frc4388.robot.Robot; +import org.usfirst.frc4388.robot.subsystems.Arm; + +import edu.wpi.first.wpilibj.command.Command; + +/** + * + */ +public class ArmSetPositionPID extends Command { + + private double targetPositionInches; + + public ArmSetPositionPID(double targetPositionInches) { + this.targetPositionInches = targetPositionInches; + requires(Robot.arm); + } + + // Called just before this Command runs the first time + protected void initialize() { + Robot.arm.setPositionPID(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 Math.abs(Robot.arm.getYPositionInches() - this.targetPositionInches) < Arm.PID_ERROR_INCHES; + } + + // 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() { + } +} \ No newline at end of file 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 962cfd1..63d219f 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 java.lang.Math; import org.usfirst.frc4388.robot.Constants; import org.usfirst.frc4388.robot.Robot; @@ -14,6 +15,12 @@ import org.usfirst.frc4388.utility.TalonSRXEncoder; import com.ctre.phoenix.motorcontrol.ControlMode; import com.ctre.phoenix.motorcontrol.FeedbackDevice; import com.ctre.phoenix.motorcontrol.can.TalonSRX; +import com.ctre.phoenix.motorcontrol.LimitSwitchSource; +import com.ctre.phoenix.motorcontrol.LimitSwitchNormal; +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.Subsystem; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; @@ -23,10 +30,15 @@ public class Arm extends Subsystem implements Loop private static Arm instance; public static enum ArmControlMode {PID, JOYSTICK_MANUAL }; - + public static final double DEGREE_START_OFFSET = 70; + + public static final double RADIUS_OF_ARM = 43.3; + public static final double OFF_SET_BELOW = 49.3; // One revolution of the 1-3 GEAR RATION ON THE ARM * 4096 ticks - public static final double ENCODER_TICKS_TO_DEGREES = (36.0 / 12.0) * (36.0 / 24.0) * (34.0 / 24.0) * 4096.0 / (1.88 * Math.PI); + public static final double ENCODER_TICKS_TO_DEGREES = ((4096/360)*(1/3))-(DEGREE_START_OFFSET); + public static final double X_POSITION_MATH = ((RADIUS_OF_ARM)*(Math.cos(ENCODER_TICKS_TO_DEGREES)))+(OFF_SET_BELOW); + public static final double Y_POSITION_MATH = (RADIUS_OF_ARM)*(Math.sin(ENCODER_TICKS_TO_DEGREES)); public double ARM_ANGLE_DEGREES = 0; @@ -34,17 +46,14 @@ public class Arm extends Subsystem implements Loop public static final double JOYSTICK_INCHES_PER_MS_HI = 0.75; // 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; + LimitSwitchSource limitSwitchSource; // 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 @@ -80,22 +89,13 @@ public class Arm extends Subsystem implements Loop //motor2 = CANTallon.createPermanentSlaveTalon(RobotMap.ARM_MOTOR_2_CAN_ID, RobotMap.ELEVATOR_MOTOR_1_CAN_ID); - + motor1.configForwardLimitSwitchSource(limitSwitchSource, LimitSwitchNormal.NormallyOpen, 0); + motor1.configReverseLimitSwitchSource(limitSwitchSource, LimitSwitchNormal.NormallyOpen, 0); } catch (Exception e) { System.err.println("An error occurred in the DriveTrain constructor"); } } - - //Set the degree to negative angle after initializing - public void setInitAngle() - { - double armAngleToHoriz = 70; - double initAngle = ENCODER_TICKS_TO_DEGREES - armAngleToHoriz; - - ARM_ANGLE_DEGREES = initAngle; - } - @Override public void initDefaultCommand() { } @@ -162,29 +162,37 @@ public class Arm extends Subsystem implements Loop case PID: controlPidWithJoystick(); break; - default: JOYSTICK_MANUAL: + case JOYSTICK_MANUAL: controlManualWithJoystick(); break; + } } } + + + private void controlPidWithJoystick() { //double joystickPosition = -Robot.oi.getOperatorController().getLeftYAxis(); //double deltaPosition = joystickPosition *.5; targetPositionInchesPID = targetPositionInchesPID;// + deltaPosition; updatePositionPID(targetPositionInchesPID); } - + private void controlManualWithJoystick() { double joyStickSpeed = -Robot.oi.getOperatorController().getLeftYAxis(); setSpeedJoystick(joyStickSpeed); } - public double getPositionInches() { - return motor1.getPositionWorld(); + public double getYPositionInches() { + return motor1.getPositionWorld()*Y_POSITION_MATH; + + } + public double getXPositionInches() { + return motor1.getPositionWorld()*X_POSITION_MATH; } // public double getAverageMotorCurrent() {