Swerve rotating issue

Technical Programming
1

This is our second year doing swerve I copied the code over from last year which worked but when I use the right stick to rotate the robot the wheels turn to the wrong angles
Desired:

/    \
\    /

Actual:

/    \
/    \

Here is my current code if anyone knows what I did wrong help would be appreciated.

 public void driveWithJoystick(Joystick _joy) {
      angle = ahrs.getYaw(); // defining variable for gyro
    radians = angle * Math.PI / 180;
    currentAngle = maxFLswerveEncoder.getPosition() / angleConstant; // setting the current angle of the wheel 11.4666
                                                                     // = tick per rotation/360
    currentAngle2 = maxFRswerveEncoder.getPosition() / angleConstant;
    currentAngle3 = maxBLswerveEncoder.getPosition() / angleConstant;
    currentAngle4 = maxBRswerveEncoder.getPosition() / angleConstant;
   // swerve formulas
		FWD = -(_joy.getRawAxis(1) /*+ _bakcupJoy.getRawAxis(1)*/);
		STR = _joy.getRawAxis(0) /*+ _bakcupJoy.getRawAxis(0)*/;
		RCW = _joy.getRawAxis(2)/* + _bakcupJoy.getRawAxis(2)*/;
		if (FWD < .08 && FWD > -.08) {
			FWD = 0;
		}
		if (STR < .08 && STR > -.08) {
			STR = 0;
		}
		if (RCW < .08 && RCW > -.08) {
			RCW = 0;
		}
		temp = FWD * Math.cos(radians) + STR * Math.sin(radians);
		STR = -FWD * Math.sin(radians) + STR * Math.cos(radians);
		FWD = temp;
		R = Math.sqrt((Constants.L * Constants.L) + (Constants.W * Constants.W));
		A = STR - RCW * (Constants.L / R);
		B = STR + RCW * (Constants.L / R);
		C = FWD - RCW * (Constants.W / R);
		D = FWD + RCW * (Constants.W / R);
		ws1 = Math.sqrt((B * B) + (C * C));
		ws2 = Math.sqrt((B * B) + (D * D));
		ws3 = Math.sqrt((A * A) + (D * D));
		ws4 = Math.sqrt((A * A) + (C * C));
		wa1 = Math.atan2(B, C) * 180 / Math.PI;
		wa2 = Math.atan2(B, D) * 180 / Math.PI;
		wa3 = Math.atan2(A, D) * 180 / Math.PI;	
		wa4 = Math.atan2(A, C) * 180 / Math.PI;
		max = ws1;
		if (ws2 > max) {
			max = ws2;
		}
		if (ws3 > max) {
			max = ws3;
		}
		if (ws4 > max) {
			max = ws4;
		}
		if (max > 1) {
			ws1 /= max;
			ws2 /= max;
			ws3 /= max;
			ws4 /= max;
		}
		// swerve formulas

    rotationAmmount = Math.IEEEremainder(wa1 - currentAngle, 360); // calculating ammount to move wheel
    rotationAmmount2 = Math.IEEEremainder(wa2 - currentAngle2, 360); // calculating ammount to move wheel
    rotationAmmount3 = Math.IEEEremainder(wa3 - currentAngle3, 360); // calculating ammount to move wheel
    rotationAmmount4 = Math.IEEEremainder(wa4 - currentAngle4, 360); // calculating ammount to move wheel

    if (FWD < .1 && FWD > -.1 && STR < .1 && STR > -.1 && RCW < .1 && RCW > -.1) { // not letting the wheels move unitll
                                                                                   // the joystick is pushed %10 down
      fxFLdrive.set(0);
      fxFRdrive.set(0);
      fxBLdrive.set(0);
      fxBRdrive.set(0);
    } else {
      fxFLdrive.set(-ws1); // driving drive wheel off the formulas
      fxFRdrive.set(ws2);
      fxBLdrive.set(ws3);
      fxBRdrive.set(-ws4);
    }
    SmartDashboard.putNumber("wa1", wa1);
    SmartDashboard.putNumber("wa2", wa2);
    SmartDashboard.putNumber("wa3", wa3);
    SmartDashboard.putNumber("wa4", wa4);
    maxFLswervePID.setReference((currentAngle + rotationAmmount) * angleConstant, ControlType.kPosition); // moving the
                                                                                                          // wheel
    // to the
    // required position
    maxFRswervePID.setReference((currentAngle2 + rotationAmmount2) * angleConstant, ControlType.kPosition); // moving
                                                                                                                        // the
    // wheel
    // to the
    // required position
    maxBLswervePID.setReference((currentAngle3 + rotationAmmount3) * angleConstant, ControlType.kPosition); // moving the
                                                                                                          // wheel
    // to the
    // required position
    maxBRswervePID.setReference((currentAngle4 + rotationAmmount4) * angleConstant, ControlType.kPosition); // moving the
                                                                                                          // wheel
    // to the
    // required position
  }
2

not much of a programmer, but could you have accidentally swapped the signal for the rear left and rear right pods?

2 Likes
3

It looks like you haven’t been consistent in the numbering of your wheels:

