Skip to content

Commit

Permalink
feat: tuned 3-ball auto w/ 2nd line tracker
Browse files Browse the repository at this point in the history
  • Loading branch information
@LouisAsanaka
LouisAsanaka committed Jan 12, 2021
1 parent cd41a8b commit 4f32e24
Show file tree
Hide file tree
Showing 7 changed files with 61 additions and 34 deletions.
4 changes: 2 additions & 2 deletions include/constants.hpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -27,7 +27,7 @@ constexpr double DRIVE_ENCODER_TARGET_ERROR = 40;
constexpr double DRIVE_ENCODER_TARGET_DERIV = 5;
constexpr okapi::QTime DRIVE_ENCODER_TARGET_TIME = 200_ms;

constexpr double STRAFING_DIST_TARGET_ERROR = 0.02;
constexpr double STRAFING_DIST_TARGET_ERROR = 0.03;
constexpr double STRAFING_DIST_TARGET_DERIV = 5;
constexpr okapi::QTime STRAFING_DIST_TARGET_TIME = 150_ms;

Expand Down Expand Up @@ -55,7 +55,7 @@ constexpr double STRAFE_DISTANCE_KP = 5;
constexpr double STRAFE_DISTANCE_KI = 0.0;
constexpr double STRAFE_DISTANCE_KD = 0.003;
constexpr planner::TrapezoidProfile::Constraints
STRAFE_DISTANCE_CONTSTRAINTS {1.0, 1.0}; // m/s, m/s^2
STRAFE_DISTANCE_CONTSTRAINTS {1.6, 2.0}; // m/s, m/s^2

