diff --git a/.vscode/settings.json b/.vscode/settings.json index bbd0bdb7..e12f2d29 100644 --- a/.vscode/settings.json +++ b/.vscode/settings.json @@ -1,95 +1,106 @@ { "files.associations": { - "ios": "cpp", - "type_traits": "cpp", - "cmath": "cpp", - "valarray": "cpp", - "hash_map": "cpp", - "deque": "cpp", - "forward_list": "cpp", - "list": "cpp", - "string": "cpp", - "unordered_map": "cpp", - "unordered_set": "cpp", - "vector": "cpp", - "system_error": "cpp", - "__hash_table": "cpp", - "__split_buffer": "cpp", - "__tree": "cpp", - "filesystem": "cpp", - "map": "cpp", - "pros": "json", - "set": "cpp", - "array": "cpp", - "atomic": "cpp", - "bit": "cpp", - "*.tcc": "cpp", - "bitset": "cpp", - "cctype": "cpp", - "compare": "cpp", - "cstdarg": "cpp", - "cstddef": "cpp", - "cstdint": "cpp", - "cstdio": "cpp", - "cstdlib": "cpp", - "cstring": "cpp", - "ctime": "cpp", - "cwchar": "cpp", - "cwctype": "cpp", - "exception": "cpp", - "algorithm": "cpp", - "functional": "cpp", - "iterator": "cpp", - "memory": "cpp", - "memory_resource": "cpp", - "random": "cpp", - "string_view": "cpp", - "tuple": "cpp", - "utility": "cpp", - "fstream": "cpp", - "initializer_list": "cpp", - "iosfwd": "cpp", - "iostream": "cpp", - "istream": "cpp", - "limits": "cpp", - "new": "cpp", - "ostream": "cpp", - "sstream": "cpp", - "stdexcept": "cpp", - "streambuf": "cpp", - "cinttypes": "cpp", - "typeindex": "cpp", - "typeinfo": "cpp", - "chrono": "cpp", - "clocale": "cpp", - "codecvt": "cpp", - "concepts": "cpp", - "condition_variable": "cpp", - "numeric": "cpp", - "optional": "cpp", - "ratio": "cpp", - "iomanip": "cpp", - "mutex": "cpp", - "numbers": "cpp", - "semaphore": "cpp", - "stop_token": "cpp", - "thread": "cpp", - "variant": "cpp", - "__bit_reference": "cpp", - "__bits": "cpp", - "__config": "cpp", - "__debug": "cpp", - "__errc": "cpp", - "__functional_base": "cpp", - "__locale": "cpp", - "__mutex_base": "cpp", - "__node_handle": "cpp", - "__nullptr": "cpp", - "__string": "cpp", - "__threading_support": "cpp", - "__tuple": "cpp", - "locale": "cpp", - "queue": "cpp", - "stack": "cpp" - } + "ios": "cpp", + "type_traits": "cpp", + "cmath": "cpp", + "valarray": "cpp", + "hash_map": "cpp", + "deque": "cpp", + "forward_list": "cpp", + "list": "cpp", + "string": "cpp", + "unordered_map": "cpp", + "unordered_set": "cpp", + "vector": "cpp", + "system_error": "cpp", + "__hash_table": "cpp", + "__split_buffer": "cpp", + "__tree": "cpp", + "filesystem": "cpp", + "map": "cpp", + "pros": "json", + "set": "cpp", + "array": "cpp", + "atomic": "cpp", + "bit": "cpp", + "*.tcc": "cpp", + "bitset": "cpp", + "cctype": "cpp", + "compare": "cpp", + "cstdarg": "cpp", + "cstddef": "cpp", + "cstdint": "cpp", + "cstdio": "cpp", + "cstdlib": "cpp", + "cstring": "cpp", + "ctime": "cpp", + "cwchar": "cpp", + "cwctype": "cpp", + "exception": "cpp", + "algorithm": "cpp", + "functional": "cpp", + "iterator": "cpp", + "memory": "cpp", + "memory_resource": "cpp", + "random": "cpp", + "string_view": "cpp", + "tuple": "cpp", + "utility": "cpp", + "fstream": "cpp", + "initializer_list": "cpp", + "iosfwd": "cpp", + "iostream": "cpp", + "istream": "cpp", + "limits": "cpp", + "new": "cpp", + "ostream": "cpp", + "sstream": "cpp", + "stdexcept": "cpp", + "streambuf": "cpp", + "cinttypes": "cpp", + "typeindex": "cpp", + "typeinfo": "cpp", + "chrono": "cpp", + "clocale": "cpp", + "codecvt": "cpp", + "concepts": "cpp", + "condition_variable": "cpp", + "numeric": "cpp", + "optional": "cpp", + "ratio": "cpp", + "iomanip": "cpp", + "mutex": "cpp", + "numbers": "cpp", + "semaphore": "cpp", + "stop_token": "cpp", + "thread": "cpp", + "variant": "cpp", + "__bit_reference": "cpp", + "__bits": "cpp", + "__config": "cpp", + "__debug": "cpp", + "__errc": "cpp", + "__functional_base": "cpp", + "__locale": "cpp", + "__mutex_base": "cpp", + "__node_handle": "cpp", + "__nullptr": "cpp", + "__string": "cpp", + "__threading_support": "cpp", + "__tuple": "cpp", + "locale": "cpp", + "queue": "cpp", + "stack": "cpp", + "any": "cpp", + "cfenv": "cpp", + "charconv": "cpp", + "complex": "cpp", + "csetjmp": "cpp", + "csignal": "cpp", + "cuchar": "cpp", + "regex": "cpp", + "future": "cpp", + "scoped_allocator": "cpp", + "shared_mutex": "cpp" + } } \ No newline at end of file diff --git a/include/autoPaths.hpp b/include/autoPaths.hpp index 52ec17a6..82b398b9 100644 --- a/include/autoPaths.hpp +++ b/include/autoPaths.hpp @@ -34,10 +34,10 @@ namespace Pronounce { int nearPlatformViaLeftNeutralToFarPlatformIndex; int nearPlatformToMidIndex; - void autoPaths(PurePursuit purePursuit) { + void autoPaths(PurePursuit* purePursuit) { // Default pure pursuit profile PurePursuitProfile defaultProfile(new PID(20, 0.0, 2.0), new PID(60.0, 0.0, 5.0), 10.0); - purePursuit.getPurePursuitProfileManager().setDefaultProfile(defaultProfile); + purePursuit->getPurePursuitProfileManager().setDefaultProfile(defaultProfile); // Test path Path testPath = Path(); @@ -49,14 +49,14 @@ namespace Pronounce { testPath.addPoint(-24, 48); testPath.addPoint(0, 24); - testPathIndex = purePursuit.addPath(testPath); + testPathIndex = purePursuit->addPath(testPath); Path rightNeutralToRightHomeZone; rightNeutralToRightHomeZone.addPoint(105.7, 60); rightNeutralToRightHomeZone.addPoint(105.7, 16); - rightNeutralToRightHomeIndex = purePursuit.addPath(rightNeutralToRightHomeZone); + rightNeutralToRightHomeIndex = purePursuit->addPath(rightNeutralToRightHomeZone); // Right Steal Right Path rightHomeToGoalNeutral; @@ -64,50 +64,50 @@ namespace Pronounce { rightHomeToGoalNeutral.addPoint(105.7, 16); rightHomeToGoalNeutral.addPoint(105.7, 61); - rightHomeToGoalNeutralIndex = purePursuit.addPath(rightHomeToGoalNeutral); + rightHomeToGoalNeutralIndex = purePursuit->addPath(rightHomeToGoalNeutral); - Path rightNeutralToMidNeutral; + SplinePath rightNeutralToMidNeutral; rightNeutralToMidNeutral.addPoint(105.7, 62); rightNeutralToMidNeutral.addPoint(75.3, 40); rightNeutralToMidNeutral.addPoint(60.3, 65); - rightNeutralToMidNeutralIndex = purePursuit.addPath(rightNeutralToMidNeutral); + rightNeutralToMidNeutralIndex = purePursuit->addPath(rightNeutralToMidNeutral.getPath(0.1)); Path midNeutralToRightAlliance; midNeutralToRightAlliance.addPoint(70.3, 65); midNeutralToRightAlliance.addPoint(120.1, 28); - midNeutralToRightAllianceIndex = purePursuit.addPath(midNeutralToRightAlliance); + midNeutralToRightAllianceIndex = purePursuit->addPath(midNeutralToRightAlliance); Path midNeutralToMidHomeZone; midNeutralToMidHomeZone.addPoint(70.3, 70.3); midNeutralToMidHomeZone.addPoint(70.3, 36); - midNeutralToMidHomeZoneIndex = purePursuit.addPath(midNeutralToMidHomeZone); + midNeutralToMidHomeZoneIndex = purePursuit->addPath(midNeutralToMidHomeZone); Path farRightHomeZoneToRightAlliance; farRightHomeZoneToRightAlliance.addPoint(127.9, 16); farRightHomeZoneToRightAlliance.addPoint(127.9, 24); - farRightHomeZoneToRightAllianceIndex = purePursuit.addPath(farRightHomeZoneToRightAlliance); + farRightHomeZoneToRightAllianceIndex = purePursuit->addPath(farRightHomeZoneToRightAlliance); Path rightAllianceToRightHomeZone; rightAllianceToRightHomeZone.addPoint(127.9, 24); rightAllianceToRightHomeZone.addPoint(105.7, 16); - rightAllianceToRightHomeZoneIndex = purePursuit.addPath(rightAllianceToRightHomeZone); + rightAllianceToRightHomeZoneIndex = purePursuit->addPath(rightAllianceToRightHomeZone); Path leftAllianceToLeftNeutral; leftAllianceToLeftNeutral.addPoint(29, 11.4); leftAllianceToLeftNeutral.addPoint(32, 67); - leftAllianceToLeftNeutralIndex = purePursuit.addPath(leftAllianceToLeftNeutral); + leftAllianceToLeftNeutralIndex = purePursuit->addPath(leftAllianceToLeftNeutral); Path leftNeutralToMidNeutral; @@ -115,7 +115,7 @@ namespace Pronounce { leftNeutralToMidNeutral.addPoint(65.3, 40); leftNeutralToMidNeutral.addPoint(70.3, 65); - leftNeutralToMidNeutralIndex = purePursuit.addPath(leftNeutralToMidNeutral); + leftNeutralToMidNeutralIndex = purePursuit->addPath(leftNeutralToMidNeutral); // mid neutral to mid home zone @@ -126,7 +126,7 @@ namespace Pronounce { rightNeutralToFarPlatform.addPoint(75, 100); rightNeutralToFarPlatform.addPoint(60.3, 115); - rightNeutralToFarPlatformIndex = purePursuit.addPath(rightNeutralToFarPlatform); + rightNeutralToFarPlatformIndex = purePursuit->addPath(rightNeutralToFarPlatform); Path farPlatformToNearPlatform; @@ -136,7 +136,7 @@ namespace Pronounce { farPlatformToNearPlatform.addPoint(58.6, 45); farPlatformToNearPlatform.addPoint(70.3, 30.7); - farPlatformToNearPlatformIndex = purePursuit.addPath(farPlatformToNearPlatform); + farPlatformToNearPlatformIndex = purePursuit->addPath(farPlatformToNearPlatform); Path nearPlatformViaLeftNeutralToFarPlatform; @@ -144,14 +144,16 @@ namespace Pronounce { nearPlatformViaLeftNeutralToFarPlatform.addPoint(35, 61); nearPlatformViaLeftNeutralToFarPlatform.addPoint(70.3, 115); - nearPlatformViaLeftNeutralToFarPlatformIndex = purePursuit.addPath(nearPlatformViaLeftNeutralToFarPlatform); + nearPlatformViaLeftNeutralToFarPlatformIndex = purePursuit->addPath(nearPlatformViaLeftNeutralToFarPlatform); Path nearPlatformToMid; nearPlatformToMid.addPoint(70.3, 115); nearPlatformToMid.addPoint(70.3, 70.3); - nearPlatformToMidIndex = purePursuit.addPath(nearPlatformToMid); + nearPlatformToMidIndex = purePursuit->addPath(nearPlatformToMid); + + printf("Array size: %d\n", purePursuit->getPaths().size()); } } // Namespace Prononce diff --git a/include/chassis/abstractDrivetrain.hpp b/include/chassis/abstractDrivetrain.hpp new file mode 100644 index 00000000..921fb6eb --- /dev/null +++ b/include/chassis/abstractDrivetrain.hpp @@ -0,0 +1,17 @@ +#pragma once + +namespace Pronounce { + /** + * @brief Abstract class to be used in both the pure pursuit simulator and on the robot. + * + */ + class AbstractDrivetrain { + private: + + public: + AbstractDrivetrain(); + + ~AbstractDrivetrain(); + }; + +} // namespace Pronounce diff --git a/include/chassis/abstractTankDrivetrain.hpp b/include/chassis/abstractTankDrivetrain.hpp new file mode 100644 index 00000000..51adf631 --- /dev/null +++ b/include/chassis/abstractTankDrivetrain.hpp @@ -0,0 +1,44 @@ +#pragma once + +#include "abstractDrivetrain.hpp" +#include "utils/utils.hpp" + +namespace Pronounce { + class AbstractTankDrivetrain : public AbstractDrivetrain { + private: + double trackWidth; + public: + AbstractTankDrivetrain(); + AbstractTankDrivetrain(double trackWidth); + + double getTrackWidth() { + return trackWidth; + } + + void setTrackWidth(double trackWidth) { + this->trackWidth = trackWidth; + } + + void driveCurvature(double speed, double curvature) { + double leftSpeed = speed * (2.0 - curvature * trackWidth) / 2.0; + double rightSpeed = speed * (2.0 + curvature * trackWidth) / 2.0; + + double maxSpeed = max(abs(leftSpeed), abs(rightSpeed)); + + if (maxSpeed > abs(speed)) { + double multiplier = abs(speed) / maxSpeed; + leftSpeed *= multiplier; + rightSpeed *= multiplier; + } + + this->tankSteerVelocity(leftSpeed, rightSpeed); + } + + virtual void skidSteerVelocity(double speed, double turn) {} + + virtual void tankSteerVelocity(double leftSpeed, double rightSpeed) {} + + ~AbstractTankDrivetrain(); + }; +} // namespace Pronounce + diff --git a/include/chassis/drivetrain.hpp b/include/chassis/drivetrain.hpp index a77271cb..58ddf604 100644 --- a/include/chassis/drivetrain.hpp +++ b/include/chassis/drivetrain.hpp @@ -3,12 +3,13 @@ #include "api.h" #include "utils/utils.hpp" #include "utils/motorGroup.hpp" +#include "abstractDrivetrain.hpp" namespace Pronounce { /** * Abstract class as a structure for different drivetrains assuming there are 4 motors */ - class Drivetrain { + class Drivetrain : public AbstractDrivetrain { protected: MotorGroup leftMotors; @@ -19,9 +20,10 @@ namespace Pronounce { public: Drivetrain(); Drivetrain(pros::Motor* frontLeft, pros::Motor* frontRight, pros::Motor* backLeft, pros::Motor* backRight, pros::Imu* imu); + Drivetrain(pros::Motor* frontLeft, pros::Motor* frontRight, pros::Motor* midLeft, pros::Motor* midRight, pros::Motor* backLeft, pros::Motor* backRight, pros::Imu* imu); Drivetrain(MotorGroup leftMotors, MotorGroup rightMotors, pros::Imu* imu); - /** + /** * Get average temperature of all the motors. */ double getTemp(); @@ -63,7 +65,9 @@ namespace Pronounce { this->rightMotors.addMotor(motor); } - virtual void update() {} + ~Drivetrain() { + + } }; } // namespace Pronounce diff --git a/include/chassis/simDrivetrain.hpp b/include/chassis/simDrivetrain.hpp new file mode 100644 index 00000000..d1fe0eb7 --- /dev/null +++ b/include/chassis/simDrivetrain.hpp @@ -0,0 +1,44 @@ +#pragma once + +#include "abstractDrivetrain.hpp" +#include "utils/position.hpp" + +namespace Pronounce { + /** + * @brief A class for all simulator drivetrains + * + */ + class SimDrivetrain { + private: + Position* position; + double resetOrientation = 0.0; + public: + SimDrivetrain(); + SimDrivetrain(Position* position); + + Position* getPosition() { + return position; + } + + void setPosition(Position* position) { + this->position = position; + } + + double getResetOrientation() { + return resetOrientation; + } + + void setResetOrientation(double resetPosition) { + this->resetOrientation = resetOrientation; + } + + void reset(Position* position) { + this->position->operator=(position); + this->resetOrientation = position->getTheta(); + } + + virtual void update() {} + + ~SimDrivetrain(); + }; +} // namespace Pronounce diff --git a/include/chassis/simTankDrivetrain.hpp b/include/chassis/simTankDrivetrain.hpp new file mode 100644 index 00000000..13d507c7 --- /dev/null +++ b/include/chassis/simTankDrivetrain.hpp @@ -0,0 +1,38 @@ +#pragma once + +#include "abstractTankDrivetrain.hpp" +#include "simDrivetrain.hpp" +#include "utils/vector.hpp" +#include "utils/utils.hpp" +#include +#include + +namespace Pronounce { + class SimTankDrivetrain : public AbstractTankDrivetrain, public SimDrivetrain { + private: + double maxAcceleration; + double maxSpeed; + double leftVelocity = 0.0, rightVelocity = 0.0; + double leftVelocityTarget = 0.0, rightVelocityTarget = 0.0; + double leftDistance = 0.0, rightDistance = 0.0; + public: + SimTankDrivetrain(); + SimTankDrivetrain(double trackWidth); + SimTankDrivetrain(double trackWidth, double maxAcceleration, double maxSpeed); + SimTankDrivetrain(double trackWidth, double maxAcceleration, double maxSpeed, Position* position); + + void update(); + + void skidSteerVelocity(double speed, double turn) { + leftVelocityTarget = speed - turn; + rightVelocityTarget = speed + turn; + } + + void tankSteerVelocity(double leftSpeed, double rightSpeed) { + leftVelocityTarget = leftSpeed; + rightVelocityTarget = rightSpeed; + } + + ~SimTankDrivetrain(); + }; +} // namespace Pronounce diff --git a/include/chassis/tankDrive.hpp b/include/chassis/tankDrive.hpp index 20b2d1fb..a2e7008e 100644 --- a/include/chassis/tankDrive.hpp +++ b/include/chassis/tankDrive.hpp @@ -2,12 +2,13 @@ #include "api.h" #include "drivetrain.hpp" +#include "abstractTankDrivetrain.hpp" #include "utils/motorGroup.hpp" namespace Pronounce { - class TankDrivetrain : public Drivetrain { + class TankDrivetrain : public AbstractTankDrivetrain, public Drivetrain { private: - double trackWidth; + public: TankDrivetrain(pros::Motor* frontLeft, pros::Motor* frontRight, pros::Motor* backLeft, pros::Motor* backRight, pros::Imu* imu); TankDrivetrain(pros::Motor* frontLeft, pros::Motor* frontRight, pros::Motor* backLeft, pros::Motor* backRight, pros::Imu* imu, double trackWidth); @@ -24,14 +25,6 @@ namespace Pronounce { this->getRightMotors().move_velocity(rightSpeed); } - double getTrackWidth() { - return this->trackWidth; - } - - void setTrackWidth(double trackWidth) { - this->trackWidth = trackWidth; - } - ~TankDrivetrain(); }; } // namespace Pronounce diff --git a/include/main.h b/include/main.h index 47978fd9..b4a52abd 100644 --- a/include/main.h +++ b/include/main.h @@ -55,6 +55,7 @@ #include "auton/autonSelector.hpp" // Chassis +#include "chassis/abstractDrivetrain.hpp" #include "chassis/drivetrain.hpp" #include "chassis/mecanumDrivetrain.hpp" #include "chassis/omniDrivetrain.hpp" diff --git a/include/motionControl/purePursuit.hpp b/include/motionControl/purePursuit.hpp index 1dab5b11..87bba47f 100644 --- a/include/motionControl/purePursuit.hpp +++ b/include/motionControl/purePursuit.hpp @@ -3,6 +3,7 @@ #include #include #include +#include #include "pid/pid.hpp" #include "utils/path.hpp" #include "utils/position.hpp" @@ -20,6 +21,7 @@ namespace Pronounce { struct PurePursuitPointData { Point lookaheadPoint; Vector lookaheadVector; + Vector localLookaheadVector; Vector normalizedLookaheadVector; double curvature; }; @@ -185,6 +187,14 @@ namespace Pronounce { this->stopDistance = stopDistance; } + double getLookahead() { + return lookahead; + } + + void setLookahead(double lookahead) { + this->lookahead = lookahead; + } + ~PurePursuit(); }; } // Pronounce diff --git a/include/motionControl/tankPurePursuit.hpp b/include/motionControl/tankPurePursuit.hpp index 5eadd620..19ef9af5 100644 --- a/include/motionControl/tankPurePursuit.hpp +++ b/include/motionControl/tankPurePursuit.hpp @@ -1,31 +1,30 @@ #pragma once -#include "api.h" #include "purePursuit.hpp" -#include "chassis/tankDrive.hpp" -#include +#include "chassis/abstractTankDrivetrain.hpp" +#include namespace Pronounce { class TankPurePursuit : public PurePursuit { private: - TankDrivetrain* drivetrain; + AbstractTankDrivetrain* drivetrain; double speed = 100; bool inverted = false; public: - TankPurePursuit(TankDrivetrain* drivetrain); - TankPurePursuit(TankDrivetrain* drivetrain, double lookaheadDistance); - TankPurePursuit(TankDrivetrain* drivetrain, Odometry* odometry, double lookaheadDistance); + TankPurePursuit(AbstractTankDrivetrain* drivetrain); + TankPurePursuit(AbstractTankDrivetrain* drivetrain, double lookaheadDistance); + TankPurePursuit(AbstractTankDrivetrain* drivetrain, Odometry* odometry, double lookaheadDistance); void updateDrivetrain(); void stop(); - TankDrivetrain* getDrivetrain() { + AbstractTankDrivetrain* getDrivetrain() { return drivetrain; } - void setDrivetrain(TankDrivetrain* drivetrain) { + void setDrivetrain(AbstractTankDrivetrain* drivetrain) { this->drivetrain = drivetrain; } diff --git a/include/odometry/simOdometry.hpp b/include/odometry/simOdometry.hpp new file mode 100644 index 00000000..f0206f58 --- /dev/null +++ b/include/odometry/simOdometry.hpp @@ -0,0 +1,32 @@ +#pragma once + +#include "chassis/simDrivetrain.hpp" +#include "odometry.hpp" + +namespace Pronounce { + class SimOdometry : public Odometry { + private: + SimDrivetrain* drivetrain; + public: + SimOdometry(); + SimOdometry(SimDrivetrain* drivetrain); + + void update(); + + void reset(Position* position) { + this->drivetrain->setPosition(position); + this->getPosition()->operator=(position); + this->getResetPosition()->operator=(position); + } + + SimDrivetrain* getDrivetrain() { + return drivetrain; + } + + void setDrivetrain(SimDrivetrain* drivetrain) { + this->drivetrain = drivetrain; + } + + ~SimOdometry(); + }; +} // namespace Pronounce diff --git a/include/utils/pointUtil.hpp b/include/utils/pointUtil.hpp index d5ea4ffe..e46a2cd5 100644 --- a/include/utils/pointUtil.hpp +++ b/include/utils/pointUtil.hpp @@ -73,6 +73,11 @@ namespace Pronounce { this->setY(point.getY()); } + void operator=(Point* point) { + this->setX(point->getX()); + this->setY(point->getY()); + } + void add(Point point) { this->setX(this->getX() + point.getX()); this->setY(this->getY() + point.getY()); @@ -83,6 +88,11 @@ namespace Pronounce { this->setY(this->getY() + point.getY()); } + void operator+=(Point* point) { + this->setX(this->getX() + point->getX()); + this->setY(this->getY() + point->getY()); + } + std::string to_string() { return "X: " + std::to_string(x) + ", Y: " + std::to_string(y); } diff --git a/include/utils/quadraticSplinePath.hpp b/include/utils/quadraticSplinePath.hpp index ab682644..45f48770 100644 --- a/include/utils/quadraticSplinePath.hpp +++ b/include/utils/quadraticSplinePath.hpp @@ -21,7 +21,7 @@ namespace Pronounce Path path; for (int i = 0; i < points.size() - 1; i++) { - printf("%d\n", i); + printf("Index: %d\n", i); SplinePoint startingSplinePoint = points.at(i); SplinePoint endingSplinePoint = points.at(i + 1); @@ -35,8 +35,10 @@ namespace Pronounce currentPath.addPoint(startingControlPoint); currentPath.addPoint(endingControlPoint); currentPath.addPoint(endingPoint); + + std::cout << "current path" << currentPath.to_string() << std::endl; - path += currentPath.getPath(pointGranularity).getPath(); + path += currentPath.getPath(pointGranularity); } return path; diff --git a/include/utils/splinePath.hpp b/include/utils/splinePath.hpp index ef3cacba..6c5141df 100644 --- a/include/utils/splinePath.hpp +++ b/include/utils/splinePath.hpp @@ -3,6 +3,7 @@ #include "path.hpp" #include "pointUtil.hpp" #include +#include namespace Pronounce { /** @@ -66,6 +67,17 @@ namespace Pronounce { return points.at(i); } + std::string to_string() { + std::string str = ""; + for (int i = 0; i < points.size(); i++) { + str += points.at(i).to_string(); + if (i != points.size() - 1) { + str += ", "; + } + } + return str; + } + ~SplinePath(); }; diff --git a/include/utils/utils.hpp b/include/utils/utils.hpp index 7bc1f786..bf6f5e83 100644 --- a/include/utils/utils.hpp +++ b/include/utils/utils.hpp @@ -4,35 +4,49 @@ #include #include #include +#include namespace Pronounce { - double toRadians(double degrees); - double toDegrees(double radians); - double angleDifference(double angle1, double angle2); - double signnum_c(double x); - - /** - * @brief A function like processing's map function. Maps a value from one range to another. - * - * @param value Value to map - * @param start1 Start of input range - * @param stop1 End of input range - * @param start2 Start of output range - * @param stop2 End of output range - * @return double Mapped value - */ - double map(double value, double start1, double stop1, double start2, double stop2); - - template - std::string string_format(const std::string& format, Args ... args) { - int size_s = std::snprintf(nullptr, 0, format.c_str(), args ...) + 1; // Extra space for '\0' - if (size_s <= 0) { throw std::runtime_error("Error during formatting."); } - auto size = static_cast(size_s); - auto buf = std::make_unique(size); - std::snprintf(buf.get(), size, format.c_str(), args ...); - return std::string(buf.get(), buf.get() + size - 1); // We don't want the '\0' inside - } + double toRadians(double degrees); + double toDegrees(double radians); + double angleDifference(double angle1, double angle2); + double signnum_c(double x); + + /** + * @brief A function like processing's map function. Maps a value from one range to another. + * + * @param value Value to map + * @param start1 Start of input range + * @param stop1 End of input range + * @param start2 Start of output range + * @param stop2 End of output range + * @return double Mapped value + */ + double map(double value, double start1, double stop1, double start2, double stop2); + +#ifdef SIM + double clamp(double value, double min, double max) { + return std::min(std::max(value, min), max); + } + + double max(double a, double b) { + return a > b ? a : b; + } +#else + using std::max; + using std::clamp; +#endif // SIM + + template + std::string string_format(const std::string& format, Args ... args) { + int size_s = std::snprintf(nullptr, 0, format.c_str(), args ...) + 1; // Extra space for '\0' + if (size_s <= 0) { throw std::runtime_error("Error during formatting."); } + auto size = static_cast(size_s); + auto buf = std::make_unique(size); + std::snprintf(buf.get(), size, format.c_str(), args ...); + return std::string(buf.get(), buf.get() + size - 1); // We don't want the '\0' inside + } } // namespace Pronounce diff --git a/pathTest.cpp b/pathTest.cpp index 0d27de31..979e5bc9 100644 --- a/pathTest.cpp +++ b/pathTest.cpp @@ -1,11 +1,13 @@ -#include +#include #include -#include -#include +#include +#include #include +#define SIM 0 + #include "include/utils/vector.hpp" #include "src/utils/vector.cpp" #include "include/utils/pointUtil.hpp" @@ -18,18 +20,52 @@ #include "src/utils/runningAverage.cpp" #include "include/utils/utils.hpp" #include "src/utils/utils.cpp" +#include "include/utils/position.hpp" +#include "src/utils/position.cpp" +#include "include/utils/purePursuitProfile.hpp" +#include "src/utils/purePursuitProfile.cpp" +#include "include/utils/purePursuitProfileManager.hpp" +#include "src/utils/purePursuitProfileManager.cpp" +#include "include/pid/pid.hpp" +#include "src/pid/pid.cpp" +#include "include/chassis/abstractDrivetrain.hpp" +#include "src/chassis/abstractDrivetrain.cpp" +#include "include/chassis/abstractTankDrivetrain.hpp" +#include "src/chassis/abstractTankDrivetrain.cpp" +#include "include/chassis/simDrivetrain.hpp" +#include "src/chassis/simDrivetrain.cpp" +#include "include/chassis/simTankDrivetrain.hpp" +#include "src/chassis/simTankDrivetrain.cpp" +#include "include/odometry/odometry.hpp" +#include "src/odometry/odometry.cpp" +#include "include/odometry/simOdometry.hpp" +#include "src/odometry/simOdometry.cpp" +#include "include/motionControl/purePursuit.hpp" +#include "src/motionControl/purePursuit.cpp" +#include "include/motionControl/tankPurePursuit.hpp" +#include "src/motionControl/tankPurePursuit.cpp" using namespace Pronounce; -#define xOffset 0 -#define yOffset 0 +int testPathIndex; +int leftAllianceToRightHomeZoneIndex; +int rightHomeZoneToRightAllianceIndex; +int rightAllianceToRightRingsIndex; + +int fps = 60; +double playbackMultiplier = 1; + +#define xOffset 30 +#define yOffset 30 #define multiplier 3 #define lookahead 10 #define starting_point_random 1 -#define PRINT_LIVE false +// ANCHOR Printing variables + +#define PRINT_LIVE true #define GRAPH true #define FIELD_WIDTH 140.6 @@ -43,6 +79,47 @@ void printRobotWithLookahead(Point point) { circle(point.getY() * multiplier, point.getX() * multiplier, lookahead * multiplier); } +void printRobot(Odometry odometry, double trackWidth) { + trackWidth /= 2; + + Position* point = odometry.getPosition(); + circle(point->getY() * multiplier, point->getX() * multiplier, 1); + circle(point->getY() * multiplier, point->getX() * multiplier, lookahead * multiplier); + + Vector forwardVector(20, point->getTheta() + M_PI_2); + + Point forwardPoint; + forwardPoint.operator=(point); + forwardPoint.add(Point(forwardVector.getCartesian().getX(), forwardVector.getCartesian().getY())); + + line(point->getY() * multiplier, point->getX() * multiplier, forwardPoint.getY() * multiplier, forwardPoint.getX() * multiplier); +} + +void printRobot(Odometry odometry, double trackWidth, double curvature) { + printRobot(odometry, trackWidth); + + setcolor(GREEN); + + double radius = 1.0/curvature; + + if (abs(radius) > 100) { + return; + } + + Vector curvatureVector(radius, odometry.getPosition()->getTheta()); + + // Make sure the radius is positive + radius = abs(radius); + + Point curvaturePoint; + curvaturePoint.operator=(odometry.getPosition()); + curvaturePoint.add(curvatureVector.getCartesian()); + + circle(curvaturePoint.getY() * multiplier, curvaturePoint.getX() * multiplier, radius * multiplier); + + setcolor(BLACK); +} + void printPath(Path path) { if (path.getPath().size() < 2) { @@ -211,52 +288,58 @@ int main() { int loopcount = 0; - // Create path - std::vector paths = std::vector(); + SimTankDrivetrain drivetrain(15.0, 15.0/fps, 60.0/fps); + SimOdometry odometry(&drivetrain); + + TankPurePursuit purePursuit(&drivetrain, &odometry, 10); + double trackWidth = drivetrain.getTrackWidth(); - // Right Steal Right - Path rightHomeToGoalNeutral; + purePursuit.setSpeed(1.0); - rightHomeToGoalNeutral.addPoint(105.7, 8); - rightHomeToGoalNeutral.addPoint(105.7, 60); + // Test path + /* + SplinePath testPath = SplinePath(); - paths.emplace_back(rightHomeToGoalNeutral); + testPath.addPoint(40, 40); + testPath.addPoint(40, 64); + testPath.addPoint(64, 64); - SplinePath rightNeutralToMidNeutral; + testPathIndex = purePursuit.addPath(testPath.getPath(0.1)); + */ - rightNeutralToMidNeutral.addPoint(105.7, 60); - rightNeutralToMidNeutral.addPoint(82.3, 40); - rightNeutralToMidNeutral.addPoint(70.3, 60); - paths.emplace_back(rightNeutralToMidNeutral.getPath(0.1)); + QuadraticSplinePath leftAllianceToRightHomeZone = QuadraticSplinePath(); - Path midNeutralToRightAlliance; + leftAllianceToRightHomeZone.addPoint(SplinePoint(Point(30.0, 11.4), Vector(15.0, M_PI_2))); + leftAllianceToRightHomeZone.addPoint(SplinePoint(Point(15.2, 35.0), Vector(15.0, 0.0))); + leftAllianceToRightHomeZone.addPoint(SplinePoint(Point(46.4, 46.4), Vector(15.0, M_PI_2))); + leftAllianceToRightHomeZone.addPoint(SplinePoint(Point(93.9, 46.4), Vector(15.0, M_PI_2))); - midNeutralToRightAlliance.addPoint(70.3, 60); - midNeutralToRightAlliance.addPoint(120.1, 36); + leftAllianceToRightHomeZoneIndex = purePursuit.addPath(leftAllianceToRightHomeZone.getPath(0.1)); - paths.emplace_back(midNeutralToRightAlliance); + QuadraticSplinePath rightHomeZoneToRightAlliance = QuadraticSplinePath(); - SplinePath rightAllianceToRightRing; + rightHomeZoneToRightAlliance.addPoint(SplinePoint(Point(93.9, 46.4), Vector(15.0, -M_PI_2))); + rightHomeZoneToRightAlliance.addPoint(SplinePoint(Point(123.0, 40.0), Vector(15.0, - M_PI_2 - M_PI_4))); - // Turn to negative 90 degrees + rightHomeZoneToRightAllianceIndex = purePursuit.addPath(rightHomeZoneToRightAlliance.getPath(0.1)); - rightAllianceToRightRing.addPoint(120.1, 36); - rightAllianceToRightRing.addPoint(117.5, 46.8); - rightAllianceToRightRing.addPoint(117.5, 70.3); - rightAllianceToRightRing.addPoint(117.5, 70.3); + QuadraticSplinePath rightAllianceToRightRings = QuadraticSplinePath(); - paths.emplace_back(rightAllianceToRightRing.getPath(0.1)); + rightAllianceToRightRings.addPoint(SplinePoint(Point(123.0, 40.0), Vector(15.0, - M_PI_2 - M_PI_4))); + rightAllianceToRightRings.addPoint(SplinePoint(Point(117.5, 46.8), Vector(0.0, 0.0))); + rightAllianceToRightRings.addPoint(SplinePoint(Point(117.5, 70.3), Vector(20.0, 0.0))); - QuadraticSplinePath rightRingToLeftHomeZone; + rightAllianceToRightRingsIndex = purePursuit.addPath(rightAllianceToRightRings.getPath(0.1)); - rightRingToLeftHomeZone.addPoint(SplinePoint(Point(117.5, 70.3), Vector(50, M_PI_2))); - rightRingToLeftHomeZone.addPoint(SplinePoint(Point(70.3, 35), Vector(50, M_PI_2))); + QuadraticSplinePath rightRingsToRightHomeZone = QuadraticSplinePath(); - paths.emplace_back(rightRingToLeftHomeZone.getPath(0.1)); + rightAllianceToRightRings.addPoint(SplinePoint(Point(117.5, 70.3), Vector(20.0, M_PI))); + rightAllianceToRightRings.addPoint(SplinePoint(Point(85, 36), Vector(20.0, M_PI_2))); + + rightAllianceToRightRingsIndex = purePursuit.addPath(rightAllianceToRightRings.getPath(0.1)); - srand(std::chrono::duration_cast(std::chrono::system_clock::now().time_since_epoch()).count()); #if GRAPH int gd = DETECT, gm; @@ -268,48 +351,36 @@ int main() { delay(1000); //printPath(path); #endif - // Set random robot starting position - Point firstPosition = paths.at(0).getPath().at(0); + Position startingPosition(30.0, 11.4, -M_PI_2); - firstPosition.setX(firstPosition.getX() + ((rand() / 1073741823) - 1) * starting_point_random); - firstPosition.setY(firstPosition.getY() + ((rand() / 1073741823) - 1) * starting_point_random); - Point robot = firstPosition; - Vector robotMovement = Vector(); + drivetrain.reset(&startingPosition); #if GRAPH std::vector robotPositions; - robotPositions.emplace_back(paths.at(0).getPath().at(0)); + robotPositions.emplace_back(startingPosition); #endif - // Use a running average to estimate momentum - const int runningAverageLength = 10; + purePursuit.setEnabled(true); - RunningAverage runningAverageX; - RunningAverage runningAverageY; + double curvature = 0.1; // Loop through paths - for (int i = 0; i < paths.size(); i++) { + for (int i = 0; i < purePursuit.getPaths().size(); i++) { + + std::vector pathVector = purePursuit.getPath(i).getPath(); + Path path = purePursuit.getPath(i); - std::vector pathVector = paths.at(i).getPath(); - Path path = paths.at(i); + purePursuit.setCurrentPathIndex(i); + purePursuit.setFollowing(true); // Loop until you get to the last point - while (pathVector.at(pathVector.size() - 1).distance(robot) > 1) { + while (!purePursuit.isDone(1)) { loopcount++; - // Get the lookahead point - Point lookaheadPoint = path.getLookAheadPoint(robot, lookahead); - robotMovement = Vector(&robot, &lookaheadPoint); - if (robotMovement.getMagnitude() > 1) { - robotMovement.normalize(); - } - robotMovement = robotMovement.scale(1); - runningAverageX.add(robotMovement.getCartesian().getX()); - runningAverageY.add(robotMovement.getCartesian().getY()); - - // Move robot - Point runningAveragePosition(runningAverageX.getAverage(), runningAverageY.getAverage()); - robot += runningAveragePosition; + odometry.update(); + // drivetrain.driveCurvature(-1.0, (1.0 / 24.0)); + purePursuit.update(); + drivetrain.update(); #if GRAPH #if PRINT_LIVE @@ -320,30 +391,33 @@ int main() { printPath(robotPositions); - robotPositions.emplace_back(robot); + robotPositions.emplace_back(Point(odometry.getPosition()->getX(), odometry.getPosition()->getY())); setlinestyle(DASHED_LINE, 5, 2); - for (int i = 0; i < paths.size(); i++) { - printPath(paths.at(i).getPath()); + for (int i = 0; i < purePursuit.getPaths().size(); i++) { + printPath(purePursuit.getPath(i).getPath()); } setcolor(LIGHTGRAY); setlinestyle(SOLID_LINE, 5, 2); - line(robot.getY() * multiplier, robot.getX() * multiplier, lookaheadPoint.getY() * multiplier, lookaheadPoint.getX() * multiplier); - printRobotWithLookahead(robot); + line(odometry.getPosition()->getY() * multiplier, odometry.getPosition()->getX() * multiplier, purePursuit.getPointData().lookaheadPoint.getY() * multiplier, purePursuit.getPointData().lookaheadPoint.getX() * multiplier); + printRobot(odometry, 15, purePursuit.getPointData().curvature); - delay(3); + delay(1000/fps/playbackMultiplier); #endif // PRINT_LIVE - robotPositions.emplace_back(robot); + robotPositions.emplace_back(Point(odometry.getPosition()->getX(), odometry.getPosition()->getY())); #endif - if (loopcount > 1000*paths.size()) { + + if (loopcount >= 5*fps * purePursuit.getPaths().size() || (odometry.getPosition()->getX() < 0 || odometry.getPosition()->getX() > 144 || odometry.getPosition()->getY() < 0 || odometry.getPosition()->getY() > 144)) { break; } } } #if GRAPH + cleardevice(); + printField(); // printRobotWithLookahead(robot); @@ -351,19 +425,22 @@ int main() { setlinestyle(DASHED_LINE, 5, 2); // Print all paths in the vector paths - for (int i = 0; i < paths.size(); i++) { - printPath(paths.at(i).getPath()); + for (int i = 0; i < purePursuit.getPaths().size(); i++) { + printPath(purePursuit.getPaths().at(i).getPath()); } - robotPositions.emplace_back(robot); + robotPositions.emplace_back(Point(odometry.getPosition()->getX(), odometry.getPosition()->getY())); setlinestyle(SOLID_LINE, 5, 2); setcolor(LIGHTGRAY); printPath(robotPositions); + line(odometry.getPosition()->getY() * multiplier, odometry.getPosition()->getX() * multiplier, purePursuit.getPointData().lookaheadPoint.getY() * multiplier, purePursuit.getPointData().lookaheadPoint.getX() * multiplier); + printRobot(odometry, 15, purePursuit.getPointData().curvature); + // Print loopcount - std::cout << "Loopcount: " << loopcount << std::endl; + std::cout << "Time: " << loopcount/(double) fps << std::endl; delay(500000); closegraph(); diff --git a/runPathTest.sh b/runPathTest.sh index 828be293..c1aa1044 100755 --- a/runPathTest.sh +++ b/runPathTest.sh @@ -1,2 +1,2 @@ -g++ pathTest.cpp -opathTest -lgraph -Iinclude/utils/ +g++ pathTest.cpp -opathTest -lgraph -Iinclude/ -Iinclude/utils/ -Iinclude/chassis/ -Iinclude/odometry/ -Iinclude/motionControl/ -Iinclude/pid/ ./pathTest \ No newline at end of file diff --git a/src/chassis/abstractDrivetrain.cpp b/src/chassis/abstractDrivetrain.cpp new file mode 100644 index 00000000..8518a9fc --- /dev/null +++ b/src/chassis/abstractDrivetrain.cpp @@ -0,0 +1,9 @@ +#include "abstractDrivetrain.hpp" + +namespace Pronounce { + AbstractDrivetrain::AbstractDrivetrain() { + } + + AbstractDrivetrain::~AbstractDrivetrain() { + } +} // namespace Pronounce diff --git a/src/chassis/abstractTankDrivetrain.cpp b/src/chassis/abstractTankDrivetrain.cpp new file mode 100644 index 00000000..6196e801 --- /dev/null +++ b/src/chassis/abstractTankDrivetrain.cpp @@ -0,0 +1,13 @@ +#include "abstractTankDrivetrain.hpp" + +namespace Pronounce { + AbstractTankDrivetrain::AbstractTankDrivetrain() : AbstractTankDrivetrain(0.0) { + } + + AbstractTankDrivetrain::AbstractTankDrivetrain(double trackWidth) : AbstractDrivetrain() { + this->trackWidth = trackWidth; + } + + AbstractTankDrivetrain::~AbstractTankDrivetrain() { + } +} // namespace Pronounce diff --git a/src/chassis/drivetrain.cpp b/src/chassis/drivetrain.cpp index 2258e08b..05e9d6b6 100644 --- a/src/chassis/drivetrain.cpp +++ b/src/chassis/drivetrain.cpp @@ -2,39 +2,53 @@ namespace Pronounce { - Drivetrain::Drivetrain() { - + Drivetrain::Drivetrain() : AbstractDrivetrain() { + + } + + Drivetrain::Drivetrain(pros::Motor* frontLeft, pros::Motor* frontRight, pros::Motor* backLeft, pros::Motor* backRight, pros::Imu* imu) : AbstractDrivetrain() { + std::vector leftMotors; + std::vector rightMotors; + leftMotors.emplace_back(frontLeft); + leftMotors.emplace_back(backLeft); + rightMotors.emplace_back(frontRight); + rightMotors.emplace_back(backRight); + this->leftMotors = MotorGroup(leftMotors); + this->rightMotors = MotorGroup(rightMotors); + this->imu = imu; } - Drivetrain::Drivetrain(pros::Motor* frontLeft, pros::Motor* frontRight, pros::Motor* backLeft, pros::Motor* backRight, pros::Imu* imu) { + Drivetrain::Drivetrain(pros::Motor* frontLeft, pros::Motor* frontRight, pros::Motor* midLeft, pros::Motor* midRight, pros::Motor* backLeft, pros::Motor* backRight, pros::Imu* imu) : AbstractDrivetrain() { std::vector leftMotors; std::vector rightMotors; leftMotors.emplace_back(frontLeft); + leftMotors.emplace_back(midLeft); leftMotors.emplace_back(backLeft); rightMotors.emplace_back(frontRight); + rightMotors.emplace_back(midRight); rightMotors.emplace_back(backRight); this->leftMotors = MotorGroup(leftMotors); this->rightMotors = MotorGroup(rightMotors); - this->imu = imu; - } + this->imu = imu; + } - Drivetrain::Drivetrain(MotorGroup leftMotors, MotorGroup rightMotors, pros::Imu* imu) { + Drivetrain::Drivetrain(MotorGroup leftMotors, MotorGroup rightMotors, pros::Imu* imu) : AbstractDrivetrain() { this->leftMotors = leftMotors; this->rightMotors = rightMotors; this->imu = imu; } - double Drivetrain::getTemp() { - double total = this->leftMotors.get_temperature() + + double Drivetrain::getTemp() { + double total = this->leftMotors.get_temperature() + this->rightMotors.get_temperature(); - return total / 2.0; - } - - double Drivetrain::getSpeed() { - double total = this->leftMotors.get_actual_velocity() + + return total / 2.0; + } + + double Drivetrain::getSpeed() { + double total = this->leftMotors.get_actual_velocity() + this->rightMotors.get_actual_velocity(); - return total / 2.0; - } + return total / 2.0; + } } // namespace Pronounce diff --git a/src/chassis/simDrivetrain.cpp b/src/chassis/simDrivetrain.cpp new file mode 100644 index 00000000..9128946e --- /dev/null +++ b/src/chassis/simDrivetrain.cpp @@ -0,0 +1,15 @@ +#include "simDrivetrain.hpp" + +namespace Pronounce { + + SimDrivetrain::SimDrivetrain() { + this->position = new Position(); + } + + SimDrivetrain::SimDrivetrain(Position* position) { + this->position = position; + } + + SimDrivetrain::~SimDrivetrain() { + } +} // namespace Pronounce diff --git a/src/chassis/simTankDrivetrain.cpp b/src/chassis/simTankDrivetrain.cpp new file mode 100644 index 00000000..342336c7 --- /dev/null +++ b/src/chassis/simTankDrivetrain.cpp @@ -0,0 +1,50 @@ +#include "simTankDrivetrain.hpp" + +namespace Pronounce { + + SimTankDrivetrain::SimTankDrivetrain() : SimTankDrivetrain(0.0, 0.0, 0.0) {} + + SimTankDrivetrain::SimTankDrivetrain(double trackWidth) : SimTankDrivetrain(trackWidth, 0.0, 0.0) {} + + SimTankDrivetrain::SimTankDrivetrain(double trackWidth, double maxAcceleration, double maxSpeed) : SimTankDrivetrain(trackWidth, maxAcceleration, maxSpeed, new Position(0.0, 0.0, 0.0)) {} + + SimTankDrivetrain::SimTankDrivetrain(double trackWidth, double maxAcceleration, double maxSpeed, Position* position) : SimDrivetrain(position), AbstractTankDrivetrain(trackWidth) { + this->maxAcceleration = maxAcceleration; + this->maxSpeed = maxSpeed; + } + + void SimTankDrivetrain::update() { + + Position* oldPosition = this->getPosition(); + + // Update the wheel velocities + double leftChange = clamp(leftVelocityTarget - leftVelocity, -maxAcceleration, maxAcceleration); + double rightChange = clamp(rightVelocityTarget - rightVelocity, -maxAcceleration, maxAcceleration); + + leftVelocity = clamp(leftVelocity + leftChange, -maxSpeed, maxSpeed); + rightVelocity = clamp(rightVelocity + rightChange, -maxSpeed, maxSpeed); + + // Calculate the local offset + double offset = (leftVelocity + rightVelocity) / 2.0; + double angle = (rightVelocity - leftVelocity) / this->getTrackWidth(); + + leftDistance += leftVelocity; + rightDistance += rightVelocity; + + double relativeAngle = ((leftDistance - rightDistance) / this->getTrackWidth()) + this->getResetOrientation(); + + // Calculate a vector + Vector localOffset(offset, relativeAngle+M_PI_2); + + Position* newPosition = new Position(); + newPosition->operator=(oldPosition); + + newPosition->add(localOffset.getCartesian()); + newPosition->setTheta(relativeAngle - (angle / 2.0)); + + this->setPosition(newPosition); + } + + SimTankDrivetrain::~SimTankDrivetrain() { + } +} // namespace Pronounce diff --git a/src/chassis/tankDrive.cpp b/src/chassis/tankDrive.cpp index d270c3ed..c605427d 100644 --- a/src/chassis/tankDrive.cpp +++ b/src/chassis/tankDrive.cpp @@ -5,37 +5,16 @@ 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) { } - TankDrivetrain::TankDrivetrain(pros::Motor* frontLeft, pros::Motor* frontRight, pros::Motor* backLeft, pros::Motor* backRight, pros::Imu* imu, double trackWidth) : Drivetrain(frontLeft, frontRight, backLeft, backRight, imu) { - this->trackWidth = trackWidth; + TankDrivetrain::TankDrivetrain(pros::Motor* frontLeft, pros::Motor* frontRight, pros::Motor* backLeft, pros::Motor* backRight, pros::Imu* imu, double trackWidth) : Drivetrain(frontLeft, frontRight, backLeft, backRight, imu), AbstractTankDrivetrain(trackWidth) { + } - TankDrivetrain::TankDrivetrain(pros::Motor* frontLeft, pros::Motor* frontRight, pros::Motor* midLeft, pros::Motor* midRight, pros::Motor* backLeft, pros::Motor* backRight, pros::Imu* imu) { - std::vector leftMotors; - std::vector rightMotors; - leftMotors.emplace_back(frontLeft); - leftMotors.emplace_back(midLeft); - leftMotors.emplace_back(backLeft); - rightMotors.emplace_back(frontRight); - rightMotors.emplace_back(midRight); - rightMotors.emplace_back(backRight); - this->setLeftMotors(MotorGroup(leftMotors)); - this->setRightMotors(MotorGroup(rightMotors)); - this->setImu(imu); + TankDrivetrain::TankDrivetrain(pros::Motor* frontLeft, pros::Motor* frontRight, pros::Motor* midLeft, pros::Motor* midRight, pros::Motor* backLeft, pros::Motor* backRight, pros::Imu* imu) : Drivetrain(frontLeft, frontRight, midLeft, midRight, backLeft, backRight, imu) { + } - TankDrivetrain::TankDrivetrain(pros::Motor* frontLeft, pros::Motor* frontRight, pros::Motor* midLeft, pros::Motor* midRight, pros::Motor* backLeft, pros::Motor* backRight, pros::Imu* imu, double trackWidth) { - std::vector leftMotors; - std::vector rightMotors; - leftMotors.emplace_back(frontLeft); - leftMotors.emplace_back(midLeft); - leftMotors.emplace_back(backLeft); - rightMotors.emplace_back(frontRight); - rightMotors.emplace_back(midRight); - rightMotors.emplace_back(backRight); - this->setLeftMotors(MotorGroup(leftMotors)); - this->setRightMotors(MotorGroup(rightMotors)); - this->setImu(imu); - this->trackWidth = trackWidth; + TankDrivetrain::TankDrivetrain(pros::Motor* frontLeft, pros::Motor* frontRight, pros::Motor* midLeft, pros::Motor* midRight, pros::Motor* backLeft, pros::Motor* backRight, pros::Imu* imu, double trackWidth) : Drivetrain(frontLeft, frontRight, midLeft, midRight, backLeft, backRight, imu), AbstractTankDrivetrain(trackWidth) { + } TankDrivetrain::~TankDrivetrain() { diff --git a/src/main.cpp b/src/main.cpp index b1c9cde6..43ed66ed 100644 --- a/src/main.cpp +++ b/src/main.cpp @@ -38,7 +38,7 @@ ThreeWheelOdom odometry(&leftOdomWheel, &rightOdomWheel, &backOdomWheel); TankDrivetrain drivetrain(&frontLeftMotor, &frontRightMotor, &midLeftMotor, &midRightMotor, &backLeftMotor, &backRightMotor, &imu, 15.0); -Pronounce::TankPurePursuit purePursuit(&drivetrain, &odometry, 10); +Pronounce::TankPurePursuit purePursuit(&drivetrain, &odometry, 20); MotorButton liftButton(&master, &lift, DIGITAL_L1, DIGITAL_L2, 200, 0, -200, 0, 0); MotorButton intakeButton(&master, &intake, DIGITAL_R2, DIGITAL_Y, 200, 0, -100, 0, 0); @@ -99,7 +99,7 @@ int rightStealRight() { purePursuit.setFollowing(true); // Wait until it is done - while (!purePursuit.isDone(0.5)) { + while (!purePursuit.isDone(2)) { pros::Task::delay(50); } @@ -113,11 +113,11 @@ int rightStealRight() { purePursuit.setFollowing(true); // Wait until it is done - while (!purePursuit.isDone(0.5)) { + while (!purePursuit.isDone(2)) { pros::Task::delay(50); } - backGrabberButton.setButtonStatus(ButtonStatus::NEUTRAL); + backGrabberButton.setButtonStatus(ButtonStatus::POSITIVE); pros::Task::delay(500); purePursuit.setCurrentPathIndex(midNeutralToMidHomeZoneIndex); @@ -125,7 +125,7 @@ int rightStealRight() { purePursuit.setFollowing(true); // Wait until it is done - while (!purePursuit.isDone(0.5)) { + while (!purePursuit.isDone(2)) { pros::Task::delay(50); } @@ -139,7 +139,7 @@ int rightAwpRight() { purePursuit.setFollowing(true); // Wait until it is done - while (!purePursuit.isDone(0.5)) { + while (!purePursuit.isDone(2)) { pros::Task::delay(50); } @@ -152,7 +152,7 @@ int rightAwpRight() { purePursuit.setFollowing(true); // Wait until it is done - while (!purePursuit.isDone(0.5)) { + while (!purePursuit.isDone(2)) { pros::Task::delay(50); } @@ -170,7 +170,7 @@ int leftAwpLeft() { purePursuit.setTurnTarget(0); // Wait until it is done - while (!purePursuit.isDone(0.5)) { + while (!purePursuit.isDone(2)) { pros::Task::delay(50); } @@ -183,7 +183,7 @@ int leftAwpLeft() { purePursuit.setTurnTarget(3.14); // Wait until it is done - while (!purePursuit.isDone(0.5)) { + while (!purePursuit.isDone(2)) { pros::Task::delay(50); } @@ -195,7 +195,7 @@ int leftAwpLeft() { purePursuit.setTurnTarget(M_PI_2); // Wait until it is done - while (!purePursuit.isDone(0.5)) { + while (!purePursuit.isDone(2)) { pros::Task::delay(50); } @@ -213,7 +213,7 @@ int skills() { purePursuit.setTurnTarget(0); // Wait until it is done - while (!purePursuit.isDone(0.5)) { + while (!purePursuit.isDone(2)) { pros::Task::delay(50); } @@ -227,7 +227,7 @@ int skills() { liftButton.setAutonomousAuthority(1500); // Wait until it is done - while (!purePursuit.isDone(0.5)) { + while (!purePursuit.isDone(2)) { pros::Task::delay(50); } @@ -250,7 +250,7 @@ int skills() { liftButton.setAutonomousAuthority(1500); // Wait until done - while (!purePursuit.isDone(0.5)) { + while (!purePursuit.isDone(2)) { pros::Task::delay(50); } @@ -273,7 +273,7 @@ int skills() { liftButton.setAutonomousAuthority(1500); // Wait until done - while (!purePursuit.isDone(0.5)) { + while (!purePursuit.isDone(2)) { pros::Task::delay(50); } @@ -285,7 +285,7 @@ int skills() { purePursuit.setFollowing(true); // Wait until it is done - while (!purePursuit.isDone(0.5)) { + while (!purePursuit.isDone(2)) { pros::Task::delay(50); } @@ -397,9 +397,9 @@ void initDrivetrain() { printf("Init drivetrain"); // odometry.setUseImu(true); - leftOdomWheel.setRadius(3.25/2); + leftOdomWheel.setRadius(3.25 / 2); leftOdomWheel.setTuningFactor(1); - rightOdomWheel.setRadius(3.25/2); + rightOdomWheel.setRadius(3.25 / 2); rightOdomWheel.setTuningFactor(1); backOdomWheel.setRadius(1.25); backOdomWheel.setTuningFactor(1); @@ -415,6 +415,7 @@ void initDrivetrain() { odometry.setMaxMovement(1); purePursuit.setNormalizeDistance(10); + purePursuit.setSpeed(250); pros::Task purePursuitTask = pros::Task(updateDrivetrain, "Pure Pursuit"); @@ -496,7 +497,7 @@ void initialize() { initSensors(); initMotors(); initDrivetrain(); - autoPaths(purePursuit); + autoPaths(&purePursuit); initController(); initLogger(); // initSelector(); diff --git a/src/motionControl/purePursuit.cpp b/src/motionControl/purePursuit.cpp index 05db9521..427226bc 100644 --- a/src/motionControl/purePursuit.cpp +++ b/src/motionControl/purePursuit.cpp @@ -3,6 +3,7 @@ namespace Pronounce { PurePursuit::PurePursuit() { this->setPurePursuitProfileManager(PurePursuitProfileManager()); + this->paths = std::vector(); this->odometry = new Odometry(); } @@ -12,6 +13,7 @@ namespace Pronounce { PurePursuit::PurePursuit(Odometry* odometry, double lookahead) { this->setPurePursuitProfileManager(PurePursuitProfileManager()); + this->paths = std::vector(); this->odometry = odometry; this->lookahead = lookahead; } @@ -38,18 +40,19 @@ namespace Pronounce { // Map the magnitude to the distance from the lookahead distance to make sure that the robot's // PID controller behaves the same for different lookahead paths double magnitude = lookaheadVector.getMagnitude(); - double mappedMagnitude = std::clamp(map(magnitude, 0, lookahead, 0, normalizeDistance), 0.0, normalizeDistance); + double mappedMagnitude = clamp(map(magnitude, 0, lookahead, 0, normalizeDistance), 0.0, normalizeDistance); Vector normalizedLookaheadVector = Vector(mappedMagnitude, lookaheadVector.getAngle()); - Vector robotRelativeLookaheadVector = lookaheadVector; + Vector robotRelativeLookaheadVector(¤tPoint, &lookaheadPoint); - robotRelativeLookaheadVector.rotate(-currentPosition->getTheta()); + robotRelativeLookaheadVector.setAngle(robotRelativeLookaheadVector.getAngle() - currentPosition->getTheta()); double xDistance = robotRelativeLookaheadVector.getCartesian().getX(); double signedCurvature = (2 * xDistance) / pow(lookaheadVector.getMagnitude(), 2); this->pointData.lookaheadPoint = lookaheadPoint; this->pointData.lookaheadVector = lookaheadVector; + this->pointData.localLookaheadVector = robotRelativeLookaheadVector; this->pointData.normalizedLookaheadVector = normalizedLookaheadVector; this->pointData.curvature = signedCurvature; } diff --git a/src/motionControl/tankPurePursuit.cpp b/src/motionControl/tankPurePursuit.cpp index 6ed2e784..37116e1f 100644 --- a/src/motionControl/tankPurePursuit.cpp +++ b/src/motionControl/tankPurePursuit.cpp @@ -1,34 +1,46 @@ #include "tankPurePursuit.hpp" namespace Pronounce { - TankPurePursuit::TankPurePursuit(TankDrivetrain* drivetrain) : PurePursuit() { + TankPurePursuit::TankPurePursuit(AbstractTankDrivetrain* drivetrain) : PurePursuit() { this->drivetrain = drivetrain; } - TankPurePursuit::TankPurePursuit(TankDrivetrain* drivetrain, double lookaheadDistance) : PurePursuit(lookaheadDistance) { + TankPurePursuit::TankPurePursuit(AbstractTankDrivetrain* drivetrain, double lookaheadDistance) : PurePursuit(lookaheadDistance) { this->drivetrain = drivetrain; } - TankPurePursuit::TankPurePursuit(TankDrivetrain* drivetrain, Odometry* odometry, double lookaheadDistance) : PurePursuit(odometry, lookaheadDistance) { + TankPurePursuit::TankPurePursuit(AbstractTankDrivetrain* drivetrain, Odometry* odometry, double lookaheadDistance) : PurePursuit(odometry, lookaheadDistance) { this->drivetrain = drivetrain; } void TankPurePursuit::updateDrivetrain() { + if (!this->isEnabled()) { + return; + } + if (isDone(this->getStopDistance())) { return; } PurePursuitPointData pointData = this->getPointData(); - printf("Curvature pointdata: %f\n", this->getPointData().curvature); - - // Drive backwards - if (inverted) { - pointData.curvature = -pointData.curvature; - drivetrain->tankSteerVelocity(-speed * ((2 + pointData.curvature * this->drivetrain->getTrackWidth()) / 2), -speed * ((2 - pointData.curvature * this->drivetrain->getTrackWidth()) / 2)); - } else { - drivetrain->tankSteerVelocity(speed * ((2 + pointData.curvature * this->drivetrain->getTrackWidth()) / 2), speed * ((2 - pointData.curvature * this->drivetrain->getTrackWidth()) / 2)); - } + + double side = sqrt(abs(clamp(pointData.localLookaheadVector.getCartesian().getY() / this->getLookahead(), -1.0, 1.0))) * signum_c(pointData.localLookaheadVector.getCartesian().getY()); + + side = abs(side) < 0.5 ? signum_c(side) : side; + + // Redundant scalar, could be used in the future + // Found that using the Y values got the same affect when this was wanted and a better affect when it wasn't + // Using the same equation with Y instead. + // Time difference on test path(In sim) + // Before change: 3.4667 + // After change: 3.08 + // 12% improvement, mostly in speed up and slow down. + double scalar = 1; // pointData.lookaheadVector.getMagnitude() / this->getLookahead(); + + double speed = clamp(this->getSpeed() * side * scalar, -this->getSpeed(), this->getSpeed()); + + drivetrain->driveCurvature(speed, pointData.curvature); } void TankPurePursuit::stop() { diff --git a/src/odometry/simOdometry.cpp b/src/odometry/simOdometry.cpp new file mode 100644 index 00000000..39cb5505 --- /dev/null +++ b/src/odometry/simOdometry.cpp @@ -0,0 +1,23 @@ +#include "simOdometry.hpp" + +namespace Pronounce { + SimOdometry::SimOdometry() { + this->drivetrain = new SimDrivetrain(); + this->reset(new Position()); + } + + SimOdometry::SimOdometry(SimDrivetrain* drivetrain) { + this->drivetrain = drivetrain; + this->reset(new Position()); + } + + void SimOdometry::update() { + Position position; + position.operator=(this->getDrivetrain()->getPosition()); + position.operator+=(this->getResetPosition()); + this->getPosition()->operator=(this->getDrivetrain()->getPosition()); + } + + SimOdometry::~SimOdometry() { + } +} // namespace Pronounce diff --git a/src/odometry/threeWheelOdom.cpp b/src/odometry/threeWheelOdom.cpp index 645be896..d07af137 100644 --- a/src/odometry/threeWheelOdom.cpp +++ b/src/odometry/threeWheelOdom.cpp @@ -28,8 +28,8 @@ namespace Pronounce { // Calculate the change in orientation double lastAngle = lastPosition->getTheta(); - double currentAngle = this->getResetPosition()->getTheta() + (leftWheel->getPosition() - rightWheel->getPosition()) / (rightOffset + rightOffset); - double angleChange = angleDifference(currentAngle, lastAngle); // fmod(currentAngle + M_PI * 2, M_PI * 2) - fmod(lastAngle + M_PI * 2, M_PI * 2); + double currentAngle = this->getResetPosition()->getTheta() + (leftWheel->getPosition() - rightWheel->getPosition()) / (leftOffset + rightOffset); + double angleChange = angleDifference(currentAngle, lastAngle); double averageOrientation = lastAngle + (angleChange / 2); // Calculate the local offset then translate it to the global offset @@ -46,10 +46,6 @@ namespace Pronounce { return; } - // Print last position - printf("Left Change: %f, Right change: %f, Back change %f\n", deltaLeft, deltaRight, deltaBack); - printf("Last position: %f, %f, %f\n", lastPosition->getX(), lastPosition->getY(), lastPosition->getTheta()); - // Update the position this->setPosition(lastPosition); } diff --git a/src/utils/vector.cpp b/src/utils/vector.cpp index d747bfa9..a881df9e 100644 --- a/src/utils/vector.cpp +++ b/src/utils/vector.cpp @@ -7,6 +7,10 @@ namespace Pronounce { } Vector::Vector(double magnitude, double angle) { + if (magnitude < 0) { + this->magnitude = -magnitude; + this->angle = angle + M_PI; + } this->magnitude = magnitude; this->angle = angle; }