Skip to content
This repository has been archived by the owner on Mar 22, 2023. It is now read-only.

Latest commit

 

History

History
36 lines (24 loc) · 2.17 KB

README.md

File metadata and controls

36 lines (24 loc) · 2.17 KB
Raw

While this code is nothing special in terms of robotics innovation, being the first major robotics codebase I have led, I'm happy with it.

Here are some of the cool features:

  1. Controller rumbles at the sg3 time (last 30sec early season, last 15 sec at world's):

    code/src/main.cpp

    Lines 150 to 185 in f981a61

    void sg3_warn()
    {
    // competition_get_status returns ENABLED,AUTONOMOUS,CONNECTED bits
    // no its CONNECTED, AUTO, DISABLE so 0b100
    // the xor (^) function negates bits, i.e. 1 ^ 1 = 0
    // Here, 0b101, ENABLED,NO_AUTON,CONNECTED is my wanted state
    // when it reaches the wanted state (i.e.) status returned is 0b101, the answer is 0
    // until the answer is 0 (i.e. while ans != 0), it loops and waits
    while ((pros::c::competition_get_status() ^ (0b1100)) != 0)
    {
    pros::delay(1000);
    }
    // printf("SG# FINISH DELAYING\n");
    // after it finishes looping, i.e. when driver control starts
    // we need to wait for 1:45 - 0:25 = 1:25 and then buzz at 40s
    pros::delay((1_min + 20_s).convert(1_ms));
    // first rumble partner to pre-warn, then rumble main controller, then re-rumble partner to remind
    int retries = 0;
    do
    {
    errno = 0;
    pros::c::controller_rumble(pros::E_CONTROLLER_MASTER, ". -- .");
    // printf("SG# err %d\n", errno);
    pros::delay(100);
    retries++;
    } while (errno != 0 && retries < 9);
    do
    {
    errno = 0;
    pros::c::controller_rumble(pros::E_CONTROLLER_PARTNER, "- --");
    pros::delay(100);
    retries++;
    } while (errno != 0 && retries < 9);
    }

  2. Auto MOGO grab using Vision and Ultrasonic:

    code/src/autons/vision.hpp

    Lines 5 to 50 in f981a61

    void front_line_up(okapi::QTime timeout = 4_s, Vision::MOGO type = Vision::ANY, bool otherline = false)
    {
    auto distancecontroller = okapi::IterativePosPIDController({0.0003, 0.00019, 0.000005}, okapi::TimeUtilFactory().withSettledUtilParams(15, 9999, 150_ms));
    auto anglecontroller = okapi::IterativePosPIDController({0.00020 * 2, 0, 0.000001}, okapi::TimeUtilFactory().withSettledUtilParams(15, 5, 150_ms));
    auto [_, _1, _2, _3, w] = Visions[Vision::FRONT]->get_mogo(type);
    anglecontroller.setTarget(158);
    distancecontroller.setTarget(30);
    int count = 0;
    do
    {
    count++;
    errno = 0;
    auto [_, top, x_mid, y_mid, _1] = Visions[Vision::FRONT]->get_mogo(w);
    auto geterr = errno;
    auto distvel = -distancecontroller.step(ultrasonic.get());
    auto yawvel = -1 * anglecontroller.step(x_mid);
    // yawvel = 0;
    if (geterr != 0)
    {
    yawvel = 0;
    // distvel = 0;
    }
    if (otherline)
    {
    if (drive.chassis->getState().y > 4_tile - 6_in)
    {
    drive.chassis->getModel()->driveVectorVoltage(-12, 0);
    return;
    }
    }
    drive.chassis->getModel()->driveVectorVoltage(12.0 * distvel, 12.0 * yawvel);
    pros::delay(20);
    // printf("LOP: %f %d %f\n", anglecontroller.getProcessValue(), x_mid, yawvel);
    // printf("LOP: %f %d %f\n", distancecontroller.getProcessValue(), ultrasonic.get(), distvel);
    // printf("%d %d\n", anglecontroller.isSettled(), distancecontroller.isSettled());
    } while (!((anglecontroller.isSettled() && distancecontroller.isSettled()) || count > timeout.convert(1_ms) / 20.0));
    anglecontroller.flipDisable();
    distancecontroller.flipDisable();
    }
    (I would NOT reuse this code as is, it's a mess and could be architected more modularly)

  3. Monitor when the PID motion is stuck on a wall or MOGO, and move on:

    code/src/autons/skills.hpp

    Lines 88 to 107 in f981a61

    void monitorStuckage()
    {
    printf("STUCKAGE\n");
    int count = 0;
    while (!drive.chassis->isSettled())
    {
    if (drive.getForce() > 35_n)
    count++;
    else if (count > 0)
    count -= 1;
    // 1 sec of stuck
    if (count > 10)
    drive.chassis->stop();
    // stop will cause return on next loop
    printf("STUCKCOUNT %d\n", count);
    pros::delay(100);
    }
    }
    and

    code/src/subsystems/drivetrain.hpp

    Lines 55 to 63 in f981a61

    okapi::QForce getForce()
    {
    double sum = 0;
    for (auto i : HARDWARE::drive_motors_left)
    sum += i.getTorque();
    for (auto i : HARDWARE::drive_motors_right)
    sum += i.getTorque();
    return sum * HARDWARE::drive_gearset.ratio * okapi::newton * okapi::meter / HARDWARE::drive_chassis_scale.middleWheelDiameter;
    }
    (Again, don't use this, it could have a much better API)

  4. My basic neutral goal grab autonomous that FINALLY worked reliably at World's:

    code/src/autonomous.hpp

    Lines 6 to 28 in f981a61

    void basic_goal_grabe(bool mode) // mode isn't read
    {
    drive.chassis->setState(okapi::OdomState{x : 0_tile, y : 1_tile - 1_in});
    claw.Leave();
    drive.setMaxVelocity(600);
    // drive.chassis->model().tank(.8, .8);
    drive.chassis->model().left(1);
    drive.chassis->model().right(1);
    while (drive.chassis->getState().y < 3_tile - 6_in - 14_in)
    pros::delay(10);
    drive.chassis->model().left(.3);
    drive.chassis->model().right(.3);
    while (drive.chassis->getState().y < 3_tile - 6_in)
    pros::delay(10);
    drive.chassis->model().tank(0, 0);
    claw.Clasp();
    skillsn::moveDistance(-2_tile);
    }
    (moral of the story don't use sensors when you only need to go straight, build a better drivetrain)

HUGE THANKS to:

  1. OkapiLib folks (for literally all the complex drive, PID, odometry this does)
  2. PROS makers (for the framework)
  3. EZ-Template (for the auton-selector and ideas about stuck monitoring)
  4. PROS-Grafana-CLI (while I didn't use this too much here, I'm really excited about its potential for spin up)
  5. Belton High School for funding this, and Mr Giustino for teaching us and enabling us to do this
  6. Finally, Vex and RECF for making this even exist

To see code history: https://github.com/vex-76513/code/commits/pros

This requires a custom version of OkapiLib at ../OkapiLib from https://github.com/vex-76513/vex-76513-OkapiLib/commit/3c5879de0cd2fa5e4e163e399a040e8aa5de1798 And EZ-Template at ../EZ-Template from https://github.com/EZ-Robotics/EZ-Template/commit/aeaf1785d5a580da01bc50094d1646d7a82383dc

To init environment:

        pros c apply --install --download --force --force-apply kernel@3.5.4