WEBVTT 00:00:00.000 --> 00:00:04.612 You already know how to read values from the controller. 00:00:04.612 --> 00:00:08.301 Let's use them to drive a mecanum robot. 00:00:08.301 --> 00:00:17.063 We have three things the driver can ask for: move forward or backward, move left or right, and turn. 00:00:17.063 --> 00:00:23.519 Our job is to combine those three inputs into one command for each wheel. 00:00:23.519 --> 00:00:31.820 For this lesson, positive y means forward, positive x means strafe right, and positive rx means turn clockwise. 00:00:31.820 --> 00:00:37.815 You will notice that y has a negative sign in the Java code. 00:00:37.815 --> 00:00:44.732 That is because pushing an FTC joystick forward normally gives us a negative Y value. 00:00:44.732 --> 00:00:53.494 We flip it once when we read the controller so that positive means forward everywhere else in our code. 00:00:53.494 --> 00:01:03.179 We are also going to keep the wheels in the same order for the entire video: front-left, front-right, back-left, and back-right. 00:01:03.179 --> 00:01:08.252 Keeping that order consistent makes the patterns much easier to see. 00:01:08.252 --> 00:01:15.630 Before we look at the code, it helps to see what the wheels are actually doing. 00:01:15.630 --> 00:01:20.703 A mecanum wheel has rollers mounted at about a 45-degree angle. 00:01:20.703 --> 00:01:29.926 Because of that angle, a spinning wheel pushes the robot both forward or backward and sideways at the same time. 00:01:29.926 --> 00:01:35.921 One wheel by itself would send the robot in a weird diagonal direction. 00:01:35.921 --> 00:01:44.222 With four wheels, we can make the parts we want add together while the other parts cancel out. 00:01:44.222 --> 00:01:51.601 On this goBILDA chart, the large arrow in the center shows which way the robot moves. 00:01:51.601 --> 00:01:56.673 The smaller arrows beside the wheels show how the wheels rotate. 00:01:56.673 --> 00:02:02.668 The green and blue arrows are used to distinguish the two wheel slants. 00:02:02.668 --> 00:02:08.202 They do not automatically mean positive or negative in our Java code. 00:02:08.202 --> 00:02:10.969 Look at the forward example first. 00:02:10.969 --> 00:02:18.348 All four wheels help push the robot forward, while their sideways pushes cancel each other out. 00:02:18.348 --> 00:02:25.265 For a right strafe, the forward and backward pushes cancel instead, leaving the sideways motion. 00:02:25.265 --> 00:02:33.105 For a turn, the wheels on the two sides work against each other and rotate the chassis. 00:02:33.105 --> 00:02:38.178 Now we can turn those movements into three simple sign patterns. 00:02:38.178 --> 00:02:40.483 Forward is the easiest one. 00:02:40.483 --> 00:02:44.173 All four wheels get the same positive command. 00:02:44.173 --> 00:02:48.784 For a right strafe, the signs alternate across the diagonals. 00:02:48.784 --> 00:02:51.090 Front-left and back-right are positive. 00:02:51.090 --> 00:02:53.396 Front-right and back-left are negative. 00:02:53.396 --> 00:03:02.158 For a clockwise turn, both wheels on the left are positive and both wheels on the right are negative. 00:03:02.158 --> 00:03:06.769 The final mixer is just those three patterns added together. 00:03:06.769 --> 00:03:09.536 Forward is added to every wheel. 00:03:09.536 --> 00:03:14.609 Strafe is added to one diagonal and subtracted from the other. 00:03:14.609 --> 00:03:19.682 Turn is added on the left and subtracted on the right. 00:03:19.682 --> 00:03:23.832 These signs describe the commands produced by our mixer. 00:03:23.832 --> 00:03:32.594 The motor direction settings on the real robot still depend on how the motors, gears, and wheels are installed. 00:03:32.594 --> 00:03:34.900 Now switch to Android Studio. 00:03:34.900 --> 00:03:38.589 This example does not control any motors yet. 00:03:38.589 --> 00:03:45.968 It only reads the controller, calculates the four wheel commands, and displays the results through telemetry. 00:03:45.968 --> 00:03:52.424 That lets us check the math by itself before connecting it to the robot. 00:03:52.424 --> 00:03:59.342 At the top of the loop, we read our three controls: forward, strafe, and turn. 00:03:59.342 --> 00:04:07.642 Forward has a negative sign because pushing an FTC joystick forward normally gives us a negative Y value. 00:04:07.642 --> 00:04:13.176 Next, the deadband treats any value very close to zero as zero. 00:04:13.176 --> 00:04:17.788 That keeps tiny joystick drift from becoming a movement command. 00:04:17.788 --> 00:04:21.938 Then we calculate a raw command for each wheel. 00:04:21.938 --> 00:04:24.705 Front-left adds forward, strafe, and turn. 00:04:24.705 --> 00:04:27.011 Front-right subtracts strafe and turn. 00:04:27.011 --> 00:04:29.778 Back-left subtracts strafe but adds turn. 00:04:29.778 --> 00:04:32.545 Back-right adds strafe and subtracts turn. 00:04:32.545 --> 00:04:36.234 Do not try to memorize four separate equations. 00:04:36.234 --> 00:04:38.503 Look for the pattern. 00:04:38.503 --> 00:04:42.193 Forward appears the same way in every equation. 00:04:42.193 --> 00:04:44.959 Strafe changes across the two diagonals. 00:04:44.959 --> 00:04:48.649 Turn changes between the left and right sides. 00:04:48.649 --> 00:04:56.488 These raw commands can sometimes be larger than one, so we calculate a denominator before using them. 00:04:56.488 --> 00:05:03.867 If the combined inputs are already within range, the denominator stays at one and nothing changes. 00:05:03.867 --> 00:05:10.323 If they are too large, we divide every wheel command by the same number. 00:05:10.323 --> 00:05:15.396 The important part is that all four commands are scaled together. 00:05:15.396 --> 00:05:23.697 That keeps the requested movement direction intact instead of changing only the wheels that went over the limit. 00:05:23.697 --> 00:05:31.537 Finally, telemetry shows the three inputs, the four raw commands, the denominator, and the four normalized commands. 00:05:31.537 --> 00:05:42.604 Move one joystick axis at a time and compare those numbers with the sign table before these outputs are ever connected to real motors. 00:05:42.604 --> 00:05:46.755 Here is one useful result of combining the inputs. 00:05:46.755 --> 00:05:49.983 Set forward and right strafe to 0. 00:05:49.983 --> 00:05:52.252 5, with no turn. 00:05:52.252 --> 00:05:54.558 Front-left and back-right become 1. 00:05:54.558 --> 00:05:57.786 0 because the two inputs add together. 00:05:57.786 --> 00:06:03.320 Front-right and back-left become zero because one input subtracts from the other. 00:06:03.320 --> 00:06:05.625 That gives us diagonal movement. 00:06:05.625 --> 00:06:13.004 We do not need a special diagonal mode or a separate if statement for every direction. 00:06:13.004 --> 00:06:17.154 The same mixer handles every angle of the joystick. 00:06:17.154 --> 00:06:21.305 Now let's look at a case that needs normalization. 00:06:21.305 --> 00:06:23.574 Forward is 0. 00:06:23.574 --> 00:06:25.880 8, right strafe is 0. 00:06:25.880 --> 00:06:28.185 6, and turn is zero. 00:06:28.185 --> 00:06:30.952 The raw wheel commands are 1. 00:06:30.952 --> 00:06:33.221 4, 0. 00:06:33.221 --> 00:06:35.490 2, 0. 00:06:35.490 --> 00:06:37.759 2, and 1. 00:06:37.759 --> 00:06:40.028 4. 00:06:40.028 --> 00:06:47.868 Motor power has to stay between negative one and positive one, so the two values at 1. 00:06:47.868 --> 00:06:50.137 4 are too large. 00:06:50.137 --> 00:06:58.437 If we clipped only those two values down to one, we would change the balance between the wheels. 00:06:58.437 --> 00:07:02.127 Instead, we divide all four values by 1. 00:07:02.127 --> 00:07:04.396 4. 00:07:04.396 --> 00:07:06.701 The final commands become 1. 00:07:06.701 --> 00:07:08.970 0, about 0. 00:07:08.970 --> 00:07:11.239 14, about 0. 00:07:11.239 --> 00:07:13.508 14, and 1. 00:07:13.508 --> 00:07:15.777 0. 00:07:15.777 --> 00:07:23.155 Now everything is in range, and we have kept the same relationship between the four wheels. 00:07:23.155 --> 00:07:29.151 You may see examples that multiply the strafe input by something like 1. 00:07:29.151 --> 00:07:31.419 1. 00:07:31.419 --> 00:07:38.798 That is a tuning choice used to compensate for imperfect sideways movement on a real drivetrain. 00:07:38.798 --> 00:07:41.104 Leave it out at first. 00:07:41.104 --> 00:07:48.944 Test the actual robot, and only add a correction if your results show that you need one. 00:07:48.944 --> 00:07:59.550 At this point, the math can look correct on the screen, but that does not prove the physical robot is set up correctly. 00:07:59.550 --> 00:08:06.468 When the team connects these outputs to the real motors, start with the controls released. 00:08:06.468 --> 00:08:09.235 All four commands should be zero. 00:08:09.235 --> 00:08:19.380 Then test forward, strafe, and turn one at a time at low power, with the robot safely supported and a mentor present. 00:08:19.380 --> 00:08:25.375 If the telemetry numbers are wrong, check the controller input or the mixer. 00:08:25.375 --> 00:08:33.215 If the telemetry is correct but the wrong wheel moves, check the hardware map, ports, and wiring. 00:08:33.215 --> 00:08:39.671 If the correct wheel moves in the wrong direction, check that motor's direction setting. 00:08:39.671 --> 00:08:42.438 Work through those layers in order. 00:08:42.438 --> 00:08:51.200 Find the first place where the result stops matching your prediction, fix that one problem, and test it again.