Here you number them FL,FR,BL,BR for 1-4:

And here you label them FR,FL,BL,BR per Ether’s paper (see pic below)

Ether's Paper Picture
Ether&#39;s Paper Picture1128Ă—818 52.6 KB

1 Like
4

So i got it rotating to the right angles but the wheels are spinning the wrong direction one side of the robots wheels are turning as if it was trying to turn left and the other side to turn right here is the updated code

  public void driveWithJoystick(Joystick _joy) {
      angle = ahrs.getYaw(); // defining variable for gyro
    radians = angle * Math.PI / 180;
    currentAngle = maxBRswerveEncoder.getPosition() / angleConstant; // setting the current angle of the wheel 11.4666
                                                                     // = tick per rotation/360
    currentAngle2 = maxBLswerveEncoder.getPosition() / angleConstant;
    currentAngle3 = maxFLswerveEncoder.getPosition() / angleConstant;
    currentAngle4 = maxFRswerveEncoder.getPosition() / angleConstant;
   // swerve formulas
		FWD = -_joy.getRawAxis(1);
		STR = -_joy.getRawAxis(0) ;
		RCW = _joy.getRawAxis(2);
		if (FWD < .08 && FWD > -.08) {
			FWD = 0;
		}
		if (STR < .08 && STR > -.08) {
			STR = 0;
		}
		if (RCW < .08 && RCW > -.08) {
			RCW = 0;
		}
		temp = FWD * Math.cos(radians) + STR * Math.sin(radians);
		STR = -FWD * Math.sin(radians) + STR * Math.cos(radians);
		FWD = temp;
		R = Math.sqrt((Constants.L * Constants.L) + (Constants.W * Constants.W));
		A = STR - RCW * (Constants.L / R);
		B = STR + RCW * (Constants.L / R);
		C = FWD - RCW * (Constants.W / R);
		D = FWD + RCW * (Constants.W / R);
		ws1 = Math.sqrt((B * B) + (C * C));
		ws2 = Math.sqrt((B * B) + (D * D));
		ws3 = Math.sqrt((A * A) + (D * D));
		ws4 = Math.sqrt((A * A) + (C * C));
		wa1 = Math.atan2(B, C) * 180 / Math.PI;
		wa2 = Math.atan2(B, D) * 180 / Math.PI;
		wa3 = Math.atan2(A, D) * 180 / Math.PI;	
		wa4 = Math.atan2(A, C) * 180 / Math.PI;
		max = ws1;
		if (ws2 > max) {
			max = ws2;
		}
		if (ws3 > max) {
			max = ws3;
		}
		if (ws4 > max) {
			max = ws4;
		}
		if (max > 1) {
			ws1 /= max;
			ws2 /= max;
			ws3 /= max;
			ws4 /= max;
		}
		// swerve formulas

    rotationAmmount = Math.IEEEremainder(wa1 - currentAngle, 360); // calculating ammount to move wheel
    rotationAmmount2 = Math.IEEEremainder(wa2 - currentAngle2, 360); // calculating ammount to move wheel
    rotationAmmount3 = Math.IEEEremainder(wa3 - currentAngle3, 360); // calculating ammount to move wheel
    rotationAmmount4 = Math.IEEEremainder(wa4 - currentAngle4, 360); // calculating ammount to move wheel

    if (FWD < .1 && FWD > -.1 && STR < .1 && STR > -.1 && RCW < .1 && RCW > -.1) { // not letting the wheels move unitll
                                                                                   // the joystick is pushed %10 down
      fxFLdrive.set(0);
      fxFRdrive.set(0);
      fxBLdrive.set(0);
      fxBRdrive.set(0);
    } else {
      fxBRdrive.set(ws1); // driving drive wheel off the formulas
      fxBLdrive.set(ws2);
      fxFLdrive.set(ws3);
      fxFRdrive.set(ws4);
    }
    SmartDashboard.putNumber("wa1", wa1);
    SmartDashboard.putNumber("wa2", wa2);
    SmartDashboard.putNumber("wa3", wa3);
    SmartDashboard.putNumber("wa4", wa4);
    maxBRswervePID.setReference((currentAngle + rotationAmmount) * angleConstant, ControlType.kPosition); // moving the
                                                                                                          // wheel
    // to the
    // required position
    maxBLswervePID.setReference((currentAngle2 + rotationAmmount2) * angleConstant, ControlType.kPosition); // moving
                                                                                                                        // the
    // wheel
    // to the
    // required position
    maxFLswervePID.setReference((currentAngle3 + rotationAmmount3) * angleConstant, ControlType.kPosition); // moving the
                                                                                                          // wheel
    // to the
    // required position
    maxFRswervePID.setReference((currentAngle4 + rotationAmmount4) * angleConstant, ControlType.kPosition); // moving the
                                                                                                          // wheel
    // to the
    // required position
  }
5

I solved the issue it seems I had the wheel speeds for half the robot backwards meaning it was doing it while turning and driving strait.

6

This topic was automatically closed 365 days after the last reply. New replies are no longer allowed.