HighStakes/src/autons.cpp
#include "autons.h"
#include "lemlib/chassis/chassis.hpp"
#include "pros/rtos.hpp"
#include "robot.h"
#include <vector>
using namespace robot::drive;
using namespace robot::intake;
void autonDemo() {
pros::delay(10000);
chassis.setPose(0, 0, 0); // Setting middle of the field as 0,0
chassis.moveToPoint(0, -15, 5000,
{.forwards = false,
.maxSpeed = 70,
.minSpeed = 10,
.earlyExitRange = 0.5},
false); // Have a good position for clamp
clamp.set_value(true); // clamp after a good position is secured
// intake.move(127); //score preload
}
std::vector<AutonPrograms> autonList = {
{
"Demo auton",
autonDemo,
"red",
},
};
void interactionCode() {
// TODO: Implement interaction code here
}HighStakes/src/main.cpp
#include "main.h" // "../include/main.h"
#include "autons.h"
#include "pros/misc.h"
#include "robot.h"
#include <vector>
using namespace robot;
using namespace robot::tasks;
void initialize() {
pros::Task startIntake(intake::intakeTask);
pros::Task checkRejection(intake::rejectTask);
pros::Task startWallStakeMech(wsm::wallStakeTask);
drive::chassis.calibrate();
}
void competition_initialize() {
// needs to be put back in
safety::initializeDrift();
if (safety::deviceCheck()) {
drive::master.rumble("........");
}
selector::autonSelection();
}
// needed jerry files
// ASSET(top_left_rings_txt);
// ASSET(bottom_right_corner_rings_txt);
// ASSET(move_to_bottom_left_txt);
// ASSET(second_goal_grab_txt);
// ASSET(bottom_left_corner_rings_txt);
// ASSET(second_goal_in_corner_txt);
// ASSET(between_second_and_third_goal_txt);
// ASSET(ring_between_second_third_goal_txt);
using namespace std;
bool isIsolation = true;
void autonomous() {
if (isIsolation) {
autonList[selector::autonNum].autonFunction();
} else {
interactionCode(); // based on team discussions, we are assuming that
// the isolation code will always leave us in the same state
// (protecting either corner)
}
isIsolation = false;
}
void opcontrol() {
using namespace pros;
using namespace robot::drive;
chassis.calibrate();
chassis.setPose({0, 0, 0});
auto printPose = [&]() {
auto pose = chassis.getPose();
cout << pose.x << " " << pose.y << " " << pose.theta << endl;
};
while (true) {
// imu good
// both wheels good?
printPose();
pros::delay(100);
if (master.get_digital_new_press(pros::E_CONTROLLER_DIGITAL_A)) {
chassis.setPose({0, 0, 0});
chassis.turnToHeading(90.0, 500);
}
}
}HighStakes/src/robot.cpp
#include "autons.h"
#include "lemlib/chassis/chassis.hpp"
#include "liblvgl/llemu.hpp"
#include "pros/adi.hpp"
#include "pros/distance.hpp"
#include "pros/llemu.hpp"
#include "pros/optical.hpp"
#include "pros/vision.hpp"
#include <cmath>
#include <numeric>
#define OLIVER_BOT true
namespace robot { // REWRITTEN FOR AI
enum RingColor { RED, BLUE, NONE };
}
namespace robot::hardware { // CHECKED FOR AI
pros::Controller master(pros::E_CONTROLLER_MASTER);
#if OLIVER_BOT
// DONE
pros::MotorGroup leftMotorGroup({-14, -15, 16}, pros::MotorGearset::blue);
pros::MotorGroup rightMotorGroup({13, 11, -12}, pros::MotorGearset::blue);
pros::Motor intakeMotor(19);
pros::Motor intakeBottom(19);
pros::Motor wsmMotor(19, pros::MotorGearset::green);
pros::adi::DigitalOut clamp('a');
pros::adi::DigitalOut flipArm('a');
pros::adi::DigitalOut ringFlipArm('a');
pros::adi::DigitalOut intakeLifter('a');
pros::adi::DigitalOut ringSort('a');
// DONE
pros::Imu imu(2);
pros::Rotation verticalSensor(18);
pros::Rotation horizontalSensor(17);
pros::Rotation wallStakeSensor(19);
pros::Optical optical(19);
pros::Vision vision(19);
pros::Distance distance(19);
pros::adi::DigitalIn rejectionLimit('a');
// horizontal tracking wheel
lemlib::TrackingWheel horizontalTracker(&horizontalSensor,
lemlib::Omniwheel::NEW_2,
1); // OG: 1.75 -1.3125
// vertical tracking wheel
lemlib::TrackingWheel verticalTracker(&verticalSensor,
lemlib::Omniwheel::NEW_2,
1); // OG: 1.6875 //2.25
#else
pros::MotorGroup leftMotorGroup({1, -5, -6}, pros::MotorGearset::blue);
pros::MotorGroup rightMotorGroup({-17, 10, 9}, pros::MotorGearset::blue);
pros::Motor intakeMotor(3);
pros::Motor intakeBottom(20);
pros::Motor wsmMotor(14, pros::MotorGearset::green);
pros::adi::DigitalOut clamp('a');
pros::adi::DigitalOut flipArm('g');
pros::adi::DigitalOut ringFlipArm('f');
pros::adi::DigitalOut intakeLifter('c');
pros::adi::DigitalOut ringSort('d');
pros::Imu imu(13);
pros::Rotation verticalSensor(15);
pros::Rotation horizontalSensor(11);
pros::Rotation wallStakeSensor(2);
pros::Optical optical(16);
pros::Vision vision(12);
pros::Distance distance(19);
pros::adi::DigitalIn rejectionLimit('b');
// horizontal tracking wheel
lemlib::TrackingWheel horizontalTracker(&horizontalSensor,
lemlib::Omniwheel::NEW_2_315A,
-1.875); // OG: 1.75 -1.3125
// vertical tracking wheel
lemlib::TrackingWheel verticalTracker(&verticalSensor,
lemlib::Omniwheel::NEW_2_315A,
1.75); // OG: 1.6875 //2.25
#endif
} // namespace robot::hardware
namespace robot::drive {
using namespace hardware;
// drivetrain settings
lemlib::Drivetrain
drivetrain(&leftMotorGroup, // left motor group
&rightMotorGroup, // right motor group
12.75, // 10 inch track width
lemlib::Omniwheel::NEW_275, // using new 4" omnis
450, // drivetrain rpm is 360
2 // horizontal drift is 2 (for now)
);
lemlib::ControllerSettings lateralController(
28, // proportional gain (kP) /28
1, // integral gain (kI) //1
120, // derivative gain (kD) //180
1, // anti windup //1
0.25, // small error range, in inches // 0.25
250, // small error range timeout, in milliseconds //300
3, // large error range, in inches //3
1000, // large error range timeout, in milliseconds //1000
0 // maximum acceleration (slew)
);
lemlib::ControllerSettings angularController(
5.9, // proportional gain (kP) //5.9
1, // integral gain (kI) //1
45, // derivative gain (kD)//45
1.5, // anti windups //1.5
0.2, // small error range, in degrees //2
250, // small error range timeout, in milliseconds //100
5, // large error range, in degrees //5
1000, // large error range timeout, in milliseconds //500
0 // maximum acceleration (slew)
);
// arm movement PID controller
lemlib::ControllerSettings
armController(0.04, // proportional gain (kP) //5
0.00, //.05, // integral gain (kI) //0.001
0.3, // derivative gain (kD) //0.4
300, // anti windup
0, // small error range
0, // small error range timeout in milliseconds
0, // large error range
0, // large error range timeout in milliseconds
0 // maximum acceleration (slew)
);
// odometry settings
lemlib::OdomSensors sensors(
&verticalTracker, // vertical tracking wheel 1, set to null
nullptr, // vertical tracking wheel 2, set to nullptr as not using it
&horizontalTracker, // horizontal tracking wheel 1
nullptr, // horizontal tracking wheel 2, set to nullptr as we don't have
// a second one
&imu // inertial sensor
);
// input curve for throttle input during driver control
lemlib::ExpoDriveCurve throttleCurve(
3, // joystick deadband out of 127
10, // minimum output where drivetrain will move out of 127
1.019 // expo curve gain
);
// input curve for steer input during driver control
lemlib::ExpoDriveCurve steerCurve(
3, // joystick deadband out of 127
30, // minimum output where drivetrain will move out of 127
1.5 // expo curve gain, OG 1.019
// smaller value = more sensitive steering (see desmos graph in
// chat) the closer it is to one, the more linear the gain is
);
lemlib::Chassis chassis(drivetrain, lateralController, angularController,
sensors, &throttleCurve, &steerCurve);
} // namespace robot::drive
namespace robot::wsm { // REWRITTEN FOR AI
using namespace robot::hardware;
using namespace robot::drive;
// TODO: tune these values
const int WALL_STAKE_DOWN = 150 * 100;
const int WALL_STAKE_ABOVE_LOADING = 200 * 100;
const int WALL_STAKE_LOADING = 180 * 100;
const int WALL_STAKE_NEUTRAL = 30 * 1000;
const int WALL_STAKE_ALLIANCE = 360 * 100;
double wallStakeTarget = 0;
double prevWallStakeTarget = 0;
bool errorAcceptable = false;
lemlib::PID armPid(armController.kP, armController.kI, armController.kD,
armController.windupRange, false);
void blockUntilWsmDone() {
while (!errorAcceptable) {
pros::delay(10);
}
}
void wallStakeTask() {
armPid.reset();
while (true) {
if (wallStakeTarget != prevWallStakeTarget) {
armPid.reset();
prevWallStakeTarget = wallStakeTarget;
}
double error =
(wallStakeSensor.get_position() % 36000) - wallStakeTarget;
wsmMotor.move(fabs(error) > 4000 ? armPid.update(error) : 0);
pros::delay(10);
}
}
} // namespace robot::wsm
namespace robot::intake { // REWRITTEN FOR AI
using namespace robot::wsm;
using namespace robot::hardware;
// See the GChat message for details of the task's state machine
// TODO: Implement jam handling, once we know common jam locations
//
// 1. The task needs to know where the code wants the ring to end up.
enum RingPosition { SCORED, WSM, HOLD };
struct IntakeTaskMessage {
// 2. The task needs to know what color the ring should be.
RingColor target;
RingPosition position;
};
// To tell the intake to run a task, set currentIntakeTask.
// To block until the task is complete, call intakeTaskMutex.lock() and
// then immediately call intakeTaskMutex.give()
pros::Mutex intakeTaskMutex;
std::optional<IntakeTaskMessage> currentIntakeTask;
RingColor heldRing = NONE;
bool intakeBreak = false; // if it is on a break or not
bool rejectNotification = false;
pros::Mutex rejectNotificationMutex;
void blockUntilIntakeDone() {
intakeTaskMutex.lock();
intakeTaskMutex.give();
}
void blockUntilRejectDone() {
rejectNotificationMutex.lock();
rejectNotificationMutex.give();
}
void intakeTask() {
while (true) {
pros::delay(10);
if (!intakeBreak && (heldRing == NONE))
intakeMotor.move(127);
else
intakeMotor.move(0);
if (!currentIntakeTask.has_value() || intakeBreak)
continue;
IntakeTaskMessage message = currentIntakeTask.value();
intakeTaskMutex.take(); // we can safely assume, since we
// are the only ones accessing it, that the mutex is not locked
auto checkCurrentRing = [&]() -> bool {
return (heldRing == message.target || message.target == NONE);
};
// either heldRing exists and the intake is stopped
// or there is no heldRing and the intake is moving
intakeMotor.move(127);
while (!checkCurrentRing()) { // this will run until the correct
// ring is finally held
if (heldRing == NONE) {
// get a new ring lol
while (distance.get_distance() > 60) {
pros::delay(10);
}
pros::delay(
100); // delay for time to let ring be seen by optical
// the following taken from the old code but heavily
// modified
double hue = optical.get_hue();
int redCount = 0;
int blueCount = 0;
int loop_check = 0;
while (distance.get_distance() < 60) {
pros::delay(5);
if (loop_check > 250)
break;
hue = optical.get_hue();
if ((hue >= 0 && hue <= 60) ||
(hue >= 320 && hue <= 360))
redCount++;
else if ((hue >= 160) && (hue <= 240))
blueCount++;
loop_check++;
}
// update the held ring
heldRing = redCount >= blueCount ? RED : BLUE;
} else {
// either the above has run or there is already a held ring!
if (heldRing == message.target) {
break;
} else {
blockUntilRejectDone();
rejectNotification = true;
pros::delay(
200); // wait for it to get out of the sensor sight
}
}
}
// we have the ring, now get it to its correct position
if (message.position == HOLD) {
intakeMotor.move(0);
} else if (message.position == SCORED) {
wallStakeTarget = WALL_STAKE_ABOVE_LOADING;
intakeMotor.move(127);
heldRing = NONE;
} else if (message.position == WSM) {
wallStakeTarget = WALL_STAKE_LOADING;
intakeMotor.move(0);
blockUntilWsmDone();
intakeMotor.move(127);
heldRing = NONE;
pros::delay(300);
intakeMotor.move(0);
wallStakeTarget = WALL_STAKE_ABOVE_LOADING;
intakeMotor.move(127);
}
intakeTaskMutex.give();
currentIntakeTask.reset();
}
}
void rejectTask() {
while (true) {
pros::delay(10);
if (!rejectNotification || intakeBreak)
continue;
// Perform color sorting logic here
rejectNotificationMutex.lock();
//
while (rejectionLimit.get_value() == 0) {
pros::delay(10);
}
ringSort.set_value(true);
pros::delay(500);
ringSort.set_value(false);
rejectNotification = false;
rejectNotificationMutex.unlock();
}
}
} // namespace robot::intake
namespace robot::tasks {}
namespace robot::tasks::safety { // CHECKED FOR AI
using namespace robot::hardware;
bool checkDriftWithStdDev() {
std::vector<double> readings;
int numData = 250; // 2500 ms of data for accuracy of drift catching
readings.reserve(numData);
// Collect readings of imu
for (int i = 0; i < numData; i++) {
readings.push_back(imu.get_rotation());
pros::delay(10);
}
// Sort readings
std::sort(readings.begin(), readings.end());
// Remove bottom and top 10%
int cutoff = numData * 0.1;
std::vector<double> trimmedReadings(readings.begin() + (cutoff),
readings.end() - (cutoff));
// Calculate mean
double sum = std::accumulate(trimmedReadings.begin(),
trimmedReadings.end(), 0.0);
double mean = sum / trimmedReadings.size();
// Calculate standard deviation
double sq_sum = 0;
for (double reading : trimmedReadings) {
sq_sum += (reading - mean) * (reading - mean);
}
double std_dev = std::sqrt(sq_sum / (trimmedReadings.size() - 1));
// Check for drift, if std_dev is greater than 0.003 then there is drift
float driftLimit = 0.003;
bool hasDrift = false;
if (std_dev > driftLimit) {
hasDrift = true;
}
return hasDrift;
}
void initializeDrift() {
for (int i = 0; i < 5; i++) {
if ((i < 4) && (checkDriftWithStdDev() == true)) {
drive::chassis.calibrate();
printf("\nre-calibrating");
printf("\n");
} else if ((i == 4) && (checkDriftWithStdDev() == true)) {
printf("Restart Program");
return; // find a way to create stronger quit message
} else {
printf("Calibrated");
break;
}
pros::delay(10);
}
}
bool deviceCheck() { // returns true if failed
auto lports = leftMotorGroup.get_port_all();
auto rports = rightMotorGroup.get_port_all();
// Left Ports
for (const int8_t &port : lports) {
pros::v5::Device device(abs(port));
if (device.get_plugged_type() != pros::v5::DeviceType::motor) {
pros::lcd::clear_line(3);
if (abs(port) == 1) {
pros::lcd::print(
3, "Left Back Top Drive Motor is disconnected!: %d",
abs(port));
}
if (abs(port) == 5) {
pros::lcd::print(
3, "Left Front Drive Motor is disconnected!: %d",
abs(port));
}
if (abs(port) == 6) {
pros::lcd::print(
3, "Left Back Bottom Drive Motor is disconnected!: %d",
abs(port));
}
return true;
}
}
// Right Ports
for (const int8_t &port : rports) {
std::uint8_t pport = abs(port);
pros::v5::Device device(pport);
if (device.get_plugged_type() != pros::v5::DeviceType::motor) {
pros::lcd::clear_line(3);
if (abs(port) == 17) {
pros::lcd::print(
3, "Right Drive Motor is disconnected!: %d", abs(port));
}
if (abs(port) == 10) {
pros::lcd::print(
3, "Right Drive Motor is disconnected!: %d", abs(port));
}
if (abs(port) == 9) {
pros::lcd::print(
3, "Right Drive Motor is disconnected!: %d", abs(port));
}
return true;
}
}
pros::v5::Device &dev = imu;
if (!dev.is_installed()) {
pros::lcd::clear_line(3);
pros::lcd::print(3, "Imu is disconnected!: %d", imu.get_port());
return true;
}
if (intakeMotor.get_plugged_type() != pros::v5::DeviceType::motor) {
pros::lcd::clear_line(3);
pros::lcd::print(3, "Intake is disconnected!: %d",
intakeMotor.get_port());
return true;
}
if (intakeBottom.get_plugged_type() != pros::v5::DeviceType::motor) {
pros::lcd::clear_line(3);
pros::lcd::print(3, "Bottom Intake is disconnected!: %d",
intakeBottom.get_port());
return true;
}
if (wallStakeSensor.get_plugged_type() !=
pros::v5::DeviceType::rotation) {
pros::lcd::clear_line(3);
pros::lcd::print(3, "Wall Stake Sensor is disconnected!: %d",
wallStakeSensor.get_port());
return true;
}
if (wsmMotor.get_plugged_type() != pros::v5::DeviceType::motor) {
pros::lcd::clear_line(3);
pros::lcd::print(3, "Wallmech is disconnected!: %d",
wsmMotor.get_port());
return true;
}
if (verticalSensor.get_plugged_type() !=
pros::v5::DeviceType::rotation) {
pros::lcd::clear_line(3);
pros::lcd::print(3, "Rotation is disconnected!: %d",
verticalSensor.get_port());
return true;
}
if (horizontalSensor.get_plugged_type() !=
pros::v5::DeviceType::rotation) {
pros::lcd::clear_line(3);
pros::lcd::print(3, "Rotation is disconnected!: %d",
horizontalSensor.get_port());
return true;
}
if (optical.get_plugged_type() != pros::v5::DeviceType::optical) {
pros::lcd::clear_line(3);
pros::lcd::print(3, "Optical is disconnected!: %d",
optical.get_port());
return true;
}
if (distance.get_plugged_type() != pros::v5::DeviceType::distance) {
pros::lcd::clear_line(3);
pros::lcd::print(3, "Distance is disconnected!: %d",
distance.get_port());
return true;
}
return false;
pros::delay(100);
}
} // namespace robot::tasks::safety
namespace robot::tasks::selector {
std::string functionName = "";
int autonNum = 0;
std::string color = "red"; // The color alliance your robot is
void onRightButton() {
autonNum = (autonNum + 1) % autonList.size();
color = autonList[autonNum].allianceColor;
pros::delay(200);
}
void onLeftButton() {
autonNum = (autonNum + (autonList.size() - 1)) % autonList.size();
color = autonList[autonNum].allianceColor;
pros::delay(200);
}
void autonSelection() {
// Initialize lcd and clear controller screen
pros::lcd::initialize();
// master.clear();
// Give prompt
while (true) {
// functionName = autonList[autonNum].autonMode;
pros::lcd::clear_line(3);
pros::lcd::print(1, "%s", autonList[autonNum].autonMode);
pros::lcd::print(2, "%s", color);
pros::lcd::register_btn2_cb(onRightButton);
pros::lcd::register_btn0_cb(onLeftButton);
pros::delay(50);
}
} // namespace robot::tasks::selector
} // namespace robot::tasks::selector