190 lesson: ask the wheels for a speed

../_images/fig-l190-wheel-numbers.png

Full stick means 40 inches per second, not full power. The drivetrain measures how fast each wheel turns and makes up the difference, on a fresh battery or a flat one, on the flat or up a slope.

the steps

L190S010: copy the two starters into mytry

This lesson has two files, as L110 did. One is the OpMode. The other is the drivetrain, which turns a speed into a power.

Copy. In lessons, click L190SpeedDriveTrain once and copy it. Click mytry once and paste. In the Copy Class box, click OK. If Android Studio asks whether to add the file to Git, click Add. Do the same for L190VelocityDriveOpMode.

Check.

  • mytry holds L190SpeedDriveTrain and L190VelocityDriveOpMode.

L190S020: switch your copy on

Change. In L190VelocityDriveOpMode, delete the line @Disabled, under the comment that names this step.

Check.

  • Under the step’s comment, the next line reads // 'public' so the robot can run this OpMode. 'public' allows access from any code.

L190S030: make room for the drivetrain

Change. Near the top of the class, find the comment that names this step. Under it, type:

    // When on L190S030, see
    // https://msb-stitt.github.io/ftc31459-26-27-biobuzz/guide/tasks/l190.html#l190s030
    // for what to do here.
    L190SpeedDriveTrain drivetrain;

Check.

  • The line has no red underline.

  • L190SpeedDriveTrain is in mytry too, so it needs no import.

L190S040: hand the drivetrain to the follower

Change. In init(), find the comment that names this step. Under it, type the first line below. Then change initAfter(); to initAfter(drivetrain);:

        // When on L190S040, see
        // https://msb-stitt.github.io/ftc31459-26-27-biobuzz/guide/tasks/l190.html#l190s040
        // for what to do here.
        drivetrain = new L190SpeedDriveTrain(hardware);

        initAfter(drivetrain);

This is L110S050 again, with the new drivetrain.

Check.

  • Neither line has a red underline.

L190S050: turn the sticks into speeds

../_images/fig-l190-asks-standing-still.png

MAX_IPS is 40, in inches per second. MAX_TURN_RADPS is pi radians per second, half a turn each second. Both are near the top of the class.

Change. In driveFromTheSticks(), find the comment that names this step. Under it are three lines that end in = 0;. Change each 0 as below. Then, under the three publish lines, type the last two lines:

        // When on L190S050, see
        // https://msb-stitt.github.io/ftc31459-26-27-biobuzz/guide/tasks/l190.html#l190s050
        // for what to do here.
        double forwardInPerS = deadband(-leftStickY, band) * MAX_IPS;
        double strafeInPerS = deadband(-leftStickX, band) * MAX_IPS;
        double turnRadPerS = deadband(-rightStickX, band) * MAX_TURN_RADPS;
        Tracker.publish("command/forward_ips", forwardInPerS);
        Tracker.publish("command/left_ips", strafeInPerS);
        Tracker.publish("command/turn_radps", turnRadPerS);
        double[] target = WheelTargets.forMecanum(forwardInPerS, strafeInPerS, turnRadPerS, Constants.turnRadiusInches);
        drivetrain.setCommandedWheelSpeeds(target[0], target[1], target[2], target[3]);

The minus signs are the ones from L030. A stick pushed up reads less than 0.0.

WheelTargets.forMecanum works out how fast each wheel has to turn for the robot to move like that. It hands back an array, front left first, then front right, back left and back right. turnRadiusInches is how far each wheel sits from the middle of the robot.

WheelTargets and Constants turn red. Hold the mouse over each one until a box offers Import class, and click it. The box for Constants names org.firstinspires.ftc.teamcode.pedro.Constants.

Simulator. In the Terminal, run:

./gradlew :TeamCode:simRun --args="mytry.L190VelocityDriveOpMode --left_stick_y=-0.5"

AdvantageScope. Connect to the simulator as in L020, click 2D Field, and open the command folder. Then open Localizer and pinPoint.

Check.

  • forward_ips reads 20, half of 40.

  • The robot does not move, and y_in stays at 10.5.

The OpMode asks for a speed, but setCommandedWheelSpeeds in your drivetrain is still empty.

L190S060: turn a speed into a power

../_images/fig-speed-loop.png

Open L190SpeedDriveTrain. It extends CorbelsMecanum, so it has everything L180’s drivetrain had. setCommandedWheelSpeeds puts the four speeds into the array wanted. Each wheel gets its power from two parts added together.

  • Feedforward is a guess, made before anything is measured. Constants.powerPerInchPerSecond is the power one inch per second needs, so the guess gets most of the way there at once.

  • Feedback is the correction. measuredSpeeds.all() reads how fast each wheel really turns. The difference between wanted and measured, times VELOCITY_KP, is added on.

The guess is always a little off, and the correction only grows once the wheel is off its speed. Together they get there and stay there.

A for loop runs the lines inside its braces once for each wheel. i starts at 0 and goes up by 1 each time, while it is less than 4. So wanted[i] is wanted[FL] the first time around, then FR, BL and BR.

Change. Press Ctrl+C in the Terminal to stop the simulator. In setCommandedWheelSpeeds, find the comment that names this step. Under it, type:

        // When on L190S060, see
        // https://msb-stitt.github.io/ftc31459-26-27-biobuzz/guide/tasks/l190.html#l190s060
        // for what to do here.
        double[] measured = measuredSpeeds.all();
        double[] powers = new double[4];
        for (int i = 0; i < 4; i++) {
            double feedforward = Constants.powerPerInchPerSecond * wanted[i];
            double feedback = VELOCITY_KP * (wanted[i] - measured[i]);
            powers[i] = clampToPower(feedforward + feedback);
        }
        setCommandedWheels(powers[FL], powers[FR], powers[BL], powers[BR]);
        publishWheelSpeeds(wanted, measured);

Android Studio types the } of the for loop for you, when you press Enter after the {.

clampToPower keeps each power between -1.0 and +1.0. setCommandedWheels sends the four powers to the wheels, as in L110. publishWheelSpeeds puts each wheel’s numbers in the wheel folder.

Constants turns red. Import it as in L190S050.

Simulator. Run the command again, with the stick pushed less so the robot stays on the field longer:

./gradlew :TeamCode:simRun --args="mytry.L190VelocityDriveOpMode --left_stick_y=-0.2"

AdvantageScope. Connect, and open wheel, then frontLeft.

Check.

  • forward_ips and target_ips read 8.

  • The robot drives up the field.

  • On the first loop power reads 0.198: the guess, 0.134, plus the correction for a wheel not yet turning.

actual_ips climbs to about 8 in under half a second and settles a little above 8, so error_ips settles a little below 0.0. power drops to about 0.13 and stays there. The guess was a little too much for the simulated robot, and the correction takes off what it needs.

L190S070: drive it up something

Robot. Stop the simulator. Send the code, run L190 Velocity Drive from the Driver Station, and watch it in AdvantageScope.

Check.

  • Drive on the flat first.

  • Then drive straight up the mat, with the stick held in the same place. The robot slows a little at the foot of the slope and then picks back up.

  • In AdvantageScope, actual_ips dips below target_ips, then climbs back, and power rises to do it.