Merge pull request #7 from team5115/testing

Author: jdawit

Z_our_docs/sysid_drivetrain_updated_3-29-23.json

@@ -6,42 +6,37 @@
 import com.revrobotics.RelativeEncoder;
 import com.revrobotics.SparkMaxRelativeEncoder;
 import edu.wpi.first.math.controller.*;
+import frc.team5115.Classes.Software.Arm;
 import edu.wpi.first.math.MathUtil;
 
 public class HardwareDrivetrain{
+    // Competition feedforward values - 6 inch diameter on KITT comp robot with arm and ballasts
+    private final double leftKs = 0.0378;
+    private final double leftKv = 2.7479;
+    private final double leftKa = 0.32825;
+    
+    private final double rightKs = 0.0528;
+    private final double rightKv = 2.8399;
+    private final double rightKa = 0.26071;
+    
+    // private final double combinedkP = 3.4349;
+    // END of comp robot values
 
-
-
-    // Competition feedforward and feedback (pid) values
-    // 6 inch diameter on COMP ROBOT WITH ARM and dumbells in back
-    private final double leftKs = 0.17463;
-    private final double leftKv = 2.8104;
-    private final double leftKa = 0.82143;
+    // Testbed feedforward values - 6 inch diameter on KATT testbed
+    // private final double leftKs = 0.090949;
+    // private final double leftKv = 2.783;
+    // private final double leftKa = 0.16477;
 
-    private final double rightKs = 0.17463;
-    private final double rightKv = 2.8104;
-    private final double rightKa = 0.82143;
-
-    // private final double leftKp = 1.6455;
-    // private final double rightKp = 1.6220;
-    // private final double Ki = 0.0;
-    // private final double Kd = 0.0;
-    // // END of comp robot values
-
-    // Testbed feedforward and feedback (pid) values - 6 inch diameter on testbed
-    /* private final double leftKs = 0.090949;
-    private final double leftKv = 2.783;
-    private final double leftKa = 0.16477;
+    // private final double rightKs = 0.099706;
+    // private final double rightKv = 2.8314;
+    // private final double rightKa = 0.14565;
+    // END of testbed values
 
-    private final double rightKs = 0.099706;
-    private final double rightKv = 2.8314;
-    private final double rightKa = 0.14565;
-    */
-
-    private final double leftKp = 0.0; // 3.7203 according to sysid
-    private final double rightKp = 0.0; // 3.7203 according to sysid
-    private final double Ki = 0.0;
-    private final double Kd = 0.0;
+
+    private final double leftKp = 0.0;
+    private final double rightKp = 0.0;
+    private final double Ki = 0.1;
+    private final double Kd = 0.1;
     // END of testbed values
 
     private final SimpleMotorFeedforward leftFeedForward = new SimpleMotorFeedforward(leftKs, leftKv, leftKa);
@@ -57,7 +52,10 @@ public class HardwareDrivetrain{
     private final RelativeEncoder leftEncoder = frontLeft.getEncoder(SparkMaxRelativeEncoder.Type.kHallSensor, 42);
     private final RelativeEncoder rightEncoder = frontRight.getEncoder(SparkMaxRelativeEncoder.Type.kHallSensor, 42);
 
-    public HardwareDrivetrain(){
+    private final Arm arm;
+
+    public HardwareDrivetrain(Arm arm){
+        this.arm = arm;
         resetEncoders();
         frontRight.setInverted(true);
     }
@@ -141,21 +139,23 @@ public void PlugandVoltDrive(double frontLeftVoltage, double frontRightVoltage,
      * @param rightSpeed the speed for the right motors in meters per second
      */
     public void plugandFFDrive(double leftSpeed, double rightSpeed) {
-        
-         if(Math.abs(leftSpeed - (leftEncoder.getVelocity()*NEO_VELOCITY_CALIBRATION))>1.5){
-            //System.out.println("Left too fast");
-            leftSpeed = (leftEncoder.getVelocity()*NEO_VELOCITY_CALIBRATION) + 1.5*Math.signum((leftSpeed - (leftEncoder.getVelocity()*NEO_VELOCITY_CALIBRATION)));
-        }
+        final double accelerationLimit = getAccelerationLimit(); // can't bother figuring the units, but it's not m/s^2
+        final double currentLeftVelocity = getLeftVelocity();
+        final double currentRightVelocity = getRightVelocity();
 
-        if(Math.abs(rightSpeed - (rightEncoder.getVelocity()*NEO_VELOCITY_CALIBRATION))>1){
-            //System.out.println("Right too fast");
-            rightSpeed = (rightEncoder.getVelocity()*NEO_VELOCITY_CALIBRATION) + 1.5*Math.signum((rightSpeed - (rightEncoder.getVelocity()*NEO_VELOCITY_CALIBRATION)));
-        }        
+        // limit left acceleration
+        if(Math.abs(leftSpeed - currentLeftVelocity) > accelerationLimit) {
+            leftSpeed = currentLeftVelocity + accelerationLimit * Math.signum(leftSpeed - currentLeftVelocity);
+        }
+        // limit right acceleration
+        if(Math.abs(rightSpeed - currentRightVelocity) > accelerationLimit) {
+            rightSpeed = currentRightVelocity + accelerationLimit * Math.signum(rightSpeed - currentRightVelocity);
+        }
 
         double leftVoltage = leftFeedForward.calculate(leftSpeed);
         double rightVoltage = rightFeedForward.calculate(rightSpeed);
-         leftVoltage += leftPID.calculate(leftEncoder.getVelocity() * NEO_VELOCITY_CALIBRATION, leftSpeed);
-         rightVoltage += rightPID.calculate(rightEncoder.getVelocity() * NEO_VELOCITY_CALIBRATION, rightSpeed);
+        leftVoltage += leftPID.calculate(currentLeftVelocity, leftSpeed);
+        rightVoltage += rightPID.calculate(currentRightVelocity, rightSpeed);
         // Work on better PID Analyzer
 
         leftVoltage = MathUtil.clamp(leftVoltage, -DRIVE_MOTOR_MAX_VOLTAGE, DRIVE_MOTOR_MAX_VOLTAGE);
@@ -167,8 +167,25 @@ public void plugandFFDrive(double leftSpeed, double rightSpeed) {
         frontRight.setVoltage(rightVoltage);
     }
 
+    private double getAccelerationLimit() {
+        return 1.5;
+        // final double angle = arm.getAngle();
+        // final double length = (arm.getBottomWinchLength() + arm.getTopWinchLength()) / 2;
+        // final double angleConstant = 0.01;
+        // final double lengthConstant = 0.017;
+        // return 1.0 / (((angle + 90.0) * angleConstant) + (length * lengthConstant) + 1.0) + 0.5;
+    }
+
     public void resetEncoders(){
         leftEncoder.setPosition(0);
         rightEncoder.setPosition(0);
     }
+
+    public double getLeftVelocity() {
+        return leftEncoder.getVelocity() * NEO_VELOCITY_CALIBRATION;
+    }
+
+    public double getRightVelocity() {
+        return rightEncoder.getVelocity() * NEO_VELOCITY_CALIBRATION;
+    }
 }

src/main/java/frc/team5115/Classes/Hardware/HardwareDrivetrain.java

@@ -59,7 +59,7 @@ public void close(){
     private void setLights(boolean wantsCones) {
         coneLight.set(wantsCones);
         cubeLight.set(!wantsCones);
-        System.out.println(wantsCones);
+        //System.out.println(wantsCones);
     }
 
 }

src/main/java/frc/team5115/Classes/Hardware/HardwareIntake.java

@@ -33,18 +33,7 @@ public void resetNAVx() {
      * @return the yaw of the navx from the last reset, ranging from -180 to 180 degrees 
      */
     public double getYawDeg() {
-        double angle = 0;
-        if(ahrs.getYaw()- yawAtReset < 0){
-        angle = 360+(ahrs.getYaw()- yawAtReset);
-        }
-        else{
-        angle = (ahrs.getYaw()- yawAtReset);
-        }
-
-        // if (angle > 180) {
-        //     return -360 + angle;
-        // }
-        return Math.abs(angle);
+        return clampAngle(ahrs.getYaw() - yawAtReset);
     }
 
     /**
@@ -57,11 +46,16 @@ public double getPitchDeg() {
         // for sideways mount
         // double angle = ahrs.getRoll() - pitchAtReset;
         // for flat mount
-        double angle = -ahrs.getRoll() - pitchAtReset;
-        if (angle > 180) {
-            return -360 + angle;
-        }
-        return angle;
+        return clampAngle(-ahrs.getRoll() - pitchAtReset);
+    }
+
+    /**
+     * Converts an angle into an equivalent angle in the range -180 to 180
+     * @param angle the input angle to convert
+     * @return the equivalent angle from -180 to 180
+     */
+    public static double clampAngle(double angle) {
+        return ((angle + 180.0) % 360.0) - 180.0;
     }
 
     public double getYawRad() {

src/main/java/frc/team5115/Classes/Hardware/NAVx.java

@@ -219,4 +219,8 @@ public void moveTop(){
     public void moveBottom(){
         intake.setBottomWinch(-0.2);
     }
+
+    public double getAngle() {
+        return angle;
+    }
 }

src/main/java/frc/team5115/Classes/Software/Arm.java

@@ -47,13 +47,13 @@ public class Drivetrain extends SubsystemBase{
     public static final double bA = 10;
     public static final double MaxArea = 0.1;
 
-    public Drivetrain(PhotonVision photonVision) {
+    public Drivetrain(PhotonVision photonVision, Arm arm) {
         this.photonVision = photonVision;
         throttle = new ThrottleControl(3, -3, 0.2);
-        anglePID = new PIDController(0.013, 0.0001, 0.0015);
+        anglePID = new PIDController(0.016, 0.0001, 0.0015);
 
         movingPID = new PIDController(0.01, 0, 0);
-        drivetrain = new HardwareDrivetrain();
+        drivetrain = new HardwareDrivetrain(arm);
         ramseteController = new RamseteController();
         kinematics = new DifferentialDriveKinematics(TRACKING_WIDTH_METERS);
         navx = new NAVx();
@@ -68,14 +68,14 @@ public void init() {
         // );
 
         poseEstimator = new DifferentialDrivePoseEstimator(
-            kinematics, navx.getYawRotation2D(), getLeftDistance(), getRightDistance(), new Pose2d()
+            kinematics, navx.getYawRotation2D(), getLeftDistance(), getRightDistance(), new Pose2d(), VecBuilder.fill(1, 1, 1), VecBuilder.fill(0, 0, 0)
         );
         System.out.println("Angle from navx" + navx.getYawDeg()
         );
     }
 
     public void stop() {
-        drivetrain.plugandFFDrive(0, 0);
+        drivetrain.PlugandVoltDrive(0, 0, 0, 0);
     }
 
     public double getLeftDistance(){
@@ -152,15 +152,15 @@ else if (Math.abs(y) > 1){
         return new double[] {x, y};
     }
 
-    @Deprecated
-    public void TankDriveToAngle(double angleDegrees) { 
+    public boolean TankDriveToAngle(double angleDegrees) { 
         double rotationDegrees = navx.getYawDeg();
-        System.out.println(rotationDegrees);
-        double turn = MathUtil.clamp(anglePID.calculate(rotationDegrees, angleDegrees), -1, 1);
+        System.out.println("remaining degrees: " + (rotationDegrees-angleDegrees));
+        double turn = MathUtil.clamp(anglePID.calculate(rotationDegrees, angleDegrees), -0.6, 0.6);
         leftSpeed = turn;
         rightSpeed = -turn;
 
         drivetrain.plugandFFDrive(leftSpeed, rightSpeed);
+        return Math.abs(rotationDegrees-angleDegrees)<17;
     }
 
     public void TankDriveToTrajectoryState(Trajectory.State tState) {
@@ -180,11 +180,13 @@ public void UpdateOdometry() {
         Optional<EstimatedRobotPose> result = photonVision.getEstimatedGlobalPose();
         if (result.isPresent()) {
             EstimatedRobotPose camPose = result.get();
+            System.out.println("its really working");
             poseEstimator.addVisionMeasurement(camPose.estimatedPose.toPose2d(), camPose.timestampSeconds);
         }
     }
 
     public Pose2d getEstimatedPose() {
+        UpdateOdometry();
         return poseEstimator.getEstimatedPosition();
     }
 
@@ -199,14 +201,12 @@ public boolean UpdateMoving(double dist, double startLeftDist, double startRight
         return Math.abs(remainingLeftDistance) < tolerance || Math.abs(remainingRightDistance) < tolerance;
     }      
 
-    public boolean UpdateTurning(double setpointAngle) {
-        double currentAngle = navx.getYawDeg();
-        double turn = MathUtil.clamp(anglePID.calculate(currentAngle, setpointAngle), -1, 1);
-        //System.out.println("turning @ " + turn + " m/s");
-        System.out.println(currentAngle);
-        drivetrain.plugandFFDrive(turn, -turn);
-
-        return Math.abs(currentAngle-setpointAngle)<5;
+    public boolean UpdateMovingWithVision(double dist, Pose2d pose, double speedMagnitude) {
+        double realdist = pose.getTranslation().getDistance(getEstimatedPose().getTranslation());
+        final double speed = speedMagnitude * Math.signum(dist);
+        drivetrain.plugandFFDrive(speed, speed);
+        final double tolerance = 0.1;
+        return Math.abs(realdist-dist) < tolerance;
     }
 
     public void resetNAVx(){

src/main/java/frc/team5115/Classes/Software/Drivetrain.java

@@ -15,13 +15,13 @@
 import frc.team5115.Constants.*;
 
 public class PhotonVision extends SubsystemBase{
-    // private PhotonCamera photonCameraL;
+     private PhotonCamera photonCameraL;
     // private PhotonCamera photonCameraR;
-    // private PhotonPoseEstimator photonPoseEstimatorL;
+     private PhotonPoseEstimator photonPoseEstimatorL;
     // private PhotonPoseEstimator photonPoseEstimatorR;
 
     public PhotonVision() {
-        // photonCameraL = new PhotonCamera(VisionConstants.leftCameraName);
+         photonCameraL = new PhotonCamera(VisionConstants.leftCameraName);
         // photonCameraR = new PhotonCamera(VisionConstants.rightCameraName);
         ArrayList<AprilTag> aprilTagList = new ArrayList<AprilTag>();
 
@@ -42,15 +42,15 @@ public PhotonVision() {
         aprilTagList.add(GenerateAprilTag(5, -7.908830, +2.741613, 000));
 
         AprilTagFieldLayout fieldLayout = new AprilTagFieldLayout(aprilTagList, FieldConstants.length, FieldConstants.width);
-        // photonPoseEstimatorL = new PhotonPoseEstimator(fieldLayout, PoseStrategy.CLOSEST_TO_REFERENCE_POSE, photonCameraL, VisionConstants.robotToCamL);
+         photonPoseEstimatorL = new PhotonPoseEstimator(fieldLayout, PoseStrategy.CLOSEST_TO_CAMERA_HEIGHT, photonCameraL, VisionConstants.robotToCamL);
         // photonPoseEstimatorR = new PhotonPoseEstimator(fieldLayout, PoseStrategy.CLOSEST_TO_REFERENCE_POSE, photonCameraR, VisionConstants.robotToCamR);
     }
 
     public Optional<EstimatedRobotPose> getEstimatedGlobalPose() {
         // The teeam assignment of the first grid the robot looks at is the team assignment of the robot
         // otherwise if we cant see any april tags trust the team assignment inputted on shuffle board
         //Trusting the left camera more, no idea on how to use filters to get the most information out of both cameras 2-6-2022
-        // if(photonPoseEstimatorL.update().isPresent()) return photonPoseEstimatorL.update();
+        if(photonPoseEstimatorL.update().isPresent()) return photonPoseEstimatorL.update();
         // if(photonPoseEstimatorR.update().isPresent()) return photonPoseEstimatorR.update();
         return Optional.empty();
     }