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

2021-22 Tipping Point Vex Robotics Competition Code for Team 76513B

License

Notifications You must be signed in to change notification settings

vex-76513/code

Folders and files

NameName
Last commit message
Last commit date

Latest commit

 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 

Repository files navigation

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:

    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
    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 [email protected]

About

2021-22 Tipping Point Vex Robotics Competition Code for Team 76513B

Resources

License

Stars

Watchers

Forks

Releases

No releases published

Packages

No packages published