diff --git a/DrivingChassis.cpp b/DrivingChassis.cpp index 49ac2a5..aa8a339 100644 --- a/DrivingChassis.cpp +++ b/DrivingChassis.cpp @@ -7,6 +7,7 @@ #include "DrivingChassis.h" + /** * Compute a delta in wheel angle to traverse a specific distance * @@ -56,7 +57,11 @@ DrivingChassis::~DrivingChassis() { */ DrivingChassis::DrivingChassis(PIDMotor * left, PIDMotor * right, float wheelTrackMM, float wheelRadiusMM,GetIMU * imu) { - + myleft = left; + myright = right; + mywheelTrackMM = wheelTrackMM; + mywheelRadiusMM = wheelRadiusMM; + IMU = imu; } /** @@ -69,7 +74,54 @@ DrivingChassis::DrivingChassis(PIDMotor * left, PIDMotor * right, * @note pidmotorInstance->overrideCurrentPosition(0); can be used to "zero out" the motor to * allow for relative moves. Otherwise the motor is always in ABSOLUTE mode */ -void DrivingChassis::driveForward(float mmDistanceFromCurrent, int msDuration) { +void DrivingChassis::driveForwardFromInterpolation(float mmDistanceFromCurrent, int msDuration) { + // We should also have a "drive straight" method that uses the IMU to actually drive straight + // Although we'll have a linefollow state so... idk maybe not + + myleft -> overrideCurrentPosition(0); + myright -> overrideCurrentPosition(0); + myleft -> startInterpolationDegrees(mmDistanceFromCurrent * MM_TO_WHEEL_DEGREES, msDuration, LIN); + myright -> startInterpolationDegrees(-mmDistanceFromCurrent * MM_TO_WHEEL_DEGREES, msDuration, LIN); +} + +void DrivingChassis::driveForward(float mmDistanceFromCurrent, int msDuration){ + // if we're not performing an action + // start a timer, reset encoders + startTimeOfMovement_ms = millis(); + myleft -> overrideCurrentPosition(0); + myright -> overrideCurrentPosition(0); + motionSetpoint = mmDistanceFromCurrent; + timeout_ms = msDuration; + motionType = DRIVING_FORWARDS; +} + +/** + * Start a drive backwards action + * + * @param mmDistanceFromCurrent is the distance the mobile base should drive backwards + * @param msDuration is the time in miliseconds that the drive action should take + * + * @note this function is fast-return and should not block +*/ +void DrivingChassis::driveBackwardsFromInterpolation(float mmDistanceFromCurrent, int msDuration) { + // We should also have a "drive straight" method that uses the IMU to actually drive straight + // Although we'll have a linefollow state so... idk maybe not + myleft -> overrideCurrentPosition(0); + myright -> overrideCurrentPosition(0); + + myleft -> startInterpolationDegrees(-mmDistanceFromCurrent * MM_TO_WHEEL_DEGREES, msDuration, LIN); + myright -> startInterpolationDegrees(mmDistanceFromCurrent * MM_TO_WHEEL_DEGREES, msDuration, LIN); +} + +void DrivingChassis::driveBackwards(float mmDistanceFromCurrent, int msDuration){ + // if we're not performing an action + // start a timer, reset encoders + startTimeOfMovement_ms = millis(); + myleft -> overrideCurrentPosition(0); + myright -> overrideCurrentPosition(0); + motionSetpoint = mmDistanceFromCurrent; + timeout_ms = msDuration; + motionType = DRIVING_BACKWARDS; } /** @@ -87,8 +139,18 @@ void DrivingChassis::driveForward(float mmDistanceFromCurrent, int msDuration) { * @note pidmotorInstance->overrideCurrentPosition(0); can be used to "zero out" the motor to * allow for relative moves. Otherwise the motor is always in ABSOLUTE mode */ -void DrivingChassis::turnDegrees(float degreesToRotateBase, int msDuration) { +void DrivingChassis::turnDegreesFromInterpolation(float degreesToRotateBase, int msDuration) { + myleft -> overrideCurrentPosition(0); + myright -> overrideCurrentPosition(0); + myleft -> startInterpolationDegrees(degreesToRotateBase * WHEEL_DEGREES_TO_BODY_DEGREES, msDuration, LIN); + myright -> startInterpolationDegrees(degreesToRotateBase * WHEEL_DEGREES_TO_BODY_DEGREES, msDuration, LIN); +} +void DrivingChassis::turnToHeading(float degreesToRotateBase, int msDuration){ + motionSetpoint = degreesToRotateBase; + timeout_ms = msDuration; + startTimeOfMovement_ms = millis(); + motionType = TURNING; } /** @@ -98,8 +160,124 @@ void DrivingChassis::turnDegrees(float degreesToRotateBase, int msDuration) { * * @note this function is fast-return and should not block */ -bool DrivingChassis::isChassisDoneDriving() { - return false; +DrivingStatus DrivingChassis::statusOfChassisDriving() { + switch(motionType){ + + case TURNING:{ + // check for timeout + if((millis() - startTimeOfMovement_ms) > timeout_ms){ + //timeout occured. Stop the robot + Serial.println("Detected Timeout\r\n"); + stop(); + performingMovement = false; + return TIMED_OUT; + } + + float currentHeading = IMU->getEULER_azimuth(); + float headingError = - currentHeading - motionSetpoint; + float motorEffort = (turningMovementKp) * headingError; + myChassisPose.heading = -currentHeading; // - to account for what is considered a "positive" rotation + if(fabs(headingError) <= wheelMovementDeadband_deg) + { + Serial.println("Reached Setpoint\r\n"); + stop(); + return REACHED_SETPOINT; + } + else{ + if(fabs(motorEffort) > MAX_MOTOR_EFFORT_DURING_TURN) + { + if(motorEffort < 0){ + motorEffort = -MAX_MOTOR_EFFORT_DURING_TURN; + } + else if(motorEffort > 0){ + motorEffort = MAX_MOTOR_EFFORT_DURING_TURN; + } + } + myleft->setVelocityDegreesPerSecond(-motorEffort); + myright->setVelocityDegreesPerSecond(-motorEffort); + } + } + break; + + case DRIVING_FORWARDS:{ + // check for timeout + if((millis() - startTimeOfMovement_ms) > timeout_ms){ + //timeout occured. Stop the robot + Serial.println("Detected Timeout\r\n"); + stop(); + return TIMED_OUT; + } + + if(motionSetpoint != -1){ + float currentDistanceMovedRightWheel_mm = (myright -> getAngleDegrees())*WHEEL_DEGREES_TO_MM; + float rightWheelError_mm = currentDistanceMovedRightWheel_mm - motionSetpoint; + driveStraight(myChassisPose.heading, DRIVING_FORWARDS); + if((fabs(rightWheelError_mm) < wheelMovementDeadband_mm)){ + Serial.println("Reached Setpoint \r\n"); + stop(); + return REACHED_SETPOINT; + } + } + + else{ + // sets speed to 20 cm per second + driveStraight(myChassisPose.heading, DRIVING_FORWARDS); + } + } + break; + + case DRIVING_BACKWARDS:{ + // check for timeout + if((millis() - startTimeOfMovement_ms) > timeout_ms){ + //timeout occured. Stop the robot + Serial.println("Detected Timeout\r\n"); + stop(); + return TIMED_OUT; + } + + if(motionSetpoint != -1){ + float currentDistanceMovedRightWheel_mm = (myright -> getAngleDegrees())*WHEEL_DEGREES_TO_MM; + float rightWheelError_mm = - currentDistanceMovedRightWheel_mm - motionSetpoint; + driveStraight(myChassisPose.heading, DRIVING_BACKWARDS); + if((fabs(rightWheelError_mm) < wheelMovementDeadband_mm)){ + Serial.println("Reached Setpoint \r\n"); + stop(); + return REACHED_SETPOINT; + } + } + + else{ + // sets speed to 20 cm per second + driveStraight(myChassisPose.heading, DRIVING_BACKWARDS); + } + } + break; + + default: + break; + } + return GOING_TO_SETPOINT; +} + +void DrivingChassis::stop(){ + myleft -> stop(); + myright -> stop(); +} + +void DrivingChassis::driveStraight(float targetHeading, MotionType direction){ + float currentHeading = IMU->getEULER_azimuth(); + float headingError = - currentHeading - targetHeading; + float motorEffort = (turningMovementKp) * headingError; + + if(direction == DRIVING_BACKWARDS){ + myleft->setVelocityDegreesPerSecond((MAX_SPEED_MM_PER_SEC - motorEffort)*MM_TO_WHEEL_DEGREES); + myright->setVelocityDegreesPerSecond((-MAX_SPEED_MM_PER_SEC - motorEffort)*MM_TO_WHEEL_DEGREES); + } + + else if(direction == DRIVING_FORWARDS){ + myright->setVelocityDegreesPerSecond((MAX_SPEED_MM_PER_SEC - motorEffort)*MM_TO_WHEEL_DEGREES); + myleft->setVelocityDegreesPerSecond((-MAX_SPEED_MM_PER_SEC - motorEffort)*MM_TO_WHEEL_DEGREES); + } } /** * loop() diff --git a/DrivingChassis.h b/DrivingChassis.h index 35278a0..2d3a935 100644 --- a/DrivingChassis.h +++ b/DrivingChassis.h @@ -10,6 +10,35 @@ #include "src/pid/PIDMotor.h" #include "src/commands/GetIMU.h" #include "config.h" +#include "Pose.h" + +#define WHEEL_DEGREES_TO_BODY_DEGREES 4.25F +#define MM_TO_WHEEL_DEGREES 2.1174F +#define WHEEL_DEGREES_TO_MM .472277F +#define MAX_SPEED_MM_PER_SEC 150 +#define MAX_MOTOR_EFFORT_DURING_TURN 260 //300 //275 // 500 + + +/** + * @enum DrivingStatus + * States when performing an drive action. + */ +enum DrivingStatus { + REACHED_SETPOINT = 0, + TIMED_OUT = 1, + GOING_TO_SETPOINT = 2, +}; + +/** + * @enum MotionType + * States when performing an drive action. + */ +enum MotionType { + DRIVING_FORWARDS = 0, + DRIVING_BACKWARDS = 1, + TURNING = 2, +}; + /** * DrivingChassis encapsulates a 2 wheel differential steered chassis that drives around * @@ -25,8 +54,6 @@ */ class DrivingChassis { private: - PIDMotor * myleft; - PIDMotor * myright; GetIMU * IMU; float mywheelTrackMM; float mywheelRadiusMM; @@ -58,6 +85,20 @@ class DrivingChassis { */ float chassisRotationToWheelDistance(float angle); public: + // moved these over for line following + PIDMotor * myleft; + PIDMotor * myright; + bool performingMovement = false; + MotionType motionType; + unsigned long startTimeOfMovement_ms; + float wheelMovementKp = 3.5;// was 3.9 + float turningMovementKp = 21; //was 9, 11.7, 17.5 + float wheelMovementDeadband_mm = 2.5; + float wheelMovementDeadband_deg = .5; + float motionSetpoint; + float timeout_ms; + Pose myChassisPose; + virtual ~DrivingChassis(); /** @@ -73,18 +114,52 @@ class DrivingChassis { float wheelRadiusMM,GetIMU * imu); /** - * Start a drive forward action + * Start a drive backwards action using the encoders and setpoint interpolation + * + * @param mmDistanceFromCurrent is the distance the mobile base should drive backwards + * @param msDuration is the time in miliseconds that the drive action should take + * + * @note this function is fast-return and should not block + */ + void driveBackwardsFromInterpolation(float mmDistanceFromCurrent, int msDuration); + + /** + * Start a drive backwards action. Will return false when finished moving. Will stop when reached setpoint or timeout. + * Specifying a mmDistanceFromCurrent of '0' will move the robot indefinitely. + * + * @param mmDistanceFromCurrent is the distance the mobile base should drive backwards + * @param msDuration is the time in miliseconds that the drive action should take (this is a timeout) + * + * @note this function is fast-return and should not block. Whatever is calling this should repeatedly do so + * until this function returns false due to reaching the set-point or timing out. + */ + void driveBackwards(float mmDistanceFromCurrent, int msDuration); + + /** + * Start a drive forward action using the encoders and setpoint interpolation * * @param mmDistanceFromCurrent is the distance the mobile base should drive forward * @param msDuration is the time in miliseconds that the drive action should take * * @note this function is fast-return and should not block - * @note pidmotorInstance->overrideCurrentPosition(0); can be used to "zero out" the motor to - * allow for relative moves. Otherwise the motor is always in ABSOLUTE mode + */ + void driveForwardFromInterpolation(float mmDistanceFromCurrent, int msDuration); + + /** + * Start a drive forwards action. Will return false when finished moving. Will stop when reached setpoint or timeout. + * Specifying a mmDistanceFromCurrent of '0' will move the robot indefinitely. + * + * @param mmDistanceFromCurrent is the distance the mobile base should drive forward + * @param msDuration is the time in miliseconds that the drive action should take (this is a timeout) + * + * @note this function is fast-return and should not block. Whatever is calling this should repeatedly do so + * until this function returns false due to reaching the set-point or timing out. Make sure to change state when this reaches a setpoint. + * Otherwise, this will try to reach a new setpoint (imagine driving indefinitely) */ void driveForward(float mmDistanceFromCurrent, int msDuration); + /** - * Start a turn action + * Start a turn action using the encoders and setpoint interpolation * * This action rotates the robot around the center line made up by the contact points of the left and right wheels. * Positive angles should rotate to the left @@ -98,7 +173,24 @@ class DrivingChassis { * @note pidmotorInstance->overrideCurrentPosition(0); can be used to "zero out" the motor to * allow for relative moves. Otherwise the motor is always in ABSOLUTE mode */ - void turnDegrees(float degreesToRotateBase, int msDuration); + void turnDegreesFromInterpolation(float degreesToRotateBase, int msDuration); + + /** + * Start a turn action. Will return false when finished moving. Will stop when reached setpoint or timeout. + * + * This action rotates the robot around the center line made up by the contact points of the left and right wheels. + * Positive angles should rotate to the left + * + * This rotation is a positive rotation about the Z axis of the robot. + * + * @param desiredHeading is the desired angle the robot should reach + * @param msDuration is the time in miliseconds that the drive action should take (this is a timeout) + * + * @note this function is fast-return and should not block. Whatever is calling this should repeatedly do so + * until this function returns false due to reaching the set-point or timing out. + */ + void turnToHeading(float desiredHeading, int msDuration); + /** * Check to see if the chassis is performing an action * @@ -106,7 +198,15 @@ class DrivingChassis { * * @note this function is fast-return and should not block */ - bool isChassisDoneDriving(); + DrivingStatus statusOfChassisDriving(); + + /** + * Stops all motors + */ + void stop(); + + void driveStraight(float targetHeading, MotionType direction); + /** * loop() * diff --git a/LineFollower.cpp b/LineFollower.cpp new file mode 100644 index 0000000..3de635e --- /dev/null +++ b/LineFollower.cpp @@ -0,0 +1,91 @@ +/* + * LineFollower.cpp + * + * Created on: Oct 15, 2020 + * Author: gentov + */ + +#include "LineFollower.h" +#include + +#define PI 3.14159265f + +LineFollower::LineFollower(DrivingChassis* myChassis){ + this->robotChassis = myChassis; +} + +void LineFollower::lineFollowBackwards(){ + int leftSensorValue = analogRead(LEFT_LINE_SENSOR); + int rightSensorValue = analogRead(RIGHT_LINE_SENSOR); + float leftCorrection = 0; + float rightCorrection = 0; + if(leftSensorValue >= ON_BLACK && rightSensorValue>= ON_BLACK) + { + if(canCountLine){ + lineCount++; + // Mathematically speaking, this should only increment one of the following. Either + // row or column + float headingInRadians = (robotChassis->myChassisPose.heading)*(PI/180.0); + Serial.println(String(round(cos(headingInRadians))) + " " + String(round(sin(headingInRadians)))); + robotChassis->myChassisPose.currentRow += round(cos(headingInRadians)); + robotChassis->myChassisPose.currentColumn += round(sin(headingInRadians)); + canCountLine = false; + } + //Serial.println("Line Count: " + String(lineCount)); + } + + + else if(leftSensorValue >= ON_BLACK || rightSensorValue >= ON_BLACK){ + rightCorrection = (ON_BLACK - rightSensorValue)*lineFollowingKpBackwards; + leftCorrection = (leftSensorValue - ON_BLACK)*lineFollowingKpBackwards; + canCountLine = true; + } + else{ + canCountLine = true; + } + robotChassis->myleft -> setVelocityDegreesPerSecond(lineFollowingSpeedBackwards_mm_per_sec*MM_TO_WHEEL_DEGREES + leftCorrection); + robotChassis->myright -> setVelocityDegreesPerSecond(-lineFollowingSpeedForwards_mm_per_sec*MM_TO_WHEEL_DEGREES + rightCorrection); +} + +void LineFollower::lineFollowForwards(){ + int leftSensorValue = analogRead(LEFT_LINE_SENSOR); + int rightSensorValue = analogRead(RIGHT_LINE_SENSOR); + float leftCorrection = 0; + float rightCorrection = 0; + if(leftSensorValue >= ON_BLACK && rightSensorValue>= ON_BLACK) + { + if(canCountLine){ + lineCount++; + // Mathematically speaking, this should only increment one of the following. Either + // row or column. Since there are two markers for each row, we need to only count once every two markers. + float headingInRadians = (robotChassis->myChassisPose.heading)*(PI/180.0); + Serial.println(String(round(cos(headingInRadians))) + " " + String(round(sin(headingInRadians)))); + robotChassis->myChassisPose.rowCount += abs(round(cos(headingInRadians))); + robotChassis->myChassisPose.colCount += round(sin(headingInRadians)); + if(robotChassis->myChassisPose.rowCount == 2){ + robotChassis->myChassisPose.currentRow += round(cos(headingInRadians)); + robotChassis->myChassisPose.rowCount = 0; + } + robotChassis->myChassisPose.currentColumn = robotChassis->myChassisPose.colCount; + canCountLine = false; // This is meant as a line "debouncing". We don't want to catch the same line twice. + } + //Serial.println("Line Count: " + String(lineCount)); + } + + + else if(leftSensorValue >= ON_BLACK || rightSensorValue >= ON_BLACK){ + rightCorrection = (ON_BLACK - rightSensorValue)*lineFollowingKpForwards; + leftCorrection = (leftSensorValue - ON_BLACK)*lineFollowingKpForwards; + canCountLine = true; + } + else{ + canCountLine = true; + } + robotChassis->myleft -> setVelocityDegreesPerSecond(-lineFollowingSpeedForwards_mm_per_sec*MM_TO_WHEEL_DEGREES + leftCorrection); + robotChassis->myright -> setVelocityDegreesPerSecond(lineFollowingSpeedForwards_mm_per_sec*MM_TO_WHEEL_DEGREES + rightCorrection); +} + +void LineFollower::resetLineCount(){ + lineCount = 0; +} + diff --git a/LineFollower.h b/LineFollower.h new file mode 100644 index 0000000..7241c66 --- /dev/null +++ b/LineFollower.h @@ -0,0 +1,37 @@ +/* + * LineFollower.h + * + * Created on: Oct 15, 2020 + * Author: gentov + */ + +#include "DrivingChassis.h" +#include "config.h" +#ifndef LINEFOLLOWER_H_ +#define LINEFOLLOWER_H_ + + +class LineFollower{ + public: + LineFollower(DrivingChassis* myChassis); + // on black line + const int ON_BLACK = 3692;//3750; + int lineFollowingSpeedForwards_mm_per_sec = 150; + int lineFollowingSpeedBackwards_mm_per_sec = 175; + float lineFollowingKpBackwards = .002*lineFollowingSpeedBackwards_mm_per_sec; + float lineFollowingKpForwards = 1.3; //1.6 + int lineCount = 0; + DrivingChassis* robotChassis; + bool canCountLine = true; + + void lineFollowBackwards(); + void lineFollowForwards(); + void resetLineCount(); + + private: +}; + + + + +#endif /* LINEFOLLOWER_H_ */ diff --git a/NavigationRoutine.cpp b/NavigationRoutine.cpp new file mode 100644 index 0000000..b43cd67 --- /dev/null +++ b/NavigationRoutine.cpp @@ -0,0 +1,116 @@ +/* + * NavigationRoutine.cpp + * + * Created on: Oct 22, 2020 + * Author: gentov + */ +#include "NavigationRoutine.h" + +void setNavGoal(int row, int col, DrivingChassis* chassis, LineFollower* lineFollowingSensor){ + goalRow = row; + goalCol = col; + drivingChassis = chassis; + lineSensor = lineFollowingSensor; +} + +NavigationStates checkNavStatus(){ + switch(navState){ + case INITIALIZE_NAVIGATION: + Serial.println("INIT NAV"); + // if we're in the outerlane, then there really isn't a need to find the outerlane + if(drivingChassis->myChassisPose.currentColumn == 0){ + navState = TURN_TOWARDS_CORRECT_ROW; + } + else{ + navState = TURN_TOWARDS_CORRECT_COLUMN; + } + break; + case TURN_TOWARDS_CORRECT_COLUMN: + Serial.println("TURNING TOWARDS COLUMN"); + // determine what is our first state + if(drivingChassis->myChassisPose.currentRow != goalRow){ + // if our current row isn't our goal row, then we need to navigate to the outer edge + // first + /// TODO: check where we are, maybe our orientation is correct + drivingChassis->turnToHeading(90, 5000); + navStateAfterMotionSetpointReached = FINDING_OUTER_EDGE; + navState = WAIT_FOR_MOTION_SETPOINT_REACHED; + } + else{ + // otherwise, we just need to find the right column + if(drivingChassis->myChassisPose.currentColumn > goalCol){ + drivingChassis->turnToHeading(-90, 5000); + } + else{ + drivingChassis->turnToHeading(90, 5000); + } + navStateAfterMotionSetpointReached = FINDING_COLUMN; + navState = WAIT_FOR_MOTION_SETPOINT_REACHED; + } + break; + case FINDING_OUTER_EDGE: + Serial.println("FINDING COL: 0, CURRENT COL: " + String(drivingChassis->myChassisPose.currentColumn)); + // navigate until column == 0 + if(drivingChassis->myChassisPose.currentColumn != 0){ + // if the column is wrong. + lineSensor->lineFollowForwards(); + } + else{ + drivingChassis->stop(); + navState = TURN_TOWARDS_CORRECT_ROW; + } + break; + case FINDING_ROW: + Serial.println("FINDING ROW: " + String(goalRow) + "CURRENT ROW: " + String(drivingChassis->myChassisPose.currentRow)); + if(drivingChassis->myChassisPose.currentRow != goalRow){ + // if the row is wrong. + lineSensor->lineFollowForwards(); + } + else{ + drivingChassis->stop(); + if(drivingChassis->myChassisPose.currentColumn == goalCol){ + navState = FINISHED; + } + else if (goalCol != 0){ + navState = TURN_TOWARDS_CORRECT_COLUMN; + } + } + break; + case TURN_TOWARDS_CORRECT_ROW: + Serial.println("TURNING TOWARDS ROW"); + // otherwise, we just need to find the right column + if(drivingChassis->myChassisPose.currentRow > goalRow){ + drivingChassis->turnToHeading(180, 5000); + } + else{ + drivingChassis->turnToHeading(0, 5000); + } + navStateAfterMotionSetpointReached = FINDING_ROW; + navState = WAIT_FOR_MOTION_SETPOINT_REACHED; + break; + case FINDING_COLUMN: + Serial.println("FINDING COL: " + String(goalCol) + "CURRENT COL: " + String(drivingChassis->myChassisPose.currentColumn)); + if(drivingChassis->myChassisPose.currentColumn != goalCol){ + // if the column is wrong. + lineSensor->lineFollowForwards(); + } + else{ + drivingChassis->stop(); + if(goalRow == drivingChassis->myChassisPose.currentRow){ + navState = FINISHED; + } + } + break; + case WAIT_FOR_MOTION_SETPOINT_REACHED: + if(drivingChassis -> statusOfChassisDriving() == REACHED_SETPOINT){ + navState = navStateAfterMotionSetpointReached; + } + break; + case FINISHED: + navState = INITIALIZE_NAVIGATION; + } + return navState; +} + + + diff --git a/NavigationRoutine.h b/NavigationRoutine.h new file mode 100644 index 0000000..3f074c4 --- /dev/null +++ b/NavigationRoutine.h @@ -0,0 +1,38 @@ +/* + * NavigationRoutine.h + * + * Created on: Oct 22, 2020 + * Author: gentov + */ + +#include "DrivingChassis.h" +#include "LineFollower.h" + +#ifndef NAVIGATIONROUTINE_H_ +#define NAVIGATIONROUTINE_H_ + +enum NavigationStates{ + INITIALIZE_NAVIGATION = 0, + TURN_TOWARDS_CORRECT_COLUMN = 1, + FINDING_OUTER_EDGE = 2, + FINDING_ROW = 3, + TURN_TOWARDS_CORRECT_ROW = 4, + FINDING_COLUMN = 5, + WAIT_FOR_MOTION_SETPOINT_REACHED = 6, + FINISHED = 7, +}; + +void setNavGoal(int row, int col, DrivingChassis* robotChassis, LineFollower* lineFollower); +NavigationStates checkNavStatus(); + +static DrivingChassis* drivingChassis; +static LineFollower* lineSensor; +static int goalRow; +static int goalCol; + +static NavigationStates navState = INITIALIZE_NAVIGATION; + +// This is the navState that occurs after a setpoint has been reached +static NavigationStates navStateAfterMotionSetpointReached; + +#endif /* NAVIGATIONROUTINE_H_ */ diff --git a/Pose.cpp b/Pose.cpp new file mode 100644 index 0000000..3d15681 --- /dev/null +++ b/Pose.cpp @@ -0,0 +1,13 @@ +/* + * Pose.cpp + * + * Created on: Oct 21, 2020 + * Author: gentov + */ + +#include "Pose.h" + +// Initialize robot pose. +Pose::Pose(){ + +} diff --git a/Pose.h b/Pose.h new file mode 100644 index 0000000..abc0925 --- /dev/null +++ b/Pose.h @@ -0,0 +1,26 @@ +/* + * Pose.h + * + * Created on: Oct 21, 2020 + * Author: Gabe's PC + */ +#ifndef POSE_H_ +#define POSE_H_ + +class Pose{ + public: + Pose(); + // current row and current column will be updated by the line sensor lineCount + int currentRow = 0; + int currentColumn = 0; + // current heading will be updated by the IMU during turnToHeading. + float heading = 0; + int rowCount = 0; + int colCount = 0; + + private: +}; + + + +#endif /* POSE_H_ */ diff --git a/StudentsRobot.cpp b/StudentsRobot.cpp index ac2483d..9c389aa 100644 --- a/StudentsRobot.cpp +++ b/StudentsRobot.cpp @@ -5,12 +5,16 @@ * Author: hephaestus */ + #include "StudentsRobot.h" +uint32_t startTime = 0; + StudentsRobot::StudentsRobot(PIDMotor * motor1, PIDMotor * motor2, PIDMotor * motor3, Servo * servo, IRCamSimplePacketComsServer * IRCam, - GetIMU * imu) { + GetIMU * imu): robotChassis(motor2, motor1, 230, 30, imu), lineSensor(&robotChassis) { Serial.println("StudentsRobot::StudentsRobot constructor called here "); + this->servo = servo; this->motor1 = motor1; this->motor2 = motor2; @@ -77,8 +81,8 @@ StudentsRobot::StudentsRobot(PIDMotor * motor1, PIDMotor * motor2, 50 // the speed in degrees per second that the motor spins when the hardware output is at creep forwards ); // Set up the Analog sensors - pinMode(ANALOG_SENSE_ONE, ANALOG); - pinMode(ANALOG_SENSE_TWO, ANALOG); + pinMode(LEFT_LINE_SENSOR, ANALOG); + pinMode(RIGHT_LINE_SENSOR, ANALOG); pinMode(ANALOG_SENSE_THREE, ANALOG); pinMode(ANALOG_SENSE_FOUR, ANALOG); // H-Bridge enable pin @@ -119,22 +123,31 @@ void StudentsRobot::updateStateMachine() { break; case Running: // Set up a non-blocking 1000 ms delay - status = WAIT_FOR_TIME; - nextTime = nextTime + 100; // ensure no timer drift by incremeting the target - // After 1000 ms, come back to this state - nextStatus = Running; - +// status = WAIT_FOR_TIME; +// nextTime = nextTime + 1; // ensure no timer drift by incremeting the target +// // After 1000 ms, come back to this state +// nextStatus = Running; // Do something + // On button press we go into testing state if (!digitalRead(BOOT_FLAG_PIN)) { Serial.println( " Running State Machine " + String((now - startTime))); + //robotChassis.turnDegrees(-90, 5000); + //robotChassis.driveForward(100, 5000); + //robotChassis.driveBackwards(300, 5000); #if defined(USE_IMU) IMU->print(); #endif #if defined(USE_IR_CAM) IRCamera->print(); #endif - + // I put in this delay so that I have time to step back + //status = WAIT_FOR_TIME; + //nextTime = millis() + 3000; // ensure no timer drift by incremeting the target + // After 1000 ms, come back to this state + //nextStatus = TestingBasicMovement; + startTime = millis(); + status = Testing; } break; case WAIT_FOR_TIME: @@ -163,20 +176,86 @@ void StudentsRobot::updateStateMachine() { case Halt: // in safe mode break; + case Testing: +/// LINE FOLLOWING +// if((millis() - startTime) < 7000){ +// lineSensor.lineFollowForwards(); +// } +// else{ +// robotChassis.stop(); +// lineSensor.resetLineCount(); +// status = Running; +// } + +// Navigation + static int myCase = 1; + static int myCaseAfterNav = 2; + switch(myCase){ + case 1: + // set a waypoint + setNavGoal(2, -2, &robotChassis, &lineSensor); + // set the state to go to after the waypoint is reached + myCaseAfterNav = 2; + // set the state + myCase = 4; + break; + case 2: + // set a waypoint + setNavGoal(0, 0, &robotChassis, &lineSensor); + // set the state to go to after the waypoint is reached + myCaseAfterNav = 3; + // set the state + myCase = 4; + break; + case 3: + myCase = 1; + status = Running; + break; + case 4: + if(checkNavStatus() == FINISHED){ + myCase = myCaseAfterNav; + } + break; + } +// BASIC MOTION +// +// static int myCase = 1; +// static int myCaseAfterMotion = 2; +// switch(myCase){ +// case 1: +// robotChassis.driveBackwards(300, 5000); +// myCase = 2; +// myCaseAfterMotion = 3; +// break; +// case 2: +// if(robotChassis.statusOfChassisDriving() == REACHED_SETPOINT){ +// myCase = myCaseAfterMotion; +// } +// break; +// case 3: +// robotChassis.driveForward(300, 5000); +// myCase = 2; +// myCaseAfterMotion = 4; +// break; +// case 4: +// robotChassis.turnToHeading(90, 5000); +// myCase = 2; +// myCaseAfterMotion = 5; +// break; +// case 5: +// robotChassis.turnToHeading(-90, 5000); +// myCase = 2; +// myCaseAfterMotion = 6; +// break; +// case 6: +// status = Running; +// break; +// } + + break; } digitalWrite(WII_CONTROLLER_DETECT, 0); } -/** - * This is run fast and should return fast - * - * You call the PIDMotor's loop function. This will update the whole motor control system - * This will read from the concoder and write to the motors and handle the hardware interface. - */ -void StudentsRobot::pidLoop() { - motor1->loop(); - motor2->loop(); - motor3->loop(); -} diff --git a/StudentsRobot.h b/StudentsRobot.h index 7db3362..a763953 100644 --- a/StudentsRobot.h +++ b/StudentsRobot.h @@ -13,8 +13,11 @@ #include "src/pid/HBridgeEncoderPIDMotor.h" #include "src/pid/ServoAnalogPIDMotor.h" #include +#include "NavigationRoutine.h" #include "DrivingChassis.h" +#include "LineFollower.h" +#include "Pose.h" #include "src/commands/IRCamSimplePacketComsServer.h" #include "src/commands/GetIMU.h" @@ -24,7 +27,8 @@ * Feel free to add ot remove values from here */ enum RobotStateMachine { - StartupRobot = 0, StartRunning = 1, Running = 2, Halting = 3, Halt = 4,WAIT_FOR_MOTORS_TO_FINNISH=5,WAIT_FOR_TIME=6 + StartupRobot = 0, StartRunning = 1, Running = 2, Halting = 3, Halt = 4,WAIT_FOR_MOTORS_TO_FINNISH=5,WAIT_FOR_TIME=6, + Testing = 7, }; /** @@ -48,6 +52,7 @@ enum ComStackStatusState { Fault_obstructed_path = 11, Fault_E_Stop_pressed = 12 }; + /** * @class StudentsRobot */ @@ -57,6 +62,8 @@ class StudentsRobot { PIDMotor * motor2; PIDMotor * motor3; Servo * servo; + DrivingChassis robotChassis; + LineFollower lineSensor; float lsensorVal=0; float rsensorVal=0; long nextTime =0; @@ -89,6 +96,12 @@ class StudentsRobot { */ RobotStateMachine status = StartupRobot; + NavigationStates navState = INITIALIZE_NAVIGATION; + + int goalColumn = -2; + int goalRow = 2; + + /** * pidLoop This functoion is called to let the StudentsRobot controll the running of the PID loop functions diff --git a/src/RobotControlCenter.cpp b/src/RobotControlCenter.cpp index b410edc..ffb69be 100644 --- a/src/RobotControlCenter.cpp +++ b/src/RobotControlCenter.cpp @@ -16,6 +16,12 @@ Adafruit_BNO055 bno; DFRobotIRPosition myDFRobotIRPosition; #endif #define loopTime 5000 + +static TaskHandle_t motorPIDTaskHandler; + +// This array contains the motors for the PID task to run +PIDMotor * RobotControlCenter::pidList[numberOfPID] = {NULL, }; + void RobotControlCenter::loop() { if (state != Startup) { // If this is run before the sensor reads, the I2C will fail because the time it takes to send the UDP causes a timeout @@ -97,6 +103,7 @@ void RobotControlCenter::setup() { servo.setPeriodHertz(50); servo.attach(SERVO_PIN, 1000, 2000); + // // Create sensors and servers #if defined(USE_IMU) sensor = new GetIMU(); @@ -142,12 +149,37 @@ void RobotControlCenter::setup() { #endif + setupMotorPIDTask(); + Serial.println("Setup PID Task"); } +/** + * This calls PIDMotor's loop function. This will update the whole motor control system + * This will read from the encoder and write to the motors and handle the hardware interface. + */ +void motorPIDTask(void *param){ + ESP_LOGI(TAG, "Starting the PID loop thread"); + TickType_t xLastWakeTime; + const TickType_t xFrequency = 1; + xLastWakeTime = xTaskGetTickCount(); + while (1) { + vTaskDelayUntil(&xLastWakeTime, xFrequency); + RobotControlCenter::pidList[0]->loop(); + RobotControlCenter::pidList[1]->loop(); + RobotControlCenter::pidList[2]->loop(); + } + ESP_LOGE(TAG, "ERROR PID thread died!"); +} + +void RobotControlCenter::setupMotorPIDTask(){ + xTaskCreatePinnedToCore(motorPIDTask, "PID loop Thread", 8192, NULL, 1, + &motorPIDTaskHandler, 0); +} + + void RobotControlCenter::fastLoop() { if (state == Startup) // Do not run before startp return; - robot->pidLoop(); #if defined(USE_WIFI) manager.loop(); if (manager.getState() == Connected) @@ -156,6 +188,7 @@ void RobotControlCenter::fastLoop() { return; } #endif + //uint32_t startTime = micros(); robot->updateStateMachine(); - + //Serial.println("Time Taken: " + String(micros() - startTime) + "\r\n" ); } diff --git a/src/RobotControlCenter.h b/src/RobotControlCenter.h index 76a8ea9..3a6bfeb 100644 --- a/src/RobotControlCenter.h +++ b/src/RobotControlCenter.h @@ -61,8 +61,6 @@ class RobotControlCenter { int64_t lastPrint = 0; // Change this to set your team name String * name; // - // List of PID objects to use with PID server - PIDMotor * pidList[numberOfPID]; // = { &motor1.myPID, &motor2.myPID }; #if defined(USE_WIFI) // SImple packet coms implementation useing WiFi @@ -99,6 +97,14 @@ class RobotControlCenter { * This function is callled over and over by the INO loop() */ void loop(); + + /** + * Creates a task for the motor PID loop + */ + void setupMotorPIDTask(); + + + /** * A pointer to the students robot * @@ -106,6 +112,10 @@ class RobotControlCenter { * This variable is set as part of @see RobotControlCenter::setup */ StudentsRobot * robot; + + // List of PID objects to use with PID server + static PIDMotor * pidList[numberOfPID]; // = { &motor1.myPID, &motor2.myPID }; + protected: HBridgeEncoderPIDMotor motor1; // PID controlled motor object HBridgeEncoderPIDMotor motor2; // PID controlled motor object