From a48a68400c20a94f478c3230962c05acf569fdfd Mon Sep 17 00:00:00 2001 From: TTVMixmix00 <68516760+TTVMixmix00@users.noreply.github.com> Date: Fri, 31 Mar 2023 15:52:10 +0000 Subject: [PATCH 01/10] Fixed it (hopefully) --- src/main/java/com/team766/robot/OI.java | 91 ++---------- .../com/team766/robot/mechanisms/Arms.java | 139 ++---------------- 2 files changed, 23 insertions(+), 207 deletions(-) diff --git a/src/main/java/com/team766/robot/OI.java b/src/main/java/com/team766/robot/OI.java index 5d354de7..068d26dc 100644 --- a/src/main/java/com/team766/robot/OI.java +++ b/src/main/java/com/team766/robot/OI.java @@ -27,35 +27,12 @@ public class OI extends Procedure { private JoystickReader joystick1; private JoystickReader joystick2; - - enum generalControl{ - CONE_HIGH_NODE, - CUBE_HIGH_NODE, - CONE_MID_NODE, - CUBE_MID_NODE, - OFF, - READY, - HUMANPLAYER_PICKUP, - MANUAL, - HYBRID_NODE - }; - - - - public generalControl generalState = generalControl.OFF; - - - - - public OI() { loggerCategory = Category.OPERATOR_INTERFACE; joystick0 = RobotProvider.instance.getJoystick(0); joystick1 = RobotProvider.instance.getJoystick(1); joystick2 = RobotProvider.instance.getJoystick(2); - - } public void run(Context context) { @@ -69,84 +46,42 @@ public void run(Context context) { if(joystick0.getButton(1)){ Robot.arms.resetEncoders(); } - if(joystick0.getButton(2)){ Robot.arms.antiGravBothJoints(); } - if(joystick0.getButton(3)){ //HIGH NODE SCORING - Robot.arms.pidForArmOne(-38.34); - Robot.arms.pidForArmTwo(-90.665); + Robot.arms.maxPidArm1(-38.34); + Robot.arms.maxPidArm2(-90.665); } if(joystick0.getButtonPressed(4)){ //MID NODE SCORING - Robot.arms.pidForArmOne(null); - Robot.arms.pidForArmTwo(null); + Robot.arms.maxPidArm1(null); + Robot.arms.maxPidArm2(null); } - if(joystick0.getButton(5)){ //TAKING FROM HUMAN PLAYER - Robot.arms.pidForArmOne(null); - Robot.arms.pidForArmTwo(null); + Robot.arms.maxPidArm1(null); + Robot.arms.maxPidArm2(null); } - if(joystick0.getButton(6)){ //STOWED - Robot.arms.pidForArmOne(null); - Robot.arms.pidForArmTwo(null); + Robot.arms.maxPidArm1(null); + Robot.arms.maxPidArm2(null); } if(joystick0.getButton(7)){ //READY - Robot.arms.pidForArmOne(null); - Robot.arms.pidForArmTwo(null); + Robot.arms.maxPidArm1(null); + Robot.arms.maxPidArm2(null); } if(joystick0.getButton(8)){ //PREP - Robot.arms.pidForArmOne(null); - Robot.arms.pidForArmTwo(null); + Robot.arms.maxPidArm1(null); + Robot.arms.maxPidArm2(null); } //TODO: can we stil use a switch? -/* - switch(generalState){ - case OFF: - log("generalControl is off"); - break; - case CONE_HIGH_NODE: - Robot.arms.pidForArmOne(0); - Robot.arms.pidForArmTwo(0); - break; - case CUBE_HIGH_NODE: - Robot.arms.pidForArmOne(0); - Robot.arms.pidForArmTwo(0); - break; - case CONE_MID_NODE: - Robot.arms.pidForArmOne(0); - Robot.arms.pidForArmTwo(0); - break; - case CUBE_MID_NODE: - Robot.arms.pidForArmOne(0); - Robot.arms.pidForArmTwo(0); - break; - case MANUAL: - Robot.arms.manuallySetArmOnePower(joystick0.getAxis(0)); - Robot.arms.manuallySetArmTwoPower(joystick0.getAxis(1)); - break; - case READY: - Robot.arms.pidForArmOne(0); - Robot.arms.pidForArmTwo(0); - break; - case HUMANPLAYER_PICKUP: - Robot.arms.pidForArmOne(0); - Robot.arms.pidForArmTwo(0); - break; - case HYBRID_NODE: - Robot.arms.pidForArmOne(0); - Robot.arms.pidForArmTwo(0); - break; - } - */ + } } } diff --git a/src/main/java/com/team766/robot/mechanisms/Arms.java b/src/main/java/com/team766/robot/mechanisms/Arms.java index 838a17fe..5733102a 100644 --- a/src/main/java/com/team766/robot/mechanisms/Arms.java +++ b/src/main/java/com/team766/robot/mechanisms/Arms.java @@ -36,7 +36,7 @@ public class Arms extends Mechanism { // Non-motor constants - private static double doubleDeadZone = 5; + private static double doubleDeadZone = 0.005; enum ArmState{ PID, // Moving @@ -54,16 +54,11 @@ enum ArmState{ // and counter-clockwise to be positive. // All the following variables are in degrees - private double firstJointPosition = 0; - private double secondJointPosition = 0; - private double firstJointCombo = 0; - private double secondJointCombo = 0; - // TODO: need to be set soon - private static final double FIRST_JOINT_MAX_LOCATION = 35; - private static final double FIRST_JOINT_MIN_LOCATION = -40; - private static final double SECOND_JOINT_MAX_LOCATION = 45; - private static final double SECOND_JOINT_MIN_LOCATION = -160; + private static final double FIRST_JOINT_MAX_LOCATION = 1; + private static final double FIRST_JOINT_MIN_LOCATION = 0; + private static final double SECOND_JOINT_MAX_LOCATION = 1; + private static final double SECOND_JOINT_MIN_LOCATION = 0; private RateLimiter runRateLimiter = new RateLimiter(0.05); @@ -158,6 +153,7 @@ public void maxPidArm1(double value){ }else{ firstJointPIDController.setFeedbackDevice(altEncoder1); //todo, is the antigrav here really needed? + //todo will the anti grav work with the absolute pids or does it just set power? firstJointPIDController.setReference(value, ControlType.kSmartMotion, 0, getAntiGravFirstJoint()); } } @@ -174,54 +170,12 @@ public void maxPidArm2(double value){ }else{ secondJointPIDController.setFeedbackDevice(altEncoder1); //todo, is the antigrav here really needed? + //todo will the anti grav work with the absolute pids or does it just set power? secondJointPIDController.setReference(value, ControlType.kSmartMotion, 0, getAntiGravSecondJoint()); } } - // PID for first arm - /** - * Set PID for the first joint. - * - * @param value desired position in degrees. - */ - /* public void pidForArmOne(double value){ // This will be run once - // log("First Joint Absolute Encoder: " + altEncoder1.getPosition()); - // log("" + firstJointCANSparkMax.getAbsoluteEncoder(Type.kDutyCycle).getPosition()); - - // If value is out of range, then adjust value. - value = clampValueToRange(value, FIRST_JOINT_MAX_LOCATION, FIRST_JOINT_MIN_LOCATION); - - firstJointPosition = value; - // if(Math.abs(EUTodegrees(firstJoint.getSensorPosition() ))) - firstJointPIDController.setReference(degreesToEU(firstJointPosition), - ControlType.kSmartMotion, - 0, - getAntiGravFirstJoint()); - firstJointState = ArmState.PID; - firstJointCombo = 0; - } -*/ - -/* - // PID for second arm - public void pidForArmTwo(double value){ - // log("Second Joint Absolute Encoder: " + altEncoder2.getPosition()); - // log("" + firstJointCANSparkMax.getAbsoluteEncoder(Type.kDutyCycle).getPosition()); - - // If value is out of range, then adjust value. - - value = clampValueToRange(value, SECOND_JOINT_MAX_LOCATION, SECOND_JOINT_MIN_LOCATION); - - secondJointPosition = value; - secondJointPIDController.setReference( - degreesToEU(secondJointPosition), - ControlType.kSmartMotion, - 0, - getAntiGravSecondJoint()); - secondJointState = ArmState.PID; - secondJointCombo = 0; - } - */ + // These next 3 antiGrav aren't used. public void antiGravBothJoints(){ @@ -265,70 +219,6 @@ public double getAntiGravSecondJoint(){ return -1*Math.signum(secondRelEncoderAngle) * (Math.cos((Math.PI / 180) * secondJointAngle) * ANTI_GRAV_SECOND_JOINT.valueOr(0.0)); } - - /*@Override - public void run() { - if(!runRateLimiter.next()) return; - - log("First Joint Absolute Encoder: " + altEncoder1.getPosition()); - log("Second Joint Absolute Encoder: " + altEncoder2.getPosition()); - // log("First Joint Relative Encoder: " + firstJoint.getSensorPosition()); - // log("Second Joint Relative Encoder: " + secondJoint.getSensorPosition()); - // log("First Joint Difference: " + (EUTodegrees(firstJoint.getSensorPosition())-firstJointPosition)); - // log("Second Joint Difference: " + (EUTodegrees(secondJoint.getSensorPosition())-secondJointPosition)); - log("Degrees Joint 1: "+EUTodegrees(firstJoint.getSensorPosition())); - log("Degrees Joint 2: "+EUTodegrees(secondJoint.getSensorPosition())); - log("First Joint State: "+firstJointState); - log("Second Joint State: "+secondJointState); - log("First Joint Combo: "+firstJointCombo); - log("Second Joint Combo: "+secondJointCombo); - - // log("First Joint AntiGrav: "+getAntiGravFirstJoint()); - // log("Second Joint AntiGrav: "+getAntiGravSecondJoint()); - if (firstJointState == ArmState.ANTIGRAV) { - antiGravFirstJoint(); - } else { - firstJointPIDController.setReference( - degreesToEU(firstJointPosition), - ControlType.kSmartMotion, - 0, - getAntiGravFirstJoint()); - - if (Math.abs(EUTodegrees(firstJoint.getSensorPosition())-firstJointPosition) <= doubleDeadZone){ - firstJointCombo ++; - } else { - firstJointCombo = 0; - } - - if (firstJointCombo >= 10){ - firstJointCombo = 0; - resetEncoders(); - firstJointState = ArmState.ANTIGRAV; - } - } - - if (secondJointState == ArmState.ANTIGRAV) { - antiGravSecondJoint(); - } else { - secondJointPIDController.setReference( - degreesToEU(secondJointPosition), - ControlType.kSmartMotion, - 0, - getAntiGravSecondJoint()); - - if (Math.abs(EUTodegrees(secondJoint.getSensorPosition())-secondJointPosition) <= doubleDeadZone){ - secondJointCombo ++; - } else { - secondJointCombo = 0; - } - - if (secondJointCombo >= 10){ - secondJointCombo = 0; - resetEncoders(); - secondJointState = ArmState.ANTIGRAV; - } - } - }*/ // Helper classes to convert from degrees to EU to absEU // TODO: update EU/degree constant for new gearbox 3:5:5 @@ -358,19 +248,10 @@ public double lawOfSines(double side1, double angle1, double side2){ return Math.asin(side2*Math.sin(angle1)/side1); } - private double clampValueToRange(double value, double max, double min) { - if(value > max){ - value = max; - } else if( value < min){ - value = min; - } - return value; - } - -} /* ~~ Code Review ~~ Use Voltage Control Mode when setting power (refer to CANSparkMaxMotorController.java) Maybe use Nicholas's formula for degrees to EU "Use break mode" - Ronald the not programmer - */ \ No newline at end of file + */ +} \ No newline at end of file From 0464d42f9b5833318aa4ac95f7505e69612e8ddd Mon Sep 17 00:00:00 2001 From: Max <68516760+TTVMixmix00@users.noreply.github.com> Date: Fri, 31 Mar 2023 19:08:23 -0700 Subject: [PATCH 02/10] w fixed it ez --- src/main/java/com/team766/robot/mechanisms/Arms.java | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/main/java/com/team766/robot/mechanisms/Arms.java b/src/main/java/com/team766/robot/mechanisms/Arms.java index 5733102a..7e1a7b1b 100644 --- a/src/main/java/com/team766/robot/mechanisms/Arms.java +++ b/src/main/java/com/team766/robot/mechanisms/Arms.java @@ -148,7 +148,7 @@ public void maxPidArm1(double value){ value = FIRST_JOINT_MIN_LOCATION; } - if(value + doubleDeadZone < altEncoder1.getSensorPosition() && value - doubleDeadZone > altEncoder1.getSensorPosition()){ + if(value + doubleDeadZone < altEncoder1.getPosition() && value - doubleDeadZone > altEncoder1.getPosition()){ getAntiGravFirstJoint(); }else{ firstJointPIDController.setFeedbackDevice(altEncoder1); @@ -165,7 +165,7 @@ public void maxPidArm2(double value){ value = SECOND_JOINT_MIN_LOCATION; } - if(value + doubleDeadZone < altEncoder2.getSensorPosition() && value - doubleDeadZone > altEncoder2.getSensorPosition()){ + if(value + doubleDeadZone < altEncoder2.getPosition() && value - doubleDeadZone > altEncoder2.getPosition()){ getAntiGravSecondJoint(); }else{ secondJointPIDController.setFeedbackDevice(altEncoder1); From 4e6ef621e01d527381074fa683ebb1338a187854 Mon Sep 17 00:00:00 2001 From: Max <68516760+TTVMixmix00@users.noreply.github.com> Date: Sat, 1 Apr 2023 00:30:43 -0700 Subject: [PATCH 03/10] Fixed code and made it much simpler. Fixed a lot of stuff. --- src/main/java/com/team766/robot/OI.java | 24 +++---- .../com/team766/robot/mechanisms/Arms.java | 65 +++++++++---------- 2 files changed, 44 insertions(+), 45 deletions(-) diff --git a/src/main/java/com/team766/robot/OI.java b/src/main/java/com/team766/robot/OI.java index 068d26dc..15dd96f4 100644 --- a/src/main/java/com/team766/robot/OI.java +++ b/src/main/java/com/team766/robot/OI.java @@ -51,33 +51,33 @@ public void run(Context context) { } if(joystick0.getButton(3)){ //HIGH NODE SCORING - Robot.arms.maxPidArm1(-38.34); - Robot.arms.maxPidArm2(-90.665); + Robot.arms.pidForArmOne(-38.34); + Robot.arms.pidForArmTwo(-90.665); } if(joystick0.getButtonPressed(4)){ //MID NODE SCORING - Robot.arms.maxPidArm1(null); - Robot.arms.maxPidArm2(null); + Robot.arms.pidForArmOne(0); + Robot.arms.pidForArmTwo(0); } if(joystick0.getButton(5)){ //TAKING FROM HUMAN PLAYER - Robot.arms.maxPidArm1(null); - Robot.arms.maxPidArm2(null); + Robot.arms.pidForArmOne(0); + Robot.arms.pidForArmTwo(0); } if(joystick0.getButton(6)){ //STOWED - Robot.arms.maxPidArm1(null); - Robot.arms.maxPidArm2(null); + Robot.arms.pidForArmOne(0); + Robot.arms.pidForArmTwo(0); } if(joystick0.getButton(7)){ //READY - Robot.arms.maxPidArm1(null); - Robot.arms.maxPidArm2(null); + Robot.arms.pidForArmOne(0); + Robot.arms.pidForArmTwo(0); } if(joystick0.getButton(8)){ //PREP - Robot.arms.maxPidArm1(null); - Robot.arms.maxPidArm2(null); + Robot.arms.pidForArmOne(0); + Robot.arms.pidForArmTwo(0); } //TODO: can we stil use a switch? diff --git a/src/main/java/com/team766/robot/mechanisms/Arms.java b/src/main/java/com/team766/robot/mechanisms/Arms.java index 7e1a7b1b..3c5facdd 100644 --- a/src/main/java/com/team766/robot/mechanisms/Arms.java +++ b/src/main/java/com/team766/robot/mechanisms/Arms.java @@ -23,7 +23,10 @@ //This is for the motor that controls the pulley public class Arms extends Mechanism { - + /* + * This defines the motors and casts them to CanSparkMaxs (CSMs) so we can use REV Robotics PID SmartMotion. + * Next, it also defines the absolute encoder. + */ private MotorController firstJoint = RobotProvider.instance.getMotor("arms.firstJoint"); private CANSparkMax firstJointCANSparkMax = (CANSparkMax)firstJoint; private SparkMaxPIDController firstJointPIDController = firstJointCANSparkMax.getPIDController(); @@ -36,36 +39,31 @@ public class Arms extends Mechanism { // Non-motor constants - private static double doubleDeadZone = 0.005; - - enum ArmState{ - PID, // Moving - ANTIGRAV // Holding - }; + //This is the deadzone, so that the arm(s) don't oscillate. For example, a value of 5 means a 5 relitive encoder unit deadzone in each direction. + private static double doubleDeadZone = 5; - private ArmState firstJointState = ArmState.ANTIGRAV; - private ArmState secondJointState = ArmState.ANTIGRAV; + //Difining the anti grav variables private ValueProvider ANTI_GRAV_FIRST_JOINT = ConfigFileReader.getInstance().getDouble("arms.antiGravFirstJoint"); private ValueProvider ANTI_GRAV_SECOND_JOINT = ConfigFileReader.getInstance().getDouble("arms.antiGravSecondJoint"); private static final double ANTI_GRAV_FIRSTSECOND_JOINT = 0.001; - // We want firstJoint/secondJoint being straight-up to be 0 rel encoder units - // and counter-clockwise to be positive. - // All the following variables are in degrees + // This sets the maximum locations so we can use them in code to make sure the arm joints dont go past there. + private static final double FIRST_JOINT_MAX_LOCATION = 35; + private static final double FIRST_JOINT_MIN_LOCATION = -40; + private static final double SECOND_JOINT_MAX_LOCATION = 45; + private static final double SECOND_JOINT_MIN_LOCATION = -160; + + //Extra variables for use in antigrav. + private static double firstJointPosition; + private static double secondJointPosition; - // TODO: need to be set soon - private static final double FIRST_JOINT_MAX_LOCATION = 1; - private static final double FIRST_JOINT_MIN_LOCATION = 0; - private static final double SECOND_JOINT_MAX_LOCATION = 1; - private static final double SECOND_JOINT_MIN_LOCATION = 0; - private RateLimiter runRateLimiter = new RateLimiter(0.05); public Arms() { loggerCategory = Category.MECHANISMS; - resetEncoders(); + //PID Constants ValueProvider firstJointP = ConfigFileReader.getInstance().getDouble("arms.firstJointP"); ValueProvider firstJointI = ConfigFileReader.getInstance().getDouble("arms.firstJointI"); ValueProvider firstJointD = ConfigFileReader.getInstance().getDouble("arms.firstJointD"); @@ -74,7 +72,8 @@ public Arms() { firstJointPIDController.setI(firstJointI.valueOr(0.0)); firstJointPIDController.setD(firstJointD.valueOr(0.0)); firstJointPIDController.setFF(firstJointFF.valueOr(0.002)); - + + //More PID constants ValueProvider secondJointP = ConfigFileReader.getInstance().getDouble("arms.secondJointP"); ValueProvider secondJointI = ConfigFileReader.getInstance().getDouble("arms.secondJointI"); ValueProvider secondJointD = ConfigFileReader.getInstance().getDouble("arms.secondJointD"); @@ -84,6 +83,7 @@ public Arms() { secondJointPIDController.setD(0.00001); secondJointPIDController.setFF(0.00109); + //These next things deal a lot with the PID SmartMotion firstJointCANSparkMax.setInverted(false); firstJointPIDController.setSmartMotionMaxVelocity(4000, 0); firstJointPIDController.setSmartMotionMinOutputVelocity(0, 0); @@ -91,16 +91,20 @@ public Arms() { firstJointPIDController.setOutputRange(-0.75, 0.75); firstJointCANSparkMax.setSmartCurrentLimit(40); + //These too secondJointPIDController.setSmartMotionMaxVelocity(4000, 0); secondJointPIDController.setSmartMotionMinOutputVelocity(0, 0); secondJointPIDController.setSmartMotionMaxAccel(3000, 0); secondJointPIDController.setOutputRange(-1, 1); secondJointCANSparkMax.setSmartCurrentLimit(40); + //This resets the degrees and stuff so that we dont have to have the arm at certain positions to reset... firstJointPIDController.setFeedbackDevice(firstJointCANSparkMax.getEncoder()); secondJointPIDController.setFeedbackDevice(secondJointCANSparkMax.getEncoder()); altEncoder1.setZeroOffset(0.68); altEncoder2.setZeroOffset(0.62); + //and this resets it! + resetEncoders(); } @@ -118,7 +122,7 @@ public void manuallySetArmTwoPower(double power){ checkContextOwnership(); secondJoint.set(power); } - + //Resets encoders public void resetEncoders(){ checkContextOwnership(); @@ -140,8 +144,9 @@ public void resetEncoders(){ secondJointPosition = EUTodegrees(secondJointRelEncoder); } + //PID For arm one - public void maxPidArm1(double value){ + public void pidForArmOne(double value){ if(value > FIRST_JOINT_MAX_LOCATION){ value = FIRST_JOINT_MAX_LOCATION; }else if(value < FIRST_JOINT_MIN_LOCATION){ @@ -151,32 +156,27 @@ public void maxPidArm1(double value){ if(value + doubleDeadZone < altEncoder1.getPosition() && value - doubleDeadZone > altEncoder1.getPosition()){ getAntiGravFirstJoint(); }else{ - firstJointPIDController.setFeedbackDevice(altEncoder1); - //todo, is the antigrav here really needed? - //todo will the anti grav work with the absolute pids or does it just set power? firstJointPIDController.setReference(value, ControlType.kSmartMotion, 0, getAntiGravFirstJoint()); } } - public void maxPidArm2(double value){ + //PID For arm two + public void pidForArmTwo(double value){ if(value > SECOND_JOINT_MAX_LOCATION){ value = SECOND_JOINT_MAX_LOCATION; }else if(value < SECOND_JOINT_MIN_LOCATION){ value = SECOND_JOINT_MIN_LOCATION; } - + if(value + doubleDeadZone < altEncoder2.getPosition() && value - doubleDeadZone > altEncoder2.getPosition()){ getAntiGravSecondJoint(); }else{ - secondJointPIDController.setFeedbackDevice(altEncoder1); - //todo, is the antigrav here really needed? - //todo will the anti grav work with the absolute pids or does it just set power? secondJointPIDController.setReference(value, ControlType.kSmartMotion, 0, getAntiGravSecondJoint()); } } - + //This is our portion with antigrav // These next 3 antiGrav aren't used. public void antiGravBothJoints(){ antiGravFirstJoint(); @@ -185,12 +185,11 @@ public void antiGravBothJoints(){ public void antiGravFirstJoint(){ firstJoint.set(getAntiGravFirstJoint()); - firstJointState = ArmState.ANTIGRAV; } public void antiGravSecondJoint(){ secondJoint.set(getAntiGravSecondJoint()); - secondJointState = ArmState.ANTIGRAV; + } // These next 2 antiGravs are used. From aaef870b598d1150bb8bcd035de11172a95ba647 Mon Sep 17 00:00:00 2001 From: Max <68516760+TTVMixmix00@users.noreply.github.com> Date: Sat, 1 Apr 2023 11:53:36 -0700 Subject: [PATCH 04/10] Update arms.java deadzone Fixed per ryans pr comment --- src/main/java/com/team766/robot/mechanisms/Arms.java | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/main/java/com/team766/robot/mechanisms/Arms.java b/src/main/java/com/team766/robot/mechanisms/Arms.java index 3c5facdd..828f1ce3 100644 --- a/src/main/java/com/team766/robot/mechanisms/Arms.java +++ b/src/main/java/com/team766/robot/mechanisms/Arms.java @@ -153,7 +153,7 @@ public void pidForArmOne(double value){ value = FIRST_JOINT_MIN_LOCATION; } - if(value + doubleDeadZone < altEncoder1.getPosition() && value - doubleDeadZone > altEncoder1.getPosition()){ + if(value + doubleDeadZone < firstJoint.getSensorPosition() && value - doubleDeadZone > firstJoint.getSensorPosition()){ getAntiGravFirstJoint(); }else{ firstJointPIDController.setReference(value, ControlType.kSmartMotion, 0, getAntiGravFirstJoint()); @@ -168,7 +168,7 @@ public void pidForArmTwo(double value){ value = SECOND_JOINT_MIN_LOCATION; } - if(value + doubleDeadZone < altEncoder2.getPosition() && value - doubleDeadZone > altEncoder2.getPosition()){ + if(value + doubleDeadZone < secondJoint.getSensorPosition() && value - doubleDeadZone > secondJoint.getSensorPosition()){ getAntiGravSecondJoint(); }else{ secondJointPIDController.setReference(value, ControlType.kSmartMotion, 0, getAntiGravSecondJoint()); From 7719aceae6da6ea61bb7d46252ef4699df84e00e Mon Sep 17 00:00:00 2001 From: Max <68516760+TTVMixmix00@users.noreply.github.com> Date: Sat, 1 Apr 2023 11:55:17 -0700 Subject: [PATCH 05/10] updated per ryans pr comment --- src/main/java/com/team766/robot/mechanisms/Arms.java | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/main/java/com/team766/robot/mechanisms/Arms.java b/src/main/java/com/team766/robot/mechanisms/Arms.java index 828f1ce3..9ba4a6ea 100644 --- a/src/main/java/com/team766/robot/mechanisms/Arms.java +++ b/src/main/java/com/team766/robot/mechanisms/Arms.java @@ -154,7 +154,7 @@ public void pidForArmOne(double value){ } if(value + doubleDeadZone < firstJoint.getSensorPosition() && value - doubleDeadZone > firstJoint.getSensorPosition()){ - getAntiGravFirstJoint(); + antiGravFirstJoint(); }else{ firstJointPIDController.setReference(value, ControlType.kSmartMotion, 0, getAntiGravFirstJoint()); } @@ -169,7 +169,7 @@ public void pidForArmTwo(double value){ } if(value + doubleDeadZone < secondJoint.getSensorPosition() && value - doubleDeadZone > secondJoint.getSensorPosition()){ - getAntiGravSecondJoint(); + antiGravSecondJoint(); }else{ secondJointPIDController.setReference(value, ControlType.kSmartMotion, 0, getAntiGravSecondJoint()); } From 04d9c3a81a41ba3a5a48fecc9854f7bea32b02c4 Mon Sep 17 00:00:00 2001 From: Max <68516760+TTVMixmix00@users.noreply.github.com> Date: Sat, 1 Apr 2023 12:15:21 -0700 Subject: [PATCH 06/10] Fixed oi a bit (not really) but added enums and run() for Arms So this lets antigrav work! --- src/main/java/com/team766/robot/OI.java | 2 + .../com/team766/robot/mechanisms/Arms.java | 40 +++++++++++++++++-- 2 files changed, 38 insertions(+), 4 deletions(-) diff --git a/src/main/java/com/team766/robot/OI.java b/src/main/java/com/team766/robot/OI.java index 15dd96f4..53a73ff7 100644 --- a/src/main/java/com/team766/robot/OI.java +++ b/src/main/java/com/team766/robot/OI.java @@ -80,6 +80,8 @@ public void run(Context context) { Robot.arms.pidForArmTwo(0); } + Robot.arms.run(); + //TODO: can we stil use a switch? } diff --git a/src/main/java/com/team766/robot/mechanisms/Arms.java b/src/main/java/com/team766/robot/mechanisms/Arms.java index 9ba4a6ea..81480ebd 100644 --- a/src/main/java/com/team766/robot/mechanisms/Arms.java +++ b/src/main/java/com/team766/robot/mechanisms/Arms.java @@ -58,6 +58,14 @@ public class Arms extends Mechanism { private static double firstJointPosition; private static double secondJointPosition; + enum armStates{ + PID, + ANTIGRAV, + OFF + } + + armStates theStateOf1 = armStates.ANTIGRAV; + armStates theStateOf2 = armStates.ANTIGRAV; public Arms() { @@ -154,9 +162,10 @@ public void pidForArmOne(double value){ } if(value + doubleDeadZone < firstJoint.getSensorPosition() && value - doubleDeadZone > firstJoint.getSensorPosition()){ - antiGravFirstJoint(); + theStateOf1 = armStates.ANTIGRAV; }else{ - firstJointPIDController.setReference(value, ControlType.kSmartMotion, 0, getAntiGravFirstJoint()); + theStateOf1 = armStates.PID; + firstJointPIDController.setReference(degreesToEU(value), ControlType.kSmartMotion, 0, getAntiGravFirstJoint()); } } @@ -169,9 +178,10 @@ public void pidForArmTwo(double value){ } if(value + doubleDeadZone < secondJoint.getSensorPosition() && value - doubleDeadZone > secondJoint.getSensorPosition()){ - antiGravSecondJoint(); + theStateOf2 = armStates.ANTIGRAV; }else{ - secondJointPIDController.setReference(value, ControlType.kSmartMotion, 0, getAntiGravSecondJoint()); + theStateOf2 = armStates.PID; + secondJointPIDController.setReference(degreesToEU(value), ControlType.kSmartMotion, 0, getAntiGravSecondJoint()); } } @@ -247,6 +257,28 @@ public double lawOfSines(double side1, double angle1, double side2){ return Math.asin(side2*Math.sin(angle1)/side1); } + public void run(){ + switch(theStateOf1){ + case OFF: + break; + case PID: + break; + case ANTIGRAV: + antiGravFirstJoint(); + break; + } + + switch(theStateOf2){ + case OFF: + break; + case PID: + break; + case ANTIGRAV: + antiGravSecondJoint(); + break; + } + } + /* ~~ Code Review ~~ Use Voltage Control Mode when setting power (refer to CANSparkMaxMotorController.java) From 9c132af6e8978ea170714b637157f13592e69c7d Mon Sep 17 00:00:00 2001 From: TTVMixmix00 <68516760+TTVMixmix00@users.noreply.github.com> Date: Sat, 1 Apr 2023 13:15:52 -0700 Subject: [PATCH 07/10] Update arms.java with correct deadzone and new pid constants --- src/main/java/com/team766/robot/mechanisms/Arms.java | 12 ++++++------ 1 file changed, 6 insertions(+), 6 deletions(-) diff --git a/src/main/java/com/team766/robot/mechanisms/Arms.java b/src/main/java/com/team766/robot/mechanisms/Arms.java index 81480ebd..3be2eeca 100644 --- a/src/main/java/com/team766/robot/mechanisms/Arms.java +++ b/src/main/java/com/team766/robot/mechanisms/Arms.java @@ -79,7 +79,7 @@ public Arms() { firstJointPIDController.setP(firstJointP.valueOr(0.0006)); firstJointPIDController.setI(firstJointI.valueOr(0.0)); firstJointPIDController.setD(firstJointD.valueOr(0.0)); - firstJointPIDController.setFF(firstJointFF.valueOr(0.002)); + firstJointPIDController.setFF(firstJointFF.valueOr(0.001)); //More PID constants ValueProvider secondJointP = ConfigFileReader.getInstance().getDouble("arms.secondJointP"); @@ -88,8 +88,8 @@ public Arms() { ValueProvider secondJointFF = ConfigFileReader.getInstance().getDouble("arms.secondJointFF"); secondJointPIDController.setP(0.0005); secondJointPIDController.setI(secondJointI.valueOr(0.0)); - secondJointPIDController.setD(0.00001); - secondJointPIDController.setFF(0.00109); + secondJointPIDController.setD(0.0000); + secondJointPIDController.setFF(0.0008); //These next things deal a lot with the PID SmartMotion firstJointCANSparkMax.setInverted(false); @@ -161,7 +161,7 @@ public void pidForArmOne(double value){ value = FIRST_JOINT_MIN_LOCATION; } - if(value + doubleDeadZone < firstJoint.getSensorPosition() && value - doubleDeadZone > firstJoint.getSensorPosition()){ + if(value + doubleDeadZone > firstJoint.getSensorPosition() && value - doubleDeadZone < firstJoint.getSensorPosition()){ theStateOf1 = armStates.ANTIGRAV; }else{ theStateOf1 = armStates.PID; @@ -177,7 +177,7 @@ public void pidForArmTwo(double value){ value = SECOND_JOINT_MIN_LOCATION; } - if(value + doubleDeadZone < secondJoint.getSensorPosition() && value - doubleDeadZone > secondJoint.getSensorPosition()){ + if(value + doubleDeadZone > secondJoint.getSensorPosition() && value - doubleDeadZone < secondJoint.getSensorPosition()){ theStateOf2 = armStates.ANTIGRAV; }else{ theStateOf2 = armStates.PID; @@ -285,4 +285,4 @@ Use Voltage Control Mode when setting power (refer to CANSparkMaxMotorController Maybe use Nicholas's formula for degrees to EU "Use break mode" - Ronald the not programmer */ -} \ No newline at end of file +} From 0df97543e0b891ae4a2a042819a09d97c25bac68 Mon Sep 17 00:00:00 2001 From: TTVMixmix00 <68516760+TTVMixmix00@users.noreply.github.com> Date: Sat, 1 Apr 2023 13:18:41 -0700 Subject: [PATCH 08/10] Added new code for enum run() --- src/main/java/com/team766/robot/mechanisms/Arms.java | 2 ++ 1 file changed, 2 insertions(+) diff --git a/src/main/java/com/team766/robot/mechanisms/Arms.java b/src/main/java/com/team766/robot/mechanisms/Arms.java index 3be2eeca..89009501 100644 --- a/src/main/java/com/team766/robot/mechanisms/Arms.java +++ b/src/main/java/com/team766/robot/mechanisms/Arms.java @@ -260,6 +260,7 @@ public double lawOfSines(double side1, double angle1, double side2){ public void run(){ switch(theStateOf1){ case OFF: + firstJoint.set(0); break; case PID: break; @@ -270,6 +271,7 @@ public void run(){ switch(theStateOf2){ case OFF: + secondJoint.set(0); break; case PID: break; From 0c9e29e58406517626c1fb0c0cb690259ee26dc0 Mon Sep 17 00:00:00 2001 From: Max <68516760+TTVMixmix00@users.noreply.github.com> Date: Sat, 1 Apr 2023 13:37:15 -0700 Subject: [PATCH 09/10] Added only 1 arm at a time control. --- src/main/java/com/team766/robot/OI.java | 6 ++++++ .../java/com/team766/robot/mechanisms/Arms.java | 14 +++++++++++++- 2 files changed, 19 insertions(+), 1 deletion(-) diff --git a/src/main/java/com/team766/robot/OI.java b/src/main/java/com/team766/robot/OI.java index 53a73ff7..999b8f4d 100644 --- a/src/main/java/com/team766/robot/OI.java +++ b/src/main/java/com/team766/robot/OI.java @@ -53,31 +53,37 @@ public void run(Context context) { //HIGH NODE SCORING Robot.arms.pidForArmOne(-38.34); Robot.arms.pidForArmTwo(-90.665); + Robot.arms.checkJointTwo(-90.665); } if(joystick0.getButtonPressed(4)){ //MID NODE SCORING Robot.arms.pidForArmOne(0); Robot.arms.pidForArmTwo(0); + Robot.arms.checkJointTwo(0); } if(joystick0.getButton(5)){ //TAKING FROM HUMAN PLAYER Robot.arms.pidForArmOne(0); Robot.arms.pidForArmTwo(0); + Robot.arms.checkJointTwo(0); } if(joystick0.getButton(6)){ //STOWED Robot.arms.pidForArmOne(0); Robot.arms.pidForArmTwo(0); + Robot.arms.checkJointTwo(0); } if(joystick0.getButton(7)){ //READY Robot.arms.pidForArmOne(0); Robot.arms.pidForArmTwo(0); + Robot.arms.checkJointTwo(0); } if(joystick0.getButton(8)){ //PREP Robot.arms.pidForArmOne(0); Robot.arms.pidForArmTwo(0); + Robot.arms.checkJointTwo(0); } Robot.arms.run(); diff --git a/src/main/java/com/team766/robot/mechanisms/Arms.java b/src/main/java/com/team766/robot/mechanisms/Arms.java index 89009501..6425b5ef 100644 --- a/src/main/java/com/team766/robot/mechanisms/Arms.java +++ b/src/main/java/com/team766/robot/mechanisms/Arms.java @@ -64,6 +64,8 @@ enum armStates{ OFF } + boolean jointOneCanContinue = false; + armStates theStateOf1 = armStates.ANTIGRAV; armStates theStateOf2 = armStates.ANTIGRAV; @@ -154,6 +156,14 @@ public void resetEncoders(){ //PID For arm one + public void checkJointTwo(double value){ + if((value + doubleDeadZone > secondJoint.getSensorPosition() && value - doubleDeadZone < secondJoint.getSensorPosition())){ + jointOneCanContinue = true; + }else{ + jointOneCanContinue = false; + } + } + public void pidForArmOne(double value){ if(value > FIRST_JOINT_MAX_LOCATION){ value = FIRST_JOINT_MAX_LOCATION; @@ -163,9 +173,11 @@ public void pidForArmOne(double value){ if(value + doubleDeadZone > firstJoint.getSensorPosition() && value - doubleDeadZone < firstJoint.getSensorPosition()){ theStateOf1 = armStates.ANTIGRAV; - }else{ + }else if(jointOneCanContinue){ theStateOf1 = armStates.PID; firstJointPIDController.setReference(degreesToEU(value), ControlType.kSmartMotion, 0, getAntiGravFirstJoint()); + }else{ + log("joint one cannot continue"); } } From c4e51d3fd17e865c09b47135832f43e127bff9db Mon Sep 17 00:00:00 2001 From: Max <68516760+TTVMixmix00@users.noreply.github.com> Date: Sat, 1 Apr 2023 14:47:17 -0700 Subject: [PATCH 10/10] uiewj --- src/main/java/com/team766/robot/OI.java | 5 ++--- .../java/com/team766/robot/mechanisms/Arms.java | 15 ++++++++++----- 2 files changed, 12 insertions(+), 8 deletions(-) diff --git a/src/main/java/com/team766/robot/OI.java b/src/main/java/com/team766/robot/OI.java index 999b8f4d..ae4b09ff 100644 --- a/src/main/java/com/team766/robot/OI.java +++ b/src/main/java/com/team766/robot/OI.java @@ -57,9 +57,8 @@ public void run(Context context) { } if(joystick0.getButtonPressed(4)){ //MID NODE SCORING - Robot.arms.pidForArmOne(0); - Robot.arms.pidForArmTwo(0); - Robot.arms.checkJointTwo(0); + Robot.arms.pidForArmTwo(-90); + Robot.arms.checkJointTwo(-90); } if(joystick0.getButton(5)){ //TAKING FROM HUMAN PLAYER diff --git a/src/main/java/com/team766/robot/mechanisms/Arms.java b/src/main/java/com/team766/robot/mechanisms/Arms.java index 6425b5ef..df0c56fb 100644 --- a/src/main/java/com/team766/robot/mechanisms/Arms.java +++ b/src/main/java/com/team766/robot/mechanisms/Arms.java @@ -66,8 +66,8 @@ enum armStates{ boolean jointOneCanContinue = false; - armStates theStateOf1 = armStates.ANTIGRAV; - armStates theStateOf2 = armStates.ANTIGRAV; + armStates theStateOf1 = armStates.OFF; + armStates theStateOf2 = armStates.OFF; public Arms() { @@ -189,12 +189,14 @@ public void pidForArmTwo(double value){ value = SECOND_JOINT_MIN_LOCATION; } - if(value + doubleDeadZone > secondJoint.getSensorPosition() && value - doubleDeadZone < secondJoint.getSensorPosition()){ + if(value + doubleDeadZone > EUTodegrees(secondJoint.getSensorPosition()) && value - doubleDeadZone < EUTodegrees(secondJoint.getSensorPosition())){ theStateOf2 = armStates.ANTIGRAV; + log("it got here"); }else{ theStateOf2 = armStates.PID; secondJointPIDController.setReference(degreesToEU(value), ControlType.kSmartMotion, 0, getAntiGravSecondJoint()); } + } @@ -272,7 +274,6 @@ public double lawOfSines(double side1, double angle1, double side2){ public void run(){ switch(theStateOf1){ case OFF: - firstJoint.set(0); break; case PID: break; @@ -283,7 +284,7 @@ public void run(){ switch(theStateOf2){ case OFF: - secondJoint.set(0); + break; case PID: break; @@ -291,6 +292,10 @@ public void run(){ antiGravSecondJoint(); break; } + log("First" + EUTodegrees(firstJoint.getSensorPosition()) ); + log(" Second" + EUTodegrees(secondJoint.getSensorPosition())); + log(theStateOf2 + ""); + log("Difference: " + EUTodegrees(firstJoint.getSensorPosition())); }