It's super important to make sure all your DcMotors have their encoder cords plugged in!

Previously we were only able to move forward, backward, and rotate. However this is pretty limited. We know that when we made TeleOps, our robot has a lot more of a range of motion. We learned that we could add and subtract drive, strafe, and turn components in order to make our Mecanum robot drive and strafe intuitively with our joysticks. An important note to remember, however, is that each DcMotor required a different combination of addition and subtraction in order to move properly. It went a bit like this (I'm ignoring the power parameter we used to regulate how fast we went):

fr.setPower(drive - strafe - turn);
fl.setPower(drive + strafe + turn);
br.setPower(drive + strafe - turn);
bl.setPower(drive - strafe + turn);

Let's think about this another way however. What if our robot was instead a point on a coordinate grid. This point could translate (move in straight lines) in the x and y direction and rotate. Well our drive component is a value that changes our robot's y, and our strafe component is a value that changes our x.

// yPow = drive | xPow = strafe | rPow = turn
fr.setPower(yPow - xPow - rPow);
fl.setPower(yPow + xPow + rPow);
br.setPower(yPow + xPow - rPow);
bl.setPower(yPow - xPow + rPow);

So when you think about it, the power components we supply should be proportional to the distances we move in each respective direction. If our xPow is greater than our yPow, our robot's distances traveled in the x-direction should be greater than that of the y. If our xPow is equal to our yPow, we should be moving diagonally, and therefore our x and y distances traveled should be equal. What if we were to dissect these combinations of x, y, and r components to solve for our distances traveled in those respective directions. Well you guys already did this technically. In your getPosition() method, you dissected the combinations above to solve for the distance the robot moved in the y direction. Take a look:

public double getPosition(){
   return (fr.getCurrentPosition() + fl.getCurrentPosition() + br.getCurrentPosition() + bl.getCurrentPosition()) / 4.0;
}

Let's substitute as a proof of concept. (Note: This is just pseudocode)

frDist = yDist - xDist  - rDist
flDist = yDist + xDist  + rDist
brDist = yDist + xDist  - rDist
blDist = yDist - xDist  + rDist

// What are we effectively doing by adding all these motor distances together?
// The xDist components all cancel out, and so do the rDist components.
  (yDist - xDist  - rDist)
  (yDist + xDist  + rDist)
  (yDist + xDist  - rDist)
+ (yDist - xDist  + rDist)
__________________________
=  yDist x 4

// All that is left to do is divide by 4.0 in order to calculate our robot's distance traveled in the y-direction.
// This all comes together in your getPosition() method as...
(frDist + flDist + brDist + blDist) / 4.0;

Cool, right? Now let's solve for the robot's distance traveled in the x-direction. The final equation will look a lot like the way you solved for the y-distance. This is because we need to get the yDist and rDist values to cancel out! Let's bite this problem in pieces by first trying to cancel out the yDist components. If you want to pause and try some algebra for yourself, go for it!

We want to solve for xDist, so we need to use some primetime algebra skills and eliminate our yDist and rDist values.

frDist = yDist - xDist  - rDist
flDist = yDist + xDist  + rDist
brDist = yDist + xDist  - rDist
blDist = yDist - xDist  + rDist

// Let's use these four equations and first solve for `yDist`:

frDist = yDist - xDist - rDist      flDist = yDist + xDist + rDist      brDist = yDist + xDist - rDist      flDist = yDist - xDist + rDist
               + xDist + rDist                     - xDist - rDist                     - xDist + rDist                     + xDist - rDist
______________________________      ______________________________      ______________________________      ______________________________
frDist + xDist + rDist = yDist      flDist - xDist - rDist = yDist      brDist - xDist + rDist = yDist      blDist + xDist - rDist = yDist

Now we can technically set all these equations equal to one another since yDist should be the same for all motors. Let's do this in pairs. If we start to solve for rDist we'll get to a point where our resulting equations are equal to each other.
Note: A single slash "/" means divide and an asterisk "*" means to multiply

frDist + xDist + rDist = flDist - xDist - rDist         brDist - xDist + rDist = blDist + xDist - rDist         
-frDist - xDist                         + rDist         -brDist + xDist                         + rDist         
_______________________________________________         _______________________________________________         
(2 * rDist) = flDist - frDist - (2 * xDist)             (2 * rDist) = blDist - brDist + (2 * xDist)         

Finally, we have boiled it down to two equations. Let's set these equal to each other, to remove rDist. Then solve for x!

flDist - frDist - (2 * xDist) = blDist - brDist + (2 * xDist)
                + (2 * xDist)   -blDist + brDist
_____________________________________________________________
flDist - frDist - blDist + brDist = 4 * xDist

// Finally we can solve for xDist by dividing by 4 on each side
(flDist - frDist - blDist + brDist) / 4 = xDist

And voila this is our xDist equation. Ready for the rDist equation? Just joking. We tried that already, it doesn't have much practical use for autos so we won't use it. Let's update our Mecanum class's getPosition() method with the new yDist and xDist equations.

Our getPosition() method should do the following:

• take no parameters
• use the equations above to calculate and store the robot's y-distance traveled in a double called yDist
• use the equations above to calculate and store the robot's x-distance traveled in a double called xDist
• return a Point of (xDist, yDist) (use the org.opencv.core version)

Remember: Use the DcMotor's getCurrentPosition() method to get a motor's current distance traveled.

It should look a little something like this:

public Point getPosition(){
   double yDist = (fr.getCurrentPosition() + fl.getCurrentPosition() + br.getCurrentPosition() + bl.getCurrentPosition()) / 4.0;
   double xDist = (fl.getCurrentPosition() - fr.getCurrentPosition() + br.getCurrentPosition() - bl.getCurrentPosition()) / 4.0;
   return new Point(xDist, yDist);
}

Now it's important to remember we'll be using a Linear OpMode. If you navigate to your Lab3 folder and open up LinearTeleOp, you'll find your autonomous code inside of the runOpMode() method. In the runOpMode() we make calls to the Mecanum's strafe() method which takes in a single parameter: ticks - the distance we should travel. However, we now want the ability to strafe in ANY direction. Let's go into our Mecanum class and add a new parameter to our strafe method.

• Make a new parameter of type double, and let's call it strafeAngle

We need to be able to take use our strafeAngle to figure out how we should be moving in the x and y-direction relative to the robot. Well, we can use some handy dandy trigonometry to help us out.



Let's say we want our robot to strafe at 45 degrees. Ideally, that means our robot should be moving equally fast in the x and y-direction. Therefore our x and y powers supplied to the setDrivePower() should be the same. (Note: Unfortunately the robot strafes slower than it drives forward and reverses which causes issues. We'll be tackling a fix for that later. Just ignore it for now.) Using your knowledge of trig so far, how can we get the x and y components above using just an angle?

