From 260699ef4ac56d98eeda11621019ee3aef916017 Mon Sep 17 00:00:00 2001 From: mayabartels Date: Mon, 28 Jan 2019 16:03:16 -0800 Subject: [PATCH 1/5] Added method for setting initial Angle below 0 deg Added to robot.java in robot initial to make initial angle correct --- .../java/org/usfirst/frc4388/robot/Robot.java | 19 +++++++++---------- .../frc4388/robot/subsystems/Wrist.java | 11 +++++++++-- 2 files changed, 18 insertions(+), 12 deletions(-) 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 65c24e8..e75f4d9 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,10 @@ public class Robot extends IterativeRobot private Command RRautonomousCommand; private Command RLautonomousCommand; private Command LRautonomousCommand; - private Command LLautonomousCommand; - + private Command LLautonomousCommand; + + public static final double OriginalDegreesWrist = arm.ENCODER_TICKS_TO_DEGREES; + public void robotInit() { //Printing game data to riolog @@ -70,16 +72,13 @@ public class Robot extends IterativeRobot operationModeChooser.addDefault("Test", OperationMode.TEST); operationModeChooser.addObject("Competition", OperationMode.COMPETITION); SmartDashboard.putData("Operation Mode", operationModeChooser); - - - - - - - + wrist.setInitAngle(); + //ledLights.setAllLightsOn(false); - } catch (Exception e) { + } + catch (Exception e) + { System.err.println("An error occurred in robotInit()"); } } 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 5edbeb9..e54cb2f 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 @@ -60,8 +60,8 @@ public class Wrist extends Subsystem public static final double MIN_POSITION_INCHES = 0.0; public static final double MAX_POSITION_INCHES = 83.4; - public static final double MIN_ANGLE_DEGREES = 0.0; ////FIND ANGLE VALUES - public static final double MAX_ANGLE_DEGREES = 0.0; + 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; @@ -89,6 +89,13 @@ public class Wrist extends Subsystem } } + //Set the degree to negative angle after initializing + public void setInitAngle() + { + double armAngleToHoriz = 70; + double initAngle = ENCODER_TICKS_TO_DEGREES - armAngleToHoriz; + } + //Method for setting the control mode for the wrist private synchronized void setWristControlMode(WristControlMode controlMode) { From 0fc322ec561159523f166648acc76f87fdcc4d4e Mon Sep 17 00:00:00 2001 From: mayabartels Date: Mon, 28 Jan 2019 16:25:05 -0800 Subject: [PATCH 2/5] Added to initial angle change --- 2019robot/src/main/java/org/usfirst/frc4388/robot/Robot.java | 2 -- .../main/java/org/usfirst/frc4388/robot/subsystems/Wrist.java | 4 ++++ 2 files changed, 4 insertions(+), 2 deletions(-) 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 e75f4d9..b4543fa 100644 --- a/2019robot/src/main/java/org/usfirst/frc4388/robot/Robot.java +++ b/2019robot/src/main/java/org/usfirst/frc4388/robot/Robot.java @@ -50,8 +50,6 @@ public class Robot extends IterativeRobot private Command RLautonomousCommand; private Command LRautonomousCommand; private Command LLautonomousCommand; - - public static final double OriginalDegreesWrist = arm.ENCODER_TICKS_TO_DEGREES; public void robotInit() { 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 e54cb2f..33e2628 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 @@ -42,6 +42,8 @@ public class Wrist extends Subsystem //Encoder ticks to inches for encoders public static final double ENCODER_TICKS_TO_INCHES = Constants.kArmEncoderTicksPerDegree; public static final double ENCODER_TICKS_TO_DEGREES = Constants.kWristEncoderTicksPerDegree * Math.PI; + + public double WRIST_ANGLE_DEGREES = 0; // PID controller and params private MPTalonPIDController mpController; @@ -94,6 +96,8 @@ public class Wrist extends Subsystem { double armAngleToHoriz = 70; double initAngle = ENCODER_TICKS_TO_DEGREES - armAngleToHoriz; + + WRIST_ANGLE_DEGREES = initAngle; } //Method for setting the control mode for the wrist From f6cd6accecd696d0b7b4ceb34d7ffeca82ac4b2e Mon Sep 17 00:00:00 2001 From: mayabartels Date: Mon, 28 Jan 2019 16:28:12 -0800 Subject: [PATCH 3/5] Changed angle stuff from wrist to arm --- .../main/java/org/usfirst/frc4388/robot/Robot.java | 3 ++- .../org/usfirst/frc4388/robot/subsystems/Arm.java | 11 +++++++++++ .../org/usfirst/frc4388/robot/subsystems/Wrist.java | 11 ----------- 3 files changed, 13 insertions(+), 12 deletions(-) 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 b4543fa..de0d898 100644 --- a/2019robot/src/main/java/org/usfirst/frc4388/robot/Robot.java +++ b/2019robot/src/main/java/org/usfirst/frc4388/robot/Robot.java @@ -71,7 +71,8 @@ public class Robot extends IterativeRobot operationModeChooser.addObject("Competition", OperationMode.COMPETITION); SmartDashboard.putData("Operation Mode", operationModeChooser); - wrist.setInitAngle(); + //Initializing the angle of the arm to be + arm.setInitAngle(); //ledLights.setAllLightsOn(false); } 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 7e8d8e7..29fc99c 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 @@ -28,6 +28,8 @@ public class Arm extends Subsystem implements Loop // 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 double ARM_ANGLE_DEGREES = 0; + // Defined speeds public static final double JOYSTICK_INCHES_PER_MS_HI = 0.75; @@ -85,6 +87,15 @@ public class Arm extends Subsystem implements Loop } } + //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() { } 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 33e2628..ffc431e 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 @@ -42,8 +42,6 @@ public class Wrist extends Subsystem //Encoder ticks to inches for encoders public static final double ENCODER_TICKS_TO_INCHES = Constants.kArmEncoderTicksPerDegree; public static final double ENCODER_TICKS_TO_DEGREES = Constants.kWristEncoderTicksPerDegree * Math.PI; - - public double WRIST_ANGLE_DEGREES = 0; // PID controller and params private MPTalonPIDController mpController; @@ -91,15 +89,6 @@ public class Wrist extends Subsystem } } - //Set the degree to negative angle after initializing - public void setInitAngle() - { - double armAngleToHoriz = 70; - double initAngle = ENCODER_TICKS_TO_DEGREES - armAngleToHoriz; - - WRIST_ANGLE_DEGREES = initAngle; - } - //Method for setting the control mode for the wrist private synchronized void setWristControlMode(WristControlMode controlMode) { From 4d9b8553422fe77035f0857f44f45e88d876c64a Mon Sep 17 00:00:00 2001 From: mayabartels Date: Mon, 28 Jan 2019 16:35:32 -0800 Subject: [PATCH 4/5] Added command group to grab ball Also added control mode in wrist to change to grab ball control mode --- .../robot/commands/GrabBallOutOfRobot.java | 35 +++++++++++++++++++ .../frc4388/robot/subsystems/Wrist.java | 2 +- 2 files changed, 36 insertions(+), 1 deletion(-) create mode 100644 2019robot/src/main/java/org/usfirst/frc4388/robot/commands/GrabBallOutOfRobot.java 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..2ab8874 --- /dev/null +++ b/2019robot/src/main/java/org/usfirst/frc4388/robot/commands/GrabBallOutOfRobot.java @@ -0,0 +1,35 @@ +/*----------------------------------------------------------------------------*/ +/* 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; + +public class GrabBallOutOfRobot extends CommandGroup { + /** + * Add your docs here. + */ + public GrabBallOutOfRobot() + { + // 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/subsystems/Wrist.java b/2019robot/src/main/java/org/usfirst/frc4388/robot/subsystems/Wrist.java index ffc431e..d168cad 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 @@ -32,7 +32,7 @@ import com.ctre.phoenix.motorcontrol.ControlMode; public class Wrist extends Subsystem { //Control Mode Array - public static enum WristControlMode {PID, JOYSTICK_MANUAL}; + public static enum WristControlMode {PID, JOYSTICK_MANUAL, GRAB_BALL}; //Motor Controllers private ArrayList motorControllers = new ArrayList(); From 2cd409e80797c8939ecb6f16bdbe349bd58aad67 Mon Sep 17 00:00:00 2001 From: mayabartels Date: Mon, 28 Jan 2019 17:58:02 -0800 Subject: [PATCH 5/5] Added arm angle to determine wrist angle --- .../java/org/usfirst/frc4388/robot/subsystems/Wrist.java | 5 ++++- 1 file changed, 4 insertions(+), 1 deletion(-) 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 d168cad..9d977c5 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 @@ -15,6 +15,7 @@ import org.usfirst.frc4388.utility.CANTalonEncoder; import org.usfirst.frc4388.utility.ControlLoopable; import org.usfirst.frc4388.utility.PIDParams; import org.usfirst.frc4388.utility.SoftwarePIDPositionController; +import org.usfirst.frc4388.robot.subsystems.Arm; import com.ctre.phoenix.motorcontrol.FeedbackDevice; import com.ctre.phoenix.motorcontrol.LimitSwitchNormal; @@ -66,12 +67,14 @@ public class Wrist extends Subsystem 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 = Robot.arm.ARM_ANGLE_DEGREES; //Misc private WristControlMode wristControlMode = WristControlMode.JOYSTICK_MANUAL; private boolean isFinished; private double targetPositionInchesPID = 0; - private double targetAngleDegreesPID = 0; + private double targetAngleDegreesPID = -(180 - armAngleDegrees); private double joystickInchesPerMs = JOYSTICK_INCHES_PER_MS_LO; private double joystickDegreesPerMs = JOYSTICK_Degrees_PER_MS_LO;