constexpr double STRAFE_ANGLE_KP = 0.03;
constexpr double STRAFE_ANGLE_KI = 0.0;
Expand Down
1 change: 0 additions & 1 deletion include/libraidzero/chassis/controller/xOdomController.hpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -22,7 +22,6 @@
while (!settled && timeLeft && task->notifyTake(0) == 0U) { \
loopedBody \
timeElapsed = pros::millis() - start; \
executeActions(iunitsError, timeElapsed, iactions); \
settled = settleFunction(); \
timeLeft = (timeElapsed < itimeout); \
rate->delayUntil(10_ms); \
Expand Down
8 changes: 6 additions & 2 deletions include/robot/conveyor.hpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -15,8 +15,10 @@ class Conveyor : public TaskWrapper {
std::unique_ptr<MotorController> topController;
std::unique_ptr<MotorController> bottomController;

pros::ADIAnalogIn lineTracker;
int average;
pros::ADIAnalogIn bottomLineTracker;
int bottomAverage;
pros::ADIAnalogIn topLineTracker;
int topAverage;

std::atomic_bool checkingForBalls;
std::atomic_int targetBallsPassed;
Expand All @@ -32,6 +34,8 @@ class Conveyor : public TaskWrapper {
void startCountingBalls();
void waitUntilPassed(int);

void calibrate();

void loop() override;
};

Expand Down
55 changes: 33 additions & 22 deletions src/autonomous.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -16,6 +16,7 @@ void autonomous() {
auto startTime = pros::millis();

const std::string& selectedAuton = GUI::getInstance().selectedAuton;

if (selectedAuton == "Right 1") {
rightSide1(true, true);
} else if (selectedAuton == "Right 2") {
Expand All @@ -24,6 +25,7 @@ void autonomous() {
rightSide3(true);
} else {
reset(0_m, 0_m, 0_deg, false);
rightSide3(true);
// std::shared_ptr<PurePursuitPath> path = std::make_shared<PurePursuitPath>(
// std::vector<Pose2d>{
// {0.6_m, 0_m, 0_deg},
Expand Down Expand Up @@ -89,29 +91,29 @@ void backupFromGoal() {

void rightSide1(bool shouldReset, bool shouldBackOut) {
if (shouldReset) {
reset(0_m, 0_m, 180_deg);
reset(0_m, 0_m, 0_deg);
}
// Backup to release intake
robot::drive->model->xArcade(0.0, -0.8, 0.0);
pros::delay(100);
pros::delay(200);
robot::drive->model->xArcade(0.0, 0.6, 0.0);
pros::delay(100);
robot::drive->model->stop();
pros::delay(100);

// Strafe to face the corner goal
robot::drive->controller->strafeToPose({0.04_m, 0.50_m, 225_deg}, 1500);
pros::delay(60);
robot::drive->controller->strafeToPose({-0.04_m, -0.55_m, 45_deg}, 1200);

// Intake the ball in front of the corner goal
robot::drive->controller->driveForDistance(0.385_m, 500);
robot::drive->controller->driveForDistance(0.67_m, 500);
robot::intake->spinIn(1.0);
pros::delay(900);
robot::intake->stop();
// TODO(louis): ASYNC ACTION

// Finish scoring the preload ball
robot::drive->controller->driveForDistance(0.20_m, 500);
robot::drive->model->xArcade(0.0, 0.7, 0.0);
pros::delay(500);
backupFromGoal();
robot::conveyor->startCountingBalls();
robot::conveyor->moveBoth(1.0);
robot::conveyor->waitUntilPassed(1);
Expand All @@ -125,17 +127,25 @@ void rightSide1(bool shouldReset, bool shouldBackOut) {

void rightSide2(bool shouldReset, bool shouldBackOut) {
if (shouldReset) {
reset(0_m, 0_m, 180_deg);
reset(0_m, 0_m, 0_deg);
}
rightSide1(false, false);
pros::delay(50);

// Backout to second goal
robot::intake->spinOut(1.0);
robot::drive->controller->setMaxVoltage(0.85 * 12000);
robot::drive->controller->strafeToPose({-0.93_m, 0.43_m, 180_deg}, 2500);
pros::delay(500);
backout(500);
robot::intake->stop();
robot::drive->controller->setMaxVoltage(0.85 * 12000);

// std::stringstream ss;
// ss << robot::drive->controller->getState().x.convert(meter);
// ss << ", ";
// ss << robot::drive->controller->getState().y.convert(meter);

// Controller master {ControllerId::master};
// master.setText(0, 0, ss.str());

robot::drive->controller->strafeToPose({0.93_m, -0.43_m, 0_deg}, 4000);
// TODO(louis): ASYNC ACTION
// std::cout << "Final: " << robot::drive->controller->getState().str() << std::endl;
// return;
Expand All @@ -151,45 +161,46 @@ void rightSide2(bool shouldReset, bool shouldBackOut) {
// Score the second ball by ramming into the goal & backing up
robot::conveyor->startCountingBalls();
robot::drive->model->xArcade(0.0, 0.7, 0.0);
pros::delay(1000); // 1000 millis
reset(-0.93_m, 0.12_m, 180_deg, true);
pros::delay(400); // 1000 millis
reset(0.93_m, -0.12_m, 0_deg, true);
backupFromGoal();
//robot::drive->controller->driveForDistance(0.27_m);
//pros::delay(150);
robot::conveyor->moveBoth(1.0);
robot::conveyor->waitUntilPassed(1);
pros::delay(1100);
robot::conveyor->stopAll();
if (shouldBackOut) {
backout(1000);
}
}

void rightSide3(bool shouldReset) {
if (shouldReset) {
reset(0_m, 0_m, 180_deg);
reset(0_m, 0_m, 0_deg);
}
rightSide2(false, false);
backout(700);

// Move to third goal while out-taking
robot::drive->controller->strafeToPose({-2.05_m, 0.40_m, 135_deg}, 2000);
robot::intake->spinOut(1.0);
pros::delay(500);
robot::drive->controller->strafeToPose({1.95_m, -0.40_m, -45_deg}, 2000);
robot::intake->stop();
// TODO(louis): ASYNC ACTION

// Intake the ball in front of the third goal & score it
robot::conveyor->startCountingBalls();
robot::drive->controller->driveForDistance(0.35_m, 500);
robot::intake->spinIn(1.0);
robot::drive->controller->driveForDistance(0.42_m, 800);
// TODO(louis): ASYNC ACTION
pros::delay(400);
pros::delay(700);
robot::intake->stop();

robot::drive->model->xArcade(0.0, 0.7, 0.0);
pros::delay(300);
pros::delay(400);
backupFromGoal();
robot::conveyor->moveBoth(1.0);
robot::conveyor->waitUntilPassed(1);
pros::delay(1000);
robot::conveyor->stopAll();

// Backup and out-take
robot::intake->spinOut(1.0);
Expand Down
2 changes: 2 additions & 0 deletions src/main.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -17,6 +17,8 @@ void initialize() {
#if defined(RUN_WITHOUT_ROBOT) && RUN_WITHOUT_ROBOT
#else
robot::init();
pros::delay(100);
robot::conveyor->calibrate();
#endif
}

Expand Down
1 change: 1 addition & 0 deletions src/opcontrol.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -24,6 +24,7 @@ void opcontrol() {

robot::drive->resetEncoders();
robot::drive->controller->setState({0_m, 0_m, 0_deg});
robot::conveyor->calibrate();

int startTime = pros::millis();
while (true) {
Expand Down
24 changes: 17 additions & 7 deletions src/robot/conveyor.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -7,7 +7,8 @@ namespace robot {

Conveyor::Conveyor()
: TaskWrapper{},
lineTracker{'B'}, average{0},
bottomLineTracker{'B'}, bottomAverage{0},
topLineTracker{'A'}, topAverage{0},
checkingForBalls{false}, targetBallsPassed{0}
{
MotorGroup topMotor {{10}};
Expand All @@ -24,7 +25,8 @@ Conveyor::Conveyor()
bottomController = std::make_unique<MotorController>(bottomMotor);
bottomController->tarePosition();

average = lineTracker.calibrate();
bottomAverage = 2700;
topAverage = 2700;
}

void Conveyor::moveUp(double voltageScale, Position position) {
Expand Down Expand Up @@ -79,25 +81,33 @@ void Conveyor::waitUntilPassed(int numberOfBalls) {
}
}

void Conveyor::calibrate() {
bottomAverage = bottomLineTracker.calibrate();
topAverage = topLineTracker.calibrate();
}

void Conveyor::loop() {
while (task->notifyTake(0) == 0U) {
//std::cout << lineTracker.get_value() << ": avg: " << average << std::endl;
if (checkingForBalls.load(std::memory_order_acquire)) {
int currentBallsPassed = 0;
while (currentBallsPassed < targetBallsPassed.load(std::memory_order_release)) {
//std::cout << average << " - " << lineTracker.get_value() << std::endl;
if (average - lineTracker.get_value() > 400) {
if (bottomAverage - bottomLineTracker.get_value() > 400) {
//std::cout << "passed" << std::endl;
++currentBallsPassed;
}
pros::delay(50);
pros::delay(10);
}
while (!(topAverage - topLineTracker.get_value() > 400)) {
pros::delay(10);
}
pros::delay(200);
stop(Position::Bottom);
pros::delay(400);
pros::delay(200);
stop(Position::Top);
checkingForBalls.store(false, std::memory_order_release);
}
pros::delay(50);
pros::delay(100);
}
}
} // namespace robot

0 comments on commit 4f32e24

Please sign in to comment.