We can use the sin() and cos() functions! Going back to our example of 45 degrees, we want two doubles, a xPower, and a yPower, to be equal. Our trig functions take two parameters:

• a double called value - the value to be input into the trig function
• an enum of either DEGREES, or RADIANS (Note: An enum is a special 'class' that represents a group of constants. We often use them as keywords to provide a little bit more intuitive sense to our code. You've actually already used enums when you set the direction of a DcMotor!) We can use the following syntax to calculate our x and y powers.
  

double xPower = cos(strafeAngle, DEGREES);
double yPower = sin(strafeAngle, DEGREES);

Next, we need to calculate the distances we will be traveling in the x and y-direction. We figured out before that our distances traveled are proportional to the powers we set. So we can just multiply our powers by the distance we need to travel, ticks, to find out how far we should go. We should store these values in new doubles called xDist and yDist respectively:

double xDist = xPower * ticks;
double yDist = yPower * ticks;

As a reminder, our strafe() will only run once in our LinearOpMode once it is called. In order to move for an extended period of time, we should continuously set our power while we haven't reached our target distance specified by the parameter ticks. We should initialize some variables before our loop to track our curPos, and finally our curHDist. To start out let's set our curPos to an empty Point and our curHDist to 0 like so:

Point curPos = new Point(0, 0);
double curHDist = 0;

Awesome. Here's an example of what your code might look like now:

public void strafe(double ticks, double targetAngle, double strafeAngle){

    // Reset our encoders to 0
    resetMotors();

    // Calculate our x and y powers
    double xPower = cos(strafeAngle, DEGREES);
    double yPower = sin(strafeAngle, DEGREES);

    // Calculate the distances we need to travel
    double xDist = xPower * ticks;
    double yDist = yPower * ticks;
    
    // Initialize our current position variables
    Point curPos = new Point(0, 0);
    double curHDist = 0;
    
    // Haven't gotten here quite yet
    while (.....){
        // Do a bunch of stuff
    }
    setAllPower(0);
}

Great, let's proceed to the while loop now. So instead of checking if we've reached our yDist, we now need to check if we've reached our total distance, a.k.a. ticks. Let's code our while loop as such:

• repeat while curHDist is less than ticks
  • update curXDist and curYDist with the getPosition() method
  • update curHDist with the hypotenuse of curXDist, and curYDist
  • use the setDrivePower() method to set the xPower and yPower to our motors accordingly (I highly suggest setting a low power for testing, like 0.3)
• set all the motors to 0 after the loop ends

while (curHDist < ticks){
    curPos = getPosition();
    curHDist = hypnot(curPos.x, curPos.y);
    setDrivePower(yPower, xPower, 0, 0.3);
}
setAllPower(0);

Let's put it all together!!!! And let's add some telemetry for debugging.

public void strafe(double ticks, double targetAngle, double strafeAngle){

    // Reset our encoders to 0
    resetMotors();

    // Calculate our x and y powers
    double xPower = cos(strafeAngle, DEGREES);
    double yPower = sin(strafeAngle, DEGREES);

    // Calculate the distances we need to travel
    double xDist = xPower * ticks;
    double yDist = yPower * ticks;
    
    // Initialize our current position variables
    Point curPos = new Point(0, 0);
    double curHDist = 0;
    
    while (curHDist < ticks){
        curPos = getPosition();
        curHDist = hypnot(curPos.x, curPos.y);
        setDrivePower(yPower, xPower, 0, 0.3);

        // Log some data out for debugging
        multTelemetry.addData("curPos", "(" + curPos.x + ", " + curPos.y + ")");
        multTelemetry.addData("curHDist", curHDist);
        multTelemetry.update();
    }
    setAllPower(0);
}

One final thing... we want to be able to strafe at an angle relative to the field regardless of where we're facing. Let's shift the powers every loop just like we do with the TeleOp. The issue is our positions will also be shifted. Let's unshift them with the following (We can put this method in our MathUtils class):

    public static Point unShift(Point sp, double shiftAngle){
        double r = toRadians(shiftAngle);
        double x = sp.x * Math.sin(r) + sp.y * Math.cos(r);
        double y = sp.x * Math.cos(r) - sp.y * Math.sin(r);
        return new Point(x, y);
    }

Our strafe method will look like this now:

public void strafe(double ticks, double targetAngle, double strafeAngle){

    // Reset our encoders to 0
    resetMotors();

    targetAngle = closestAngle(targetAngle, imu.getAngle());

    // Calculate our x and y powers
    double xPower = cos(strafeAngle, DEGREES);
    double yPower = sin(strafeAngle, DEGREES);

    // Calculate the distances we need to travel
    double xDist = xPower * ticks;
    double yDist = yPower * ticks;
    
    // Initialize our current position variables
    Point curPos = new Point(0, 0);
    double curHDist = 0;
    
    while (curHDist < ticks){
        curPos = getPosition();
        curHDist = hypnot(curPos.x, curPos.y);

        Point shiftedPowers = shift(new Point(xPower, yPower), imu.getAngle());
        setDrivePower(shiftedPowers.y, shiftedPowers.x, pid.update(imu.getAngle() - targetAngle), 0.3);

        // Log some data out for debugging
        multTelemetry.addData("curPos", "(" + curPos.x + ", " + curPos.y + ")");
        multTelemetry.addData("curHDist", curHDist);
        multTelemetry.update();
    }
    setAllPower(0);
}

Let's also update our getPosition() method with our unShift() function:

public Point getPosition(){
   double yDist = (fr.getCurrentPosition() + fl.getCurrentPosition() + br.getCurrentPosition() + bl.getCurrentPosition()) / 4.0;
   double xDist = (fl.getCurrentPosition() - fr.getCurrentPosition() + br.getCurrentPosition() - bl.getCurrentPosition()) / 4.0;
   Point unShiftedDistances = unShift(new Point(xDist, yDist), imu.getAngle());
   return unShiftedDistances;
}