diff --git a/src/main/java/com/team766/robot/OI.java b/src/main/java/com/team766/robot/OI.java index 5d354de7..ae4b09ff 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,49 @@ 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.checkJointTwo(-90.665); } if(joystick0.getButtonPressed(4)){ //MID NODE SCORING - Robot.arms.pidForArmOne(null); - Robot.arms.pidForArmTwo(null); + Robot.arms.pidForArmTwo(-90); + Robot.arms.checkJointTwo(-90); } - if(joystick0.getButton(5)){ //TAKING FROM HUMAN PLAYER - Robot.arms.pidForArmOne(null); - Robot.arms.pidForArmTwo(null); + Robot.arms.pidForArmOne(0); + Robot.arms.pidForArmTwo(0); + Robot.arms.checkJointTwo(0); } - if(joystick0.getButton(6)){ //STOWED - Robot.arms.pidForArmOne(null); - Robot.arms.pidForArmTwo(null); + Robot.arms.pidForArmOne(0); + Robot.arms.pidForArmTwo(0); + Robot.arms.checkJointTwo(0); } if(joystick0.getButton(7)){ //READY - Robot.arms.pidForArmOne(null); - Robot.arms.pidForArmTwo(null); + Robot.arms.pidForArmOne(0); + Robot.arms.pidForArmTwo(0); + Robot.arms.checkJointTwo(0); } if(joystick0.getButton(8)){ //PREP - Robot.arms.pidForArmOne(null); - Robot.arms.pidForArmTwo(null); + Robot.arms.pidForArmOne(0); + Robot.arms.pidForArmTwo(0); + Robot.arms.checkJointTwo(0); } + Robot.arms.run(); + //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..df0c56fb 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,41 +39,41 @@ public class Arms extends Mechanism { // Non-motor constants + //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; - enum ArmState{ - PID, // Moving - ANTIGRAV // Holding - }; - - 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 - - private double firstJointPosition = 0; - private double secondJointPosition = 0; - private double firstJointCombo = 0; - private double secondJointCombo = 0; - - // TODO: need to be set soon + // 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; - private RateLimiter runRateLimiter = new RateLimiter(0.05); + //Extra variables for use in antigrav. + private static double firstJointPosition; + private static double secondJointPosition; + + enum armStates{ + PID, + ANTIGRAV, + OFF + } + + boolean jointOneCanContinue = false; + + armStates theStateOf1 = armStates.OFF; + armStates theStateOf2 = armStates.OFF; + 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"); @@ -78,17 +81,19 @@ 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"); ValueProvider secondJointI = ConfigFileReader.getInstance().getDouble("arms.secondJointI"); ValueProvider secondJointD = ConfigFileReader.getInstance().getDouble("arms.secondJointD"); 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); firstJointPIDController.setSmartMotionMaxVelocity(4000, 0); firstJointPIDController.setSmartMotionMinOutputVelocity(0, 0); @@ -96,16 +101,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(); } @@ -123,7 +132,7 @@ public void manuallySetArmTwoPower(double power){ checkContextOwnership(); secondJoint.set(power); } - + //Resets encoders public void resetEncoders(){ checkContextOwnership(); @@ -145,84 +154,53 @@ public void resetEncoders(){ secondJointPosition = EUTodegrees(secondJointRelEncoder); } + //PID For arm one - public void maxPidArm1(double value){ + 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; }else if(value < FIRST_JOINT_MIN_LOCATION){ value = FIRST_JOINT_MIN_LOCATION; } - if(value + doubleDeadZone < altEncoder1.getSensorPosition() && value - doubleDeadZone > altEncoder1.getSensorPosition()){ - getAntiGravFirstJoint(); + if(value + doubleDeadZone > firstJoint.getSensorPosition() && value - doubleDeadZone < firstJoint.getSensorPosition()){ + theStateOf1 = armStates.ANTIGRAV; + }else if(jointOneCanContinue){ + theStateOf1 = armStates.PID; + firstJointPIDController.setReference(degreesToEU(value), ControlType.kSmartMotion, 0, getAntiGravFirstJoint()); }else{ - firstJointPIDController.setFeedbackDevice(altEncoder1); - //todo, is the antigrav here really needed? - firstJointPIDController.setReference(value, ControlType.kSmartMotion, 0, getAntiGravFirstJoint()); + log("joint one cannot continue"); } } - 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.getSensorPosition() && value - doubleDeadZone > altEncoder2.getSensorPosition()){ - getAntiGravSecondJoint(); + + if(value + doubleDeadZone > EUTodegrees(secondJoint.getSensorPosition()) && value - doubleDeadZone < EUTodegrees(secondJoint.getSensorPosition())){ + theStateOf2 = armStates.ANTIGRAV; + log("it got here"); }else{ - secondJointPIDController.setFeedbackDevice(altEncoder1); - //todo, is the antigrav here really needed? - secondJointPIDController.setReference(value, ControlType.kSmartMotion, 0, getAntiGravSecondJoint()); + theStateOf2 = armStates.PID; + secondJointPIDController.setReference(degreesToEU(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; - } - */ + //This is our portion with antigrav // These next 3 antiGrav aren't used. public void antiGravBothJoints(){ antiGravFirstJoint(); @@ -231,12 +209,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. @@ -265,70 +242,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 +271,37 @@ 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; + public void run(){ + switch(theStateOf1){ + case OFF: + break; + case PID: + break; + case ANTIGRAV: + antiGravFirstJoint(); + break; } - return value; + + switch(theStateOf2){ + case OFF: + + break; + case PID: + break; + case ANTIGRAV: + antiGravSecondJoint(); + break; + } + log("First" + EUTodegrees(firstJoint.getSensorPosition()) ); + log(" Second" + EUTodegrees(secondJoint.getSensorPosition())); + log(theStateOf2 + ""); + log("Difference: " + EUTodegrees(firstJoint.getSensorPosition())); } -} /* ~~ 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 + */ +}