From 00562462ade770f3fbeb969de98ccd29c02235bb Mon Sep 17 00:00:00 2001 From: Alex Date: Fri, 22 Oct 2021 17:14:08 -0600 Subject: [PATCH 1/3] Test PID loop --- include/chassis/drivetrain.hpp | 4 +-- include/chassis/tankDrive.hpp | 6 ++-- src/autonSelector.cpp | 6 ++-- src/chassis/tankDrive.cpp | 27 ++++++++++++---- src/main.cpp | 59 ++++++++++++++++++++-------------- src/pid/pid.cpp | 2 +- src/vision/tippingVision.cpp | 9 ++---- 7 files changed, 67 insertions(+), 46 deletions(-) diff --git a/include/chassis/drivetrain.hpp b/include/chassis/drivetrain.hpp index c268055f..5f8c919a 100644 --- a/include/chassis/drivetrain.hpp +++ b/include/chassis/drivetrain.hpp @@ -74,7 +74,7 @@ namespace Pronounce { * @return backLeft motor pointer */ pros::Motor* getBackLeft() { - return this->frontLeft; + return this->backLeft; } /** @@ -92,7 +92,7 @@ namespace Pronounce { * @return backRight motor pointer */ pros::Motor* getBackRight() { - return this->frontLeft; + return this->backRight; } /** diff --git a/include/chassis/tankDrive.hpp b/include/chassis/tankDrive.hpp index 431383c6..a435fad6 100644 --- a/include/chassis/tankDrive.hpp +++ b/include/chassis/tankDrive.hpp @@ -4,6 +4,8 @@ #include "drivetrain.hpp" #include "position/tankOdom.hpp" #include "pid/pid.hpp" +#include "okapi/api.hpp" +#include "utils/utils.hpp" namespace Pronounce { class TankDrivetrain : public Drivetrain { @@ -25,10 +27,10 @@ namespace Pronounce { double maxVoltage = 127.0; double speedThreshhold = 2.0; - double errorThreshhold = 3.0; + double errorThreshhold = 0.5; double turnThreshhold = 2.0; - double turnErrorThreshhold = 3.0; + double turnErrorThreshhold = 5.0; public: TankDrivetrain(pros::Motor* frontLeft, pros::Motor* frontRight, pros::Motor* backLeft, pros::Motor* backRight, pros::Imu* imu); void reset(); diff --git a/src/autonSelector.cpp b/src/autonSelector.cpp index 41602d84..7702c0db 100644 --- a/src/autonSelector.cpp +++ b/src/autonSelector.cpp @@ -42,9 +42,9 @@ int autonSelector::choose() { } int autonSelector::runSelection() { - //this->preRun(); - //this->functionMap[userSelection](); - //this->postAuton(); + this->preRun(); + this->functionMap[userSelection](); + this->postAuton(); } void autonSelector::setFunction(int position, autonFunction function) { diff --git a/src/chassis/tankDrive.cpp b/src/chassis/tankDrive.cpp index ec38fa2d..26dfce4a 100644 --- a/src/chassis/tankDrive.cpp +++ b/src/chassis/tankDrive.cpp @@ -1,5 +1,11 @@ #include "tankDrive.hpp" +std::shared_ptr visionLogger = std::make_shared( + okapi::TimeUtilFactory::createDefault().getTimer(), + "/ser/sout", // "/usd/visionLog.txt", + okapi::Logger::LogLevel::debug // Show everything + ); + namespace Pronounce { TankDrivetrain::TankDrivetrain(pros::Motor* frontLeft, pros::Motor* frontRight, pros::Motor* backLeft, pros::Motor* backRight, pros::Imu* imu) : Drivetrain(frontLeft, frontRight, backLeft, backRight, imu) { @@ -21,8 +27,9 @@ namespace Pronounce { void TankDrivetrain::update() { - if (!enabled) + if (!enabled) { return; + } tankOdom->update(); @@ -33,9 +40,13 @@ namespace Pronounce { double xDiff = this->targetPosition->getX() - currentPosition->getX(); double yDiff = this->targetPosition->getY() - currentPosition->getY(); - double distance = sqrt(pow(xDiff, 2) + pow(yDiff, 2)) * (reversed ? -1 : 1); + double angleToTarget = toDegrees(atan2(yDiff, xDiff)); + double robotAngleToTarget = fmod(angleToTarget - imu->get_heading(), 360.0); + bool flipDistance = fmod(robotAngleToTarget, 360.0) < 0; //|| (270 < fmod(robotAngleToTarget, 360.0) || fmod(robotAngleToTarget, 360.0) < 90); // < 90.0; // Allow the robot to reverse when it passes the target + + double distance = sqrt(pow(xDiff, 2) + pow(yDiff, 2)) * (flipDistance ? -1 : 1);// * (reversed ? -1 : 1); double linearPosition = sqrt(pow(this->targetPosition->getX() - this->startingPosition->getX(), 2) + pow(this->targetPosition->getY() - this->startingPosition->getY(), 2)) - distance; - double angle = nullRotationDistance < distance ? atan2(yDiff, xDiff) + (reversed ? 180 : 0) : prevAngle; + double angle = nullRotationDistance < distance ? angleToTarget + (reversed ? 180 : 0) : prevAngle; this->prevAngle = angle; this->movePid->setPosition(linearPosition); @@ -46,14 +57,18 @@ namespace Pronounce { double lateral = this->movePid->update(); double turn = this->turnPid->update(); + visionLogger.get()->debug("robotAngleToTarget: " + std::to_string(robotAngleToTarget) + " robotY:" + std::to_string(currentPosition->getY())); + this->getFrontLeft()->move(std::clamp(lateral + turn, -maxVoltage, maxVoltage)); - this->getFrontLeft()->move(std::clamp(lateral - turn, -maxVoltage, maxVoltage)); + this->getBackLeft()->move(std::clamp(lateral + turn, -maxVoltage, maxVoltage)); + this->getFrontRight()->move(std::clamp(lateral - turn, -maxVoltage, maxVoltage)); + this->getBackRight()->move(std::clamp(lateral - turn, -maxVoltage, maxVoltage)); } bool TankDrivetrain::getStopped() { // Derivitive ~= speed - return this->movePid->getDerivitive() < speedThreshhold && this->movePid->getError() < errorThreshhold || - this->turnPid->getDerivitive() < turnThreshhold && this->turnPid->getError() < turnErrorThreshhold; + return abs(this->movePid->getError()) < errorThreshhold && + abs(this->turnPid->getError()) < turnErrorThreshhold; } void TankDrivetrain::waitForStop() { diff --git a/src/main.cpp b/src/main.cpp index 9e06c716..ff9c3069 100644 --- a/src/main.cpp +++ b/src/main.cpp @@ -17,8 +17,6 @@ pros::Motor frontRightMotor(2, true); pros::Motor backLeftMotor(9); pros::Motor backRightMotor(10, true); -// Intake -pros::Motor intakeMotor(8); // Flippers pros::Motor frontLiftLeftMotor(4, MOTOR_GEARSET_36, true); @@ -51,6 +49,12 @@ bool preDriverTasksDone = false; * */ int preAutonRun() { + + // frontLeftMotor.set_brake_mode(pros::motor_brake_mode_e::E_MOTOR_BRAKE_COAST); + // frontRightMotor.set_brake_mode(pros::motor_brake_mode_e::E_MOTOR_BRAKE_COAST); + // backLeftMotor.set_brake_mode(pros::motor_brake_mode_e::E_MOTOR_BRAKE_COAST); + // backRightMotor.set_brake_mode(pros::motor_brake_mode_e::E_MOTOR_BRAKE_COAST); + // Drivetrain tankDrivetrain.setEnabled(true); @@ -79,13 +83,12 @@ int leftAwpRight() { // Move to line tankDrivetrain.setTargetPosition(new Position(33.5, 33.5, -1)); + pros::Task::delay(500); tankDrivetrain.waitForStop(); - // Start collecting and scoring rings - intakeMotor.move(127); - // Collect rings tankDrivetrain.setTargetPosition(new Position(94, 46.8)); + pros::Task::delay(500); tankDrivetrain.waitForStop(); // Manually turn @@ -95,15 +98,14 @@ int leftAwpRight() { // Set down goal frontFlipperMotor.move_absolute(0, 200); - // Stop collecting and scoring rings - intakeMotor.move(0); - // Drop goal backwards tankDrivetrain.setTargetPosition(new Position(2600, 43, -1)); + pros::Task::delay(500); tankDrivetrain.waitForStop(); // Move to right goal tankDrivetrain.setTargetPosition(new Position(124, 35)); + pros::Task::delay(500); tankDrivetrain.waitForStop(); // Pick up goal @@ -111,14 +113,11 @@ int leftAwpRight() { // Move off AWP tankDrivetrain.setTargetPosition(new Position(114, 23.2, -1)); - - // Score in goal - intakeMotor.move(127); + pros::Task::delay(500); + tankDrivetrain.waitForStop(); pros::Task::delay(1000); - intakeMotor.move(0); - return 0; } @@ -170,26 +169,28 @@ int rightStealRight() { * @return 0 */ int testAuton() { + + tankDrivetrain.setEnabled(true); + startingPosition->setX(0); startingPosition->setY(0); startingPosition->setTheta(90); tankDrivetrain.setStartingPosition(startingPosition); - tankDrivetrain.setEnabled(true); - tankDrivetrain.setTargetPosition(new Position(0, 10)); tankDrivetrain.waitForStop(); - // pros::Task::delay(2000); + pros::Task::delay(5000); // tankDrivetrain.setAngle(180); // tankDrivetrain.waitForStop(); // pros::Task::delay(2000); - // tankDrivetrain.setTargetPosition(new Position(0, 0, -1)); - // tankDrivetrain.waitForStop(); + tankDrivetrain.setTargetPosition(new Position(0, 0, -1)); + pros::Task::delay(500); + tankDrivetrain.waitForStop(); return 0; } @@ -218,12 +219,12 @@ void renderThread() { * @brief Tank drive thread * */ -void tankDriveThread() { +pros::task_fn_t tankDriveThread(void) { while (true) { tankDrivetrain.update(); uint32_t now = pros::millis(); - pros::Task::delay_until(&now, pros::millis() + 20); + pros::Task::delay_until(&now, 50); } } @@ -303,6 +304,8 @@ void initSelector() { autonomousSel->setFunction(2, rightAwpLeft); autonomousSel->setFunction(4, rightStealRight); autonomousSel->setFunction(6, testAuton); + + autonomousSel->setSelection(0); } /** @@ -320,7 +323,6 @@ void initLogger() { } void initDrivetrain() { - pros::Task tankDriveTask(tankDriveThread); tankDrivetrain.getTankOdom()->getLeftPivot()->setTuningFactor(1.0); tankDrivetrain.getTankOdom()->getRightPivot()->setTuningFactor(1.0); @@ -328,11 +330,13 @@ void initDrivetrain() { tankDrivetrain.getTankOdom()->setTuningFactor(1.0); PID* turnPid = new PID(0.0, 0.0, 0.0); - PID* movePid = new PID(1.0, 0.0, 0.0); + PID* movePid = new PID(5.0, 0.0, 0.0); tankDrivetrain.setTurnPid(turnPid); tankDrivetrain.setMovePid(movePid); tankDrivetrain.setStartingPosition(startingPosition); + + pros::Task tankDriveTask(tankDriveThread, "TankDrive"); } /** @@ -360,6 +364,7 @@ void updateVisionTask() { } void reset() { + tankDrivetrain.reset(); } /** @@ -466,20 +471,22 @@ void opcontrol() { // Used to test odom on the robot currently if (driveOdomEnabled) { - tankDrivetrain.getTankOdom()->update(); + //tankDrivetrain.getTankOdom()->update(); // Used for testing how well the inertial sensor will keep orientation - lv_label_set_text(infoLabel, tankDrivetrain.getPosition()->to_string().c_str()); + lv_label_set_text(infoLabel, tankDrivetrain.getTankOdom()->to_string()); } if (master.get_digital_new_press(DIGITAL_Y)) { reset(); } - if (master.get_digital_new_press(DIGITAL_B)) { + if (master.get_digital(DIGITAL_B)) { preAutonRun(); testAuton(); postAuton(); + } else { + tankDrivetrain.setEnabled(false); } // Buttons @@ -487,6 +494,8 @@ void opcontrol() { frontLiftLeftButton.update(); frontLiftRightButton.update(); backFlipperButton.update(); + + tankDrivetrain.update(); // Prevent wasted resources pros::delay(10); diff --git a/src/pid/pid.cpp b/src/pid/pid.cpp index baf1dfce..71f347fb 100644 --- a/src/pid/pid.cpp +++ b/src/pid/pid.cpp @@ -19,7 +19,7 @@ namespace Pronounce { } double PID::update() { - this->error = position - target; + this->error = target - position; this->derivitive = error - prevError; if (abs(error) < integralBound) { diff --git a/src/vision/tippingVision.cpp b/src/vision/tippingVision.cpp index 42b1889f..bf8a610e 100644 --- a/src/vision/tippingVision.cpp +++ b/src/vision/tippingVision.cpp @@ -2,11 +2,6 @@ #include "utils/utils.hpp" #include "okapi/api.hpp" -std::shared_ptr visionLogger = std::make_shared( - okapi::TimeUtilFactory::createDefault().getTimer(), - "/ser/sout", // "/usd/visionLog.txt", - okapi::Logger::LogLevel::debug // Show everything - ); namespace PronounceTiP { @@ -38,11 +33,11 @@ namespace PronounceTiP positions[i].setX(x); positions[i].setY(y); } - visionLogger.get()->debug(Pronounce::string_format("Object Count: %d, X: %d, Y: %d", this->get_object_count(), positions[0].getX(), positions[0].getY())); + // visionLogger.get()->debug(Pronounce::string_format("Object Count: %d, X: %d, Y: %d", this->get_object_count(), positions[0].getX(), positions[0].getY())); } - visionLogger.get()->debug(Pronounce::string_format("Object Count: %d", this->get_object_count())); + // visionLogger.get()->debug(Pronounce::string_format("Object Count: %d", this->get_object_count())); } From fab7863dcd8a9ffb56f58c29e83fcdcca0b0aa3b Mon Sep 17 00:00:00 2001 From: Alex Date: Sat, 23 Oct 2021 07:17:25 -0600 Subject: [PATCH 2/3] Tuned PID values for 1 diectional movement Moving to a point is occilating a lot --- include/chassis/tankDrive.hpp | 2 ++ src/chassis/tankDrive.cpp | 20 +++++++++++++------- src/main.cpp | 18 +++++++++++++----- src/position/tankOdom.cpp | 2 +- 4 files changed, 29 insertions(+), 13 deletions(-) diff --git a/include/chassis/tankDrive.hpp b/include/chassis/tankDrive.hpp index a435fad6..34375ae6 100644 --- a/include/chassis/tankDrive.hpp +++ b/include/chassis/tankDrive.hpp @@ -92,6 +92,7 @@ namespace Pronounce { void setStartingPosition(Position* position) { this->tankOdom->setPosition(position); this->startingPosition = startingPosition; + angle = NAN; } void setPosition(Position* position) { @@ -104,6 +105,7 @@ namespace Pronounce { void setTargetPosition(Position* targetPosition) { this->startingPosition = this->getPosition(); + angle = NAN; this->targetPosition = targetPosition; } diff --git a/src/chassis/tankDrive.cpp b/src/chassis/tankDrive.cpp index 26dfce4a..3b451c5a 100644 --- a/src/chassis/tankDrive.cpp +++ b/src/chassis/tankDrive.cpp @@ -41,8 +41,8 @@ namespace Pronounce { double yDiff = this->targetPosition->getY() - currentPosition->getY(); double angleToTarget = toDegrees(atan2(yDiff, xDiff)); - double robotAngleToTarget = fmod(angleToTarget - imu->get_heading(), 360.0); - bool flipDistance = fmod(robotAngleToTarget, 360.0) < 0; //|| (270 < fmod(robotAngleToTarget, 360.0) || fmod(robotAngleToTarget, 360.0) < 90); // < 90.0; // Allow the robot to reverse when it passes the target + double robotAngleToTarget = fmod(angleToTarget - fmod(imu->get_rotation() + 720, 360.0), 360.0); + bool flipDistance = fmod(robotAngleToTarget, 360.0) < 0 && fmod(robotAngleToTarget, 360.0) > -180;// Allow the robot to reverse when it passes the target double distance = sqrt(pow(xDiff, 2) + pow(yDiff, 2)) * (flipDistance ? -1 : 1);// * (reversed ? -1 : 1); double linearPosition = sqrt(pow(this->targetPosition->getX() - this->startingPosition->getX(), 2) + pow(this->targetPosition->getY() - this->startingPosition->getY(), 2)) - distance; @@ -50,14 +50,20 @@ namespace Pronounce { this->prevAngle = angle; this->movePid->setPosition(linearPosition); - this->movePid->setTarget(distance); - this->turnPid->setPosition(imu->get_heading()); - this->turnPid->setTarget(this->getStopped() ? this->angle : toDegrees(angle)); + this->movePid->setTarget(isnan(this->angle) ? distance : linearPosition); + this->turnPid->setPosition(imu->get_rotation()); + this->turnPid->setTarget(isnan(this->angle) ? angle : this->angle); - double lateral = this->movePid->update(); + double lateral = std::clamp(this->movePid->update(), -127.0, 127.0); double turn = this->turnPid->update(); - visionLogger.get()->debug("robotAngleToTarget: " + std::to_string(robotAngleToTarget) + " robotY:" + std::to_string(currentPosition->getY())); + visionLogger.get()->debug("imu angle: " + std::to_string(turnPid->getPosition()) + + " Turn target:" + std::to_string(turnPid->getTarget()) + + " X:" + std::to_string(currentPosition->getX()) + + " Y:" + std::to_string(currentPosition->getY()) + + " TargetX:" + std::to_string(targetPosition->getX()) + + " targetY:" + std::to_string(targetPosition->getY()) + + " lateral:" + std::to_string(std::clamp(lateral, -maxVoltage, maxVoltage))); this->getFrontLeft()->move(std::clamp(lateral + turn, -maxVoltage, maxVoltage)); this->getBackLeft()->move(std::clamp(lateral + turn, -maxVoltage, maxVoltage)); diff --git a/src/main.cpp b/src/main.cpp index ff9c3069..4b82951d 100644 --- a/src/main.cpp +++ b/src/main.cpp @@ -178,14 +178,22 @@ int testAuton() { tankDrivetrain.setStartingPosition(startingPosition); - tankDrivetrain.setTargetPosition(new Position(0, 10)); + tankDrivetrain.setTargetPosition(new Position(24, 24)); tankDrivetrain.waitForStop(); - pros::Task::delay(5000); + pros::Task::delay(4000); // tankDrivetrain.setAngle(180); // tankDrivetrain.waitForStop(); + // pros::Task::delay(5000); + + // tankDrivetrain.setAngle(0); + // pros::Task::delay(500); + // tankDrivetrain.waitForStop(); + + // pros::Task::delay(2000); + // pros::Task::delay(2000); tankDrivetrain.setTargetPosition(new Position(0, 0, -1)); @@ -329,8 +337,8 @@ void initDrivetrain() { tankDrivetrain.getTankOdom()->setTuningFactor(1.0); - PID* turnPid = new PID(0.0, 0.0, 0.0); - PID* movePid = new PID(5.0, 0.0, 0.0); + PID* turnPid = new PID(4, 0.0, -2); + PID* movePid = new PID(15.0, 0.0, -10.0); tankDrivetrain.setTurnPid(turnPid); tankDrivetrain.setMovePid(movePid); @@ -474,7 +482,7 @@ void opcontrol() { //tankDrivetrain.getTankOdom()->update(); // Used for testing how well the inertial sensor will keep orientation - lv_label_set_text(infoLabel, tankDrivetrain.getTankOdom()->to_string()); + lv_label_set_text(infoLabel, tankDrivetrain.getTankOdom()->to_string().c_str()); } if (master.get_digital_new_press(DIGITAL_Y)) { diff --git a/src/position/tankOdom.cpp b/src/position/tankOdom.cpp index 9c100c4d..fc5d60c9 100644 --- a/src/position/tankOdom.cpp +++ b/src/position/tankOdom.cpp @@ -24,7 +24,7 @@ namespace Pronounce double x1 = 0; double y1 = average; - double x2 = - (x1 * cos(angle)) + (y1 * sin(angle)); + double x2 = (x1 * cos(angle)) + (y1 * sin(angle)); double y2 = (x1 * sin(angle)) + (y1 * cos(angle)); if (std::isnan(x2) || std::isnan(y2)) { From 59b88742a83d9369c1d0c9e016abc014d00d6026 Mon Sep 17 00:00:00 2001 From: Alex Date: Sat, 23 Oct 2021 14:47:18 -0600 Subject: [PATCH 3/3] I don't know what changes I made they just may have made it work --- include/chassis/tankDrive.hpp | 1 + src/autonSelector.cpp | 7 ------- src/chassis/tankDrive.cpp | 10 +++++----- src/main.cpp | 34 +++++++++++++--------------------- 4 files changed, 19 insertions(+), 33 deletions(-) diff --git a/include/chassis/tankDrive.hpp b/include/chassis/tankDrive.hpp index 34375ae6..0f371db9 100644 --- a/include/chassis/tankDrive.hpp +++ b/include/chassis/tankDrive.hpp @@ -92,6 +92,7 @@ namespace Pronounce { void setStartingPosition(Position* position) { this->tankOdom->setPosition(position); this->startingPosition = startingPosition; + this->imu->set_rotation(position->getTheta()); angle = NAN; } diff --git a/src/autonSelector.cpp b/src/autonSelector.cpp index 7702c0db..4b3994de 100644 --- a/src/autonSelector.cpp +++ b/src/autonSelector.cpp @@ -30,13 +30,6 @@ int autonSelector::choose() { this->draw(); this->addCallbacks(); - // Wait until finnished. - while (!guiFinnished) { - pros::delay(20); - } - - this->del(); - // Return the selection. return userSelection; } diff --git a/src/chassis/tankDrive.cpp b/src/chassis/tankDrive.cpp index 3b451c5a..e21b1076 100644 --- a/src/chassis/tankDrive.cpp +++ b/src/chassis/tankDrive.cpp @@ -41,10 +41,10 @@ namespace Pronounce { double yDiff = this->targetPosition->getY() - currentPosition->getY(); double angleToTarget = toDegrees(atan2(yDiff, xDiff)); - double robotAngleToTarget = fmod(angleToTarget - fmod(imu->get_rotation() + 720, 360.0), 360.0); - bool flipDistance = fmod(robotAngleToTarget, 360.0) < 0 && fmod(robotAngleToTarget, 360.0) > -180;// Allow the robot to reverse when it passes the target + double robotAngleToTarget = fmod(angleToTarget - imu->get_heading() + 360, 360.0); + bool flipDistance = -90 < fmod(robotAngleToTarget, 360.0) < 90;// Allow the robot to reverse when it passes the target - double distance = sqrt(pow(xDiff, 2) + pow(yDiff, 2)) * (flipDistance ? -1 : 1);// * (reversed ? -1 : 1); + double distance = sqrt(pow(xDiff, 2) + pow(yDiff, 2)) * (flipDistance ? -1 : 1) * (reversed ? -1 : 1); double linearPosition = sqrt(pow(this->targetPosition->getX() - this->startingPosition->getX(), 2) + pow(this->targetPosition->getY() - this->startingPosition->getY(), 2)) - distance; double angle = nullRotationDistance < distance ? angleToTarget + (reversed ? 180 : 0) : prevAngle; this->prevAngle = angle; @@ -58,12 +58,12 @@ namespace Pronounce { double turn = this->turnPid->update(); visionLogger.get()->debug("imu angle: " + std::to_string(turnPid->getPosition()) - + " Turn target:" + std::to_string(turnPid->getTarget()) + + " robotAngleToTarget:" + std::to_string(robotAngleToTarget) + " X:" + std::to_string(currentPosition->getX()) + " Y:" + std::to_string(currentPosition->getY()) + " TargetX:" + std::to_string(targetPosition->getX()) + " targetY:" + std::to_string(targetPosition->getY()) - + " lateral:" + std::to_string(std::clamp(lateral, -maxVoltage, maxVoltage))); + + " heading:" + std::to_string(imu->get_heading()));// std::clamp(lateral, -maxVoltage, maxVoltage))); this->getFrontLeft()->move(std::clamp(lateral + turn, -maxVoltage, maxVoltage)); this->getBackLeft()->move(std::clamp(lateral + turn, -maxVoltage, maxVoltage)); diff --git a/src/main.cpp b/src/main.cpp index 4b82951d..0547f56e 100644 --- a/src/main.cpp +++ b/src/main.cpp @@ -55,6 +55,10 @@ int preAutonRun() { // backLeftMotor.set_brake_mode(pros::motor_brake_mode_e::E_MOTOR_BRAKE_COAST); // backRightMotor.set_brake_mode(pros::motor_brake_mode_e::E_MOTOR_BRAKE_COAST); + while(imu.is_calibrating()) { + pros::Task::delay(50); + } + // Drivetrain tankDrivetrain.setEnabled(true); @@ -181,7 +185,7 @@ int testAuton() { tankDrivetrain.setTargetPosition(new Position(24, 24)); tankDrivetrain.waitForStop(); - pros::Task::delay(4000); + pros::Task::delay(6000); // tankDrivetrain.setAngle(180); // tankDrivetrain.waitForStop(); @@ -338,7 +342,7 @@ void initDrivetrain() { tankDrivetrain.getTankOdom()->setTuningFactor(1.0); PID* turnPid = new PID(4, 0.0, -2); - PID* movePid = new PID(15.0, 0.0, -10.0); + PID* movePid = new PID(10.0, 0.0, 0.0); tankDrivetrain.setTurnPid(turnPid); tankDrivetrain.setMovePid(movePid); @@ -419,9 +423,6 @@ void disabled() { */ void competition_initialize() { - // Show GUI - autonomousSel->choose(); - } /** @@ -430,7 +431,7 @@ void competition_initialize() { void autonomous() { // This calls the user selection, all the functions prototypes are in // autonRoutines.hpp and the implementation is autonRoutines.cpp - autonomousSel->runSelection(); + //autonomousSel->runSelection(); } @@ -440,16 +441,15 @@ void autonomous() { */ void opcontrol() { + // Show GUI + //autonomousSel->choose(); + + tankDrivetrain.setEnabled(false); + if (!preDriverTasksDone) { preDriver(); } - // Delete all items on screen - lv_obj_clean(lv_scr_act()); - - // Condensed way to put a few pieces of information on screen - lv_obj_t* infoLabel = lv_label_create(lv_scr_act(), NULL); - // Motor buttons MotorButton frontFlipperButton(&master, &frontFlipperMotor, DIGITAL_R1, DIGITAL_R2, 90, 0, -90, 0, 0); MotorButton frontLiftLeftButton(&master, &frontLiftLeftMotor, DIGITAL_L1, DIGITAL_L2, 90, 0, -90, 0, 0); @@ -482,21 +482,13 @@ void opcontrol() { //tankDrivetrain.getTankOdom()->update(); // Used for testing how well the inertial sensor will keep orientation - lv_label_set_text(infoLabel, tankDrivetrain.getTankOdom()->to_string().c_str()); + //lv_label_set_text(infoLabel, tankDrivetrain.getTankOdom()->to_string().c_str()); } if (master.get_digital_new_press(DIGITAL_Y)) { reset(); } - if (master.get_digital(DIGITAL_B)) { - preAutonRun(); - testAuton(); - postAuton(); - } else { - tankDrivetrain.setEnabled(false); - } - // Buttons frontFlipperButton.update(); frontLiftLeftButton.update();