work please

Author: twistybit

src/main/java/frc/team5115/Subsystems/Drivetrain.java

@@ -13,6 +13,7 @@
 
 public class Drivetrain extends SubsystemBase implements DriveBase {
     private Locationator locationator;
+
     //instances of the speed controllers
     private VictorSPX frontLeft;
     private VictorSPX frontRight;
@@ -36,9 +37,6 @@ public Drivetrain(RobotContainer x) {
 
         backRight.setInverted(true);
 
-        frontLeft.set(ControlMode.Follower, backLeft.getDeviceID());
-        frontRight.set(ControlMode.Follower, backRight.getDeviceID());
-
         //back motors are master
 
         frontLeft.configSelectedFeedbackSensor(FeedbackDevice.CTRE_MagEncoder_Relative);
@@ -58,36 +56,36 @@ public void drive(double x, double y, double throttle) { //Change the drive outp
         //called lots of times per seconds.
         //System.out.println("Driving with X:" + x + " Y: " + y + " throttle: " + throttle);
         //Math.sqrt(3.4* Math.log(x + y + 1));
-
-            leftSpd = (x + y) * throttle;
-            rightSpd = (x - y) * throttle;
-
+        //todome
+        //System.out.println("x = " + x);
+        //System.out.println("y = " + y);
+        leftSpd = (x-y) * throttle;
+        rightSpd = (x+y) * throttle;
 //        System.out.println("Setting Right Pair to :" + (int) rightSpd * 100);
 //        System.out.println("Setting Left Pair to :" + (int) leftSpd * 100);
 
+        frontLeft.set(ControlMode.PercentOutput, leftSpd);
+        frontRight.set(ControlMode.PercentOutput, rightSpd);
         backLeft.set(ControlMode.PercentOutput, leftSpd);
-        backRight.set(ControlMode.PercentOutput, -rightSpd);
+        backRight.set(ControlMode.PercentOutput, rightSpd);
     }
 
     public void XBoxDrive(Joystick joy) {
         double x = joy.getRawAxis(XBOX_X_AXIS_ID);
         double y = -joy.getRawAxis(XBOX_Y_AXIS_ID);
-        System.out.println("y = " + y);
+
         double throttle1 = joy.getRawAxis(XBOX_THROTTLE_1_ID);
         double throttle2 = joy.getRawAxis(XBOX_THROTTLE_2_ID);
 
         //throttle is between 0.5 and 1
-        double throttle = (1-throttle1) + (1-throttle2);
+        double throttle = (1 - throttle1) + (1 - throttle2);
         throttle /= 2;
         //throttle is between 0 (dont move) and 1, (full move)
-        throttle = ((1-MIN_XBOX_THROTTLE) * throttle) + MIN_XBOX_THROTTLE;
+        throttle = ((1 - MIN_XBOX_THROTTLE) * throttle) + MIN_XBOX_THROTTLE;
         //new Throttle is now max 1 and min 0.2
         //System.out.println("throttle = " + throttle);
-        x*=0.5;
-        y*=0.4;
         //drive(x,y,throttle);
 
-        System.out.println("Remove me if working!");
         driveByWire(x, y, throttle);
     }
 
@@ -101,54 +99,51 @@ public static double clamp(double d, double max) {
     }
 
     @Override
-    public void resetTargetAngle() { //set the current target angle to where we currently are.
+    public void resetTargetAngle() { //set the current txarget angle to where we currently are.
         targetAngle = locationator.getAngle();
         System.out.println("RESET RBW: Target Angle: " + targetAngle + " Current Angle: " + locationator.getAngle());
     }
 
     double I = 0;
-    double lastAngle = 0;
+    double lastAngle;
 
     public void relativeAngleHold(double targetAngle, double y) {
         this.targetAngle = targetAngle;
-        double kP = 0.025;
         double kI = 0.0;
         double kD = 0.1;
         double currentAngle = 0;
 //        System.out.println("currentAngle = " + currentAngle);
 //        System.out.println("lastAngle = " + lastAngle);
         double P = kP * (currentAngle - targetAngle);
-        if(P < 0.2 && P > -0.2) {
+        if (P < 0.2 && P > -0.2) {
             I = I + (P * kI);
         }
         double D = -kD * (lastAngle - currentAngle); //finds the difference in the last tick.
         double output = P + I + D;
         output = clamp(output, 0.5);
-
 //        System.out.println("P = " + P);
 //        System.out.println("I = " + I);
 //        System.out.println("D = " + D);
 //        System.out.println("output = " + output);
         this.drive(-output, y, 1);
         lastAngle = currentAngle;
     }
-
-
+    double kP = 0.1;
     public void angleHold(double targetAngle, double y, double throttle) {
         this.targetAngle = targetAngle;
-        double kP = 0.025;
         double kI = 0.0;
-        double kD = 0.1;// Hey if you are implementing a d part, use the navx.getRate
+        double kD = .1;// Hey if you are implementing a d part, use the navx.getRate
         double currentAngle = locationator.getAngle();
 //        System.out.println("currentAngle = " + currentAngle);
 //        System.out.println("lastAngle = " + lastAngle);
         double P = kP * (currentAngle - targetAngle);
-        if(P < 0.2 && P > -0.2) {
+        if (P < 0.2 && P > -0.2) {
             I = I + (P * kI);
         }
+        P *= (1.5-Math.abs(y));
+
         double D = -kD * (lastAngle - currentAngle); //finds the difference in the last tick.
         double output = P + I + D;
-        output = clamp(output, 0.5);
 //        System.out.println("P = " + P);
 //        System.out.println("I = " + I);
 //        System.out.println("D = " + D);
@@ -162,10 +157,10 @@ public void angleHold(double targetAngle, double y) {
     }
 
     public void driveByWire(double x, double y, double throttle) { //rotate by wire
-        System.out.println("x = " + x);
-        System.out.println("throttle = " + throttle);
-        targetAngle += x * 2.5; //at 50 ticks a second, this is 50 degrees a second because the max x is 1.
-        if(Math.abs(targetAngle - locationator.getAngle()) > 30) {
+        if (Math.abs(x) < XBOX_X_DEADZONE) x = 0;
+        targetAngle += x; //at 50 ticks a second, this is 50 degrees a second because the max x is 1.
+
+        if (Math.abs(targetAngle - locationator.getAngle()) > 30) {
             targetAngle = locationator.getAngle();
         }
         angleHold(targetAngle, y, throttle);
@@ -191,3 +186,4 @@ public double getSpeedInchesPerSecond() {
 
 }
 
+