From a2eb154b66e03ac7daf2196e41b4569181bcde9e Mon Sep 17 00:00:00 2001
From: gentov <33622808+gentov@users.noreply.github.com>
Date: Mon, 5 Oct 2020 12:32:50 -0400
Subject: [PATCH 01/19] basic movement on new repo

Need to make pose object.

Uses encoder instead of IMU right now
---
 DrivingChassis.cpp | 37 +++++++++++++++++++++++++++++++++++--
 DrivingChassis.h   | 22 ++++++++++++++++++----
 StudentsRobot.cpp  |  5 ++++-
 StudentsRobot.h    |  1 +
 4 files changed, 58 insertions(+), 7 deletions(-)

diff --git a/DrivingChassis.cpp b/DrivingChassis.cpp
index 49ac2a5..9ceea3d 100644
--- a/DrivingChassis.cpp
+++ b/DrivingChassis.cpp
@@ -56,7 +56,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;
 }
 
 /**
@@ -70,6 +74,25 @@ DrivingChassis::DrivingChassis(PIDMotor * left, PIDMotor * right,
  * 		 allow for relative moves. Otherwise the motor is always in ABSOLUTE mode
  */
 void DrivingChassis::driveForward(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 -> startInterpolationDegrees(mmDistanceFromCurrent * WHEEL_DEGREES_TO_MM, msDuration, LIN);
+    myright -> startInterpolationDegrees(-mmDistanceFromCurrent * WHEEL_DEGREES_TO_MM, msDuration, LIN);
+}
+
+/**
+	 * 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::driveBackwards(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 -> startInterpolationDegrees(-mmDistanceFromCurrent * WHEEL_DEGREES_TO_MM, msDuration, LIN);
+    myright -> startInterpolationDegrees(mmDistanceFromCurrent * WHEEL_DEGREES_TO_MM, msDuration, LIN);
 }
 
 /**
@@ -88,7 +111,17 @@ void DrivingChassis::driveForward(float mmDistanceFromCurrent, int msDuration) {
  * 		  allow for relative moves. Otherwise the motor is always in ABSOLUTE mode
  */
 void DrivingChassis::turnDegrees(float degreesToRotateBase, int msDuration) {
-
+	/* As of 10/4/2020 Gabe doesn't have an IMU... rippu
+	  Two variants, one with IMU and one without IMU.
+	  IMU variant: will use P controller to modulate speed to make the turn based on
+	  heading. Maybe we can even make this absolute in the future.
+	  Encoder variant: Make the turn based on encoder ticks
+	  */
+	#ifdef USE_IMU
+	#else
+	   myleft -> startInterpolationDegrees(degreesToRotateBase * WHEEL_DEGREES_TO_BODY_DEGREES, msDuration, LIN);
+	   myright -> startInterpolationDegrees(degreesToRotateBase * WHEEL_DEGREES_TO_BODY_DEGREES, msDuration, LIN);
+	#endif
 }
 
 /**
diff --git a/DrivingChassis.h b/DrivingChassis.h
index 35278a0..6a55c77 100644
--- a/DrivingChassis.h
+++ b/DrivingChassis.h
@@ -10,6 +10,11 @@
 #include "src/pid/PIDMotor.h"
 #include "src/commands/GetIMU.h"
 #include "config.h"
+
+#define WHEEL_DEGREES_TO_BODY_DEGREES 4.25F//3.8333447041F
+#define WHEEL_DEGREES_TO_MM 2.1174F
+
+
 /**
  * DrivingChassis encapsulates a 2 wheel differential steered chassis that drives around
  *
@@ -73,16 +78,25 @@ class DrivingChassis {
 			float wheelRadiusMM,GetIMU * imu);
 
 	/**
-	 * Start a drive forward action
+	 * Start a drive backwards action
 	 *
-	 * @param mmDistanceFromCurrent is the distance the mobile base should drive forward
+	 * @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
-	 * @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 driveBackwards(float mmDistanceFromCurrent, int msDuration);
+
+	/**
+		 * Start a drive forward action
+		 *
+		 * @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
+		 */
 	void driveForward(float mmDistanceFromCurrent, int msDuration);
+
 	/**
 	 * Start a turn action
 	 *
diff --git a/StudentsRobot.cpp b/StudentsRobot.cpp
index ac2483d..089a0e8 100644
--- a/StudentsRobot.cpp
+++ b/StudentsRobot.cpp
@@ -9,7 +9,7 @@
 
 StudentsRobot::StudentsRobot(PIDMotor * motor1, PIDMotor * motor2,
 		PIDMotor * motor3, Servo * servo, IRCamSimplePacketComsServer * IRCam,
-		GetIMU * imu) {
+		GetIMU * imu): robotChassis(motor2, motor1, 230, 30, imu) {
 	Serial.println("StudentsRobot::StudentsRobot constructor called here ");
 	this->servo = servo;
 	this->motor1 = motor1;
@@ -128,6 +128,9 @@ void StudentsRobot::updateStateMachine() {
 		if (!digitalRead(BOOT_FLAG_PIN)) {
 			Serial.println(
 					" Running State Machine " + String((now - startTime)));
+			//robotChassis.turnDegrees(-90, 5000);
+			//robotChassis.driveForward(300, 5000);
+			robotChassis.driveBackwards(300, 5000);
 #if defined(USE_IMU)
 			IMU->print();
 #endif
diff --git a/StudentsRobot.h b/StudentsRobot.h
index 7db3362..5a04d8d 100644
--- a/StudentsRobot.h
+++ b/StudentsRobot.h
@@ -57,6 +57,7 @@ class StudentsRobot {
 	PIDMotor * motor2;
 	PIDMotor * motor3;
 	Servo * servo;
+	DrivingChassis robotChassis;
 	float lsensorVal=0;
 	float rsensorVal=0;
 	long nextTime =0;

From 25cb1b517e8f244bf4d75691ff8c27cf5fb17002 Mon Sep 17 00:00:00 2001
From: gentov <33622808+gentov@users.noreply.github.com>
Date: Mon, 5 Oct 2020 21:59:11 -0400
Subject: [PATCH 02/19] added "reset to 0" for relative movements

---
 DrivingChassis.cpp | 16 ++++++++++++----
 DrivingChassis.h   |  2 +-
 StudentsRobot.cpp  |  4 ++--
 3 files changed, 15 insertions(+), 7 deletions(-)

diff --git a/DrivingChassis.cpp b/DrivingChassis.cpp
index 9ceea3d..8170e2f 100644
--- a/DrivingChassis.cpp
+++ b/DrivingChassis.cpp
@@ -76,8 +76,11 @@ DrivingChassis::DrivingChassis(PIDMotor * left, PIDMotor * right,
 void DrivingChassis::driveForward(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 -> startInterpolationDegrees(mmDistanceFromCurrent * WHEEL_DEGREES_TO_MM, msDuration, LIN);
-    myright -> startInterpolationDegrees(-mmDistanceFromCurrent * WHEEL_DEGREES_TO_MM, msDuration, LIN);
+
+	myleft -> overrideCurrentPosition(0);
+	myright -> overrideCurrentPosition(0);
+	myleft -> startInterpolationDegrees(mmDistanceFromCurrent * MM_TO_WHEEL_DEGREES, msDuration, LIN);
+    myright -> startInterpolationDegrees(-mmDistanceFromCurrent * MM_TO_WHEEL_DEGREES, msDuration, LIN);
 }
 
 /**
@@ -91,8 +94,11 @@ void DrivingChassis::driveForward(float mmDistanceFromCurrent, int msDuration) {
 void DrivingChassis::driveBackwards(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 -> startInterpolationDegrees(-mmDistanceFromCurrent * WHEEL_DEGREES_TO_MM, msDuration, LIN);
-    myright -> startInterpolationDegrees(mmDistanceFromCurrent * WHEEL_DEGREES_TO_MM, msDuration, LIN);
+	myleft -> overrideCurrentPosition(0);
+	myright -> overrideCurrentPosition(0);
+
+	myleft -> startInterpolationDegrees(-mmDistanceFromCurrent * MM_TO_WHEEL_DEGREES, msDuration, LIN);
+    myright -> startInterpolationDegrees(mmDistanceFromCurrent * MM_TO_WHEEL_DEGREES, msDuration, LIN);
 }
 
 /**
@@ -119,6 +125,8 @@ void DrivingChassis::turnDegrees(float degreesToRotateBase, int msDuration) {
 	  */
 	#ifdef USE_IMU
 	#else
+	   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);
 	#endif
diff --git a/DrivingChassis.h b/DrivingChassis.h
index 6a55c77..12206cf 100644
--- a/DrivingChassis.h
+++ b/DrivingChassis.h
@@ -12,7 +12,7 @@
 #include "config.h"
 
 #define WHEEL_DEGREES_TO_BODY_DEGREES 4.25F//3.8333447041F
-#define WHEEL_DEGREES_TO_MM 2.1174F
+#define MM_TO_WHEEL_DEGREES 2.1174F
 
 
 /**
diff --git a/StudentsRobot.cpp b/StudentsRobot.cpp
index 089a0e8..b20041d 100644
--- a/StudentsRobot.cpp
+++ b/StudentsRobot.cpp
@@ -128,9 +128,9 @@ void StudentsRobot::updateStateMachine() {
 		if (!digitalRead(BOOT_FLAG_PIN)) {
 			Serial.println(
 					" Running State Machine " + String((now - startTime)));
-			//robotChassis.turnDegrees(-90, 5000);
+			robotChassis.turnDegrees(-90, 5000);
 			//robotChassis.driveForward(300, 5000);
-			robotChassis.driveBackwards(300, 5000);
+			//robotChassis.driveBackwards(300, 5000);
 #if defined(USE_IMU)
 			IMU->print();
 #endif

From fc2cbd5c665dca5d1d06356a602784cb005b6fee Mon Sep 17 00:00:00 2001
From: gentov <33622808+gentov@users.noreply.github.com>
Date: Wed, 7 Oct 2020 14:18:20 -0400
Subject: [PATCH 03/19] preliminary redesign of move-forward

---
 DrivingChassis.cpp | 71 +++++++++++++++++++++++++++++++++++++------
 DrivingChassis.h   | 76 ++++++++++++++++++++++++++++++++++++++--------
 StudentsRobot.cpp  | 47 +++++++++++++++++++++++-----
 StudentsRobot.h    |  3 +-
 4 files changed, 166 insertions(+), 31 deletions(-)

diff --git a/DrivingChassis.cpp b/DrivingChassis.cpp
index 8170e2f..3a3084b 100644
--- a/DrivingChassis.cpp
+++ b/DrivingChassis.cpp
@@ -73,7 +73,7 @@ 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
 
@@ -83,6 +83,47 @@ void DrivingChassis::driveForward(float mmDistanceFromCurrent, int msDuration) {
     myright -> startInterpolationDegrees(-mmDistanceFromCurrent * MM_TO_WHEEL_DEGREES, msDuration, LIN);
 }
 
+bool DrivingChassis::driveForward(float mmDistanceFromCurrent, int msDuration){
+	// if we're not performing an action
+	// start a timer, reset encoders
+    if(!performingMovement){
+    	startTimeOfMovement_ms = millis();
+    	performingMovement = true;
+    	//myleft -> overrideCurrentPosition(0);
+    	//myright -> overrideCurrentPosition(0);
+    	// TODO: call driveStraight method which uses IMU (if we're using an IMU)
+    	Serial.println("HERE\r\n");
+    }
+	// check for timeout
+	if((millis() - startTimeOfMovement_ms) > msDuration){
+		//timeout occured. Stop the robot
+		Serial.println("Detected Timeout\r\n");
+		stop();
+		//performingMovement = false;
+		return false;
+	}
+
+	float currentDistanceMovedRightWheel_mm = (myright -> getAngleDegrees())*WHEEL_DEGREES_TO_MM;
+	float currentDistanceMovedLeftWheel_mm = (myleft -> getAngleDegrees())*WHEEL_DEGREES_TO_MM;
+	float rightWheelError_mm = currentDistanceMovedRightWheel_mm - mmDistanceFromCurrent;
+	float leftWheelError_mm = - currentDistanceMovedLeftWheel_mm - mmDistanceFromCurrent;
+
+	if((fabs(rightWheelError_mm) < wheelMovementDeadband_mm) && (fabs(leftWheelError_mm) < wheelMovementDeadband_mm)){
+		Serial.println("Reached Setpoint \r\n");
+		stop();
+		//performingMovement = false;
+		return false;
+	}
+	else{
+		Serial.println("Right Error: " + String(rightWheelError_mm) + "\r\n" );
+		Serial.println("Left Error: " + String(leftWheelError_mm) + "\r\n" );
+		myright -> setVelocityDegreesPerSecond(-wheelMovementKp*rightWheelError_mm);
+		myleft -> setVelocityDegreesPerSecond(wheelMovementKp*leftWheelError_mm);
+	}
+
+    return true;
+}
+
 /**
 	 * Start a drive backwards action
 	 *
@@ -91,7 +132,7 @@ void DrivingChassis::driveForward(float mmDistanceFromCurrent, int msDuration) {
 	 *
 	 * @note this function is fast-return and should not block
 */
-void DrivingChassis::driveBackwards(float mmDistanceFromCurrent, int msDuration) {
+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);
@@ -101,6 +142,10 @@ void DrivingChassis::driveBackwards(float mmDistanceFromCurrent, int msDuration)
     myright -> startInterpolationDegrees(mmDistanceFromCurrent * MM_TO_WHEEL_DEGREES, msDuration, LIN);
 }
 
+bool DrivingChassis::driveBackwards(float mmDistanceFromCurrent, int msDuration){
+	return false;
+}
+
 /**
  * Start a turn action
  *
@@ -116,20 +161,21 @@ void DrivingChassis::driveBackwards(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);
+}
+
+bool DrivingChassis::turnDegrees(float degreesToRotateBase, int msDuration){
 	/* As of 10/4/2020 Gabe doesn't have an IMU... rippu
 	  Two variants, one with IMU and one without IMU.
 	  IMU variant: will use P controller to modulate speed to make the turn based on
 	  heading. Maybe we can even make this absolute in the future.
 	  Encoder variant: Make the turn based on encoder ticks
 	  */
-	#ifdef USE_IMU
-	#else
-	   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);
-	#endif
+	return false;
 }
 
 /**
@@ -142,6 +188,11 @@ void DrivingChassis::turnDegrees(float degreesToRotateBase, int msDuration) {
 bool DrivingChassis::isChassisDoneDriving() {
 	return false;
 }
+
+void DrivingChassis::stop(){
+	myleft -> stop();
+	myright -> stop();
+}
 /**
  * loop()
  *
diff --git a/DrivingChassis.h b/DrivingChassis.h
index 12206cf..4943fbc 100644
--- a/DrivingChassis.h
+++ b/DrivingChassis.h
@@ -11,8 +11,9 @@
 #include "src/commands/GetIMU.h"
 #include "config.h"
 
-#define WHEEL_DEGREES_TO_BODY_DEGREES 4.25F//3.8333447041F
+#define WHEEL_DEGREES_TO_BODY_DEGREES 4.25F
 #define MM_TO_WHEEL_DEGREES 2.1174F
+#define WHEEL_DEGREES_TO_MM .472277F
 
 
 /**
@@ -63,6 +64,12 @@ class DrivingChassis {
 	 */
 	float chassisRotationToWheelDistance(float angle);
 public:
+	bool performingMovement = false;
+	unsigned long startTimeOfMovement_ms;
+	float wheelMovementKp = 10;
+	int wheelMovementDeadband_mm = 1;
+	float wheelMovementDeadband_deg = .5;
+
 	virtual ~DrivingChassis();
 
 	/**
@@ -78,27 +85,52 @@ class DrivingChassis {
 			float wheelRadiusMM,GetIMU * imu);
 
 	/**
-	 * Start a drive backwards 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.
+	 */
+	bool 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
 	 */
-	void driveBackwards(float mmDistanceFromCurrent, int msDuration);
+	void driveForwardFromInterpolation(float mmDistanceFromCurrent, int msDuration);
 
 	/**
-		 * Start a drive forward action
-		 *
-		 * @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
-		 */
-	void driveForward(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)
+	 */
+	bool 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
@@ -112,7 +144,19 @@ 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.
+	 *
+	 * @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.
+	 */
+	bool turnDegrees(float degreesToRotateBase, int msDuration);
+
 	/**
 	 * Check to see if the chassis is performing an action
 	 *
@@ -121,6 +165,12 @@ class DrivingChassis {
 	 * @note this function is fast-return and should not block
 	 */
 	bool isChassisDoneDriving();
+
+	/**
+	 * Stops all motors
+	 */
+	void stop();
+
 	/**
 	 * loop()
 	 *
diff --git a/StudentsRobot.cpp b/StudentsRobot.cpp
index b20041d..cfe23a1 100644
--- a/StudentsRobot.cpp
+++ b/StudentsRobot.cpp
@@ -119,17 +119,16 @@ 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
+		//status = WAIT_FOR_TIME;
+		nextTime = nextTime + 10; // ensure no timer drift by incremeting the target
 		// After 1000 ms, come back to this state
-		nextStatus = Running;
-
+		//nextStatus = Running;
 		// Do something
 		if (!digitalRead(BOOT_FLAG_PIN)) {
 			Serial.println(
 					" Running State Machine " + String((now - startTime)));
-			robotChassis.turnDegrees(-90, 5000);
-			//robotChassis.driveForward(300, 5000);
+			//robotChassis.turnDegrees(-90, 5000);
+			//robotChassis.driveForward(100, 5000);
 			//robotChassis.driveBackwards(300, 5000);
 #if defined(USE_IMU)
 			IMU->print();
@@ -137,7 +136,7 @@ void StudentsRobot::updateStateMachine() {
 #if defined(USE_IR_CAM)
 			IRCamera->print();
 #endif
-
+        status = TestingBasicMovement;
 		}
 		break;
 	case WAIT_FOR_TIME:
@@ -166,6 +165,40 @@ void StudentsRobot::updateStateMachine() {
 	case Halt:
 		// in safe mode
 		break;
+	case TestingBasicMovement:
+//		// Move forward for 300 mm, then turn right
+//		static bool navigatedToBox = false;
+//		static bool turnedToBox    = false;
+//		static bool gotBox         = false;
+//
+//		// if we haven't gotten to the right grid space
+//		if(!navigatedToBox)
+//		{
+//			//Drive until we reach 300 mm
+//			if(!robotChassis.driveForward(300, 5000)){
+//				navigatedToBox = true;
+//				//we found the box, now we have to turn to it
+//			}
+//		}
+//		// we've found the box but haven't turned to it
+//		else if(navigatedToBox && !turnedToBox){
+//			//Turn 90 degrees
+//			if(!robotChassis.turnDegrees(90, 1000)){
+//				turnedToBox = true;
+//				//we found the box, now we have to turn to it
+//			}
+//		}
+//
+//		else if (!gotBox && turnedToBox){
+//	        //drive forward until limit switch
+//			robotChassis.driveForward(0, 1000);
+//			if(digitalRead(limitSwitch)){
+//				gotBox = true;
+//				status = Running;
+//			}
+//		}
+//
+		break;
 
 	}
 	digitalWrite(WII_CONTROLLER_DETECT, 0);
diff --git a/StudentsRobot.h b/StudentsRobot.h
index 5a04d8d..dc102eb 100644
--- a/StudentsRobot.h
+++ b/StudentsRobot.h
@@ -24,7 +24,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,
+	TestingBasicMovement = 7,
 
 };
 /**

From 6919170c45472f93c2c9f688ca9c8654583055ac Mon Sep 17 00:00:00 2001
From: gentov <33622808+gentov@users.noreply.github.com>
Date: Wed, 7 Oct 2020 21:36:21 -0400
Subject: [PATCH 04/19] Basic movement but newer

Newer basic movement

Test state made

TODO: Drive straight
---
 DrivingChassis.cpp | 126 ++++++++++++++++++++++++++++++++++++---------
 DrivingChassis.h   |  11 ++--
 StudentsRobot.cpp  |  49 ++++++++++++++++--
 3 files changed, 155 insertions(+), 31 deletions(-)

diff --git a/DrivingChassis.cpp b/DrivingChassis.cpp
index 3a3084b..66ff585 100644
--- a/DrivingChassis.cpp
+++ b/DrivingChassis.cpp
@@ -89,37 +89,44 @@ bool DrivingChassis::driveForward(float mmDistanceFromCurrent, int msDuration){
     if(!performingMovement){
     	startTimeOfMovement_ms = millis();
     	performingMovement = true;
-    	//myleft -> overrideCurrentPosition(0);
-    	//myright -> overrideCurrentPosition(0);
+    	myleft -> overrideCurrentPosition(0);
+    	myright -> overrideCurrentPosition(0);
     	// TODO: call driveStraight method which uses IMU (if we're using an IMU)
-    	Serial.println("HERE\r\n");
     }
 	// check for timeout
 	if((millis() - startTimeOfMovement_ms) > msDuration){
 		//timeout occured. Stop the robot
 		Serial.println("Detected Timeout\r\n");
 		stop();
-		//performingMovement = false;
+		performingMovement = false;
 		return false;
 	}
 
-	float currentDistanceMovedRightWheel_mm = (myright -> getAngleDegrees())*WHEEL_DEGREES_TO_MM;
-	float currentDistanceMovedLeftWheel_mm = (myleft -> getAngleDegrees())*WHEEL_DEGREES_TO_MM;
-	float rightWheelError_mm = currentDistanceMovedRightWheel_mm - mmDistanceFromCurrent;
-	float leftWheelError_mm = - currentDistanceMovedLeftWheel_mm - mmDistanceFromCurrent;
+    if(mmDistanceFromCurrent != -1){
+    	float currentDistanceMovedRightWheel_mm = (myright -> getAngleDegrees())*WHEEL_DEGREES_TO_MM;
+	    float currentDistanceMovedLeftWheel_mm = (myleft -> getAngleDegrees())*WHEEL_DEGREES_TO_MM;
+	    float rightWheelError_mm = currentDistanceMovedRightWheel_mm - mmDistanceFromCurrent;
+	    float leftWheelError_mm = - currentDistanceMovedLeftWheel_mm - mmDistanceFromCurrent;
 
-	if((fabs(rightWheelError_mm) < wheelMovementDeadband_mm) && (fabs(leftWheelError_mm) < wheelMovementDeadband_mm)){
-		Serial.println("Reached Setpoint \r\n");
-		stop();
-		//performingMovement = false;
-		return false;
-	}
-	else{
-		Serial.println("Right Error: " + String(rightWheelError_mm) + "\r\n" );
-		Serial.println("Left Error: " + String(leftWheelError_mm) + "\r\n" );
-		myright -> setVelocityDegreesPerSecond(-wheelMovementKp*rightWheelError_mm);
-		myleft -> setVelocityDegreesPerSecond(wheelMovementKp*leftWheelError_mm);
-	}
+	    if((fabs(rightWheelError_mm) < wheelMovementDeadband_mm) && (fabs(leftWheelError_mm) < wheelMovementDeadband_mm)){
+		    Serial.println("Reached Setpoint \r\n");
+		    stop();
+		    performingMovement = false;
+		    return false;
+	    }
+	    else{
+		    //Serial.println("Right Error: " + String(rightWheelError_mm) + "\r\n" );
+		    //Serial.println("Left Error: " + String(leftWheelError_mm) + "\r\n" );
+		    myright -> setVelocityDegreesPerSecond(-wheelMovementKp*rightWheelError_mm);
+		    myleft -> setVelocityDegreesPerSecond(wheelMovementKp*leftWheelError_mm);
+	    }
+    }
+
+    else{
+    	// sets speed to 10 cm per second
+    	myright -> setVelocityDegreesPerSecond(100*MM_TO_WHEEL_DEGREES);
+        myleft -> setVelocityDegreesPerSecond(-100*MM_TO_WHEEL_DEGREES);
+    }
 
     return true;
 }
@@ -143,7 +150,51 @@ void DrivingChassis::driveBackwardsFromInterpolation(float mmDistanceFromCurrent
 }
 
 bool DrivingChassis::driveBackwards(float mmDistanceFromCurrent, int msDuration){
-	return false;
+		// if we're not performing an action
+		// start a timer, reset encoders
+	    if(!performingMovement){
+	    	startTimeOfMovement_ms = millis();
+	    	performingMovement = true;
+	    	myleft -> overrideCurrentPosition(0);
+	    	myright -> overrideCurrentPosition(0);
+	    	// TODO: call driveStraight method which uses IMU (if we're using an IMU)
+	    }
+		// check for timeout
+		if((millis() - startTimeOfMovement_ms) > msDuration){
+			//timeout occured. Stop the robot
+			Serial.println("Detected Timeout\r\n");
+			stop();
+			performingMovement = false;
+			return false;
+		}
+
+	    if(mmDistanceFromCurrent != -1){
+	    	float currentDistanceMovedRightWheel_mm = (myright -> getAngleDegrees())*WHEEL_DEGREES_TO_MM;
+		    float currentDistanceMovedLeftWheel_mm = (myleft -> getAngleDegrees())*WHEEL_DEGREES_TO_MM;
+		    float rightWheelError_mm = - currentDistanceMovedRightWheel_mm - mmDistanceFromCurrent;
+		    float leftWheelError_mm = currentDistanceMovedLeftWheel_mm - mmDistanceFromCurrent;
+
+		    if((fabs(rightWheelError_mm) < wheelMovementDeadband_mm) && (fabs(leftWheelError_mm) < wheelMovementDeadband_mm)){
+			    Serial.println("Reached Setpoint \r\n");
+			    stop();
+			    performingMovement = false;
+			    return false;
+		    }
+		    else{
+			    //Serial.println("Right Error: " + String(rightWheelError_mm) + "\r\n" );
+			    //Serial.println("Left Error: " + String(leftWheelError_mm) + "\r\n" );
+			    myright -> setVelocityDegreesPerSecond(wheelMovementKp*rightWheelError_mm);
+			    myleft -> setVelocityDegreesPerSecond(-wheelMovementKp*leftWheelError_mm);
+		    }
+	    }
+
+	    else{
+	    	// sets speed to 10 cm per second
+	    	myright -> setVelocityDegreesPerSecond(-100*MM_TO_WHEEL_DEGREES);
+	        myleft -> setVelocityDegreesPerSecond(100*MM_TO_WHEEL_DEGREES);
+	    }
+
+	    return true;
 }
 
 /**
@@ -168,14 +219,43 @@ void DrivingChassis::turnDegreesFromInterpolation(float degreesToRotateBase, int
 	   myright -> startInterpolationDegrees(degreesToRotateBase * WHEEL_DEGREES_TO_BODY_DEGREES, msDuration, LIN);
 }
 
-bool DrivingChassis::turnDegrees(float degreesToRotateBase, int msDuration){
+bool DrivingChassis::turnToHeading(float degreesToRotateBase, int msDuration){
 	/* As of 10/4/2020 Gabe doesn't have an IMU... rippu
 	  Two variants, one with IMU and one without IMU.
 	  IMU variant: will use P controller to modulate speed to make the turn based on
 	  heading. Maybe we can even make this absolute in the future.
 	  Encoder variant: Make the turn based on encoder ticks
 	  */
-	return false;
+	 if(!performingMovement){
+		    	startTimeOfMovement_ms = millis();
+		    	performingMovement = true;
+     }
+	 // check for timeout
+     if((millis() - startTimeOfMovement_ms) > msDuration){
+			//timeout occured. Stop the robot
+			Serial.println("Detected Timeout\r\n");
+			stop();
+			performingMovement = false;
+			return false;
+    }
+
+	float currentHeading = IMU->getEULER_azimuth();
+
+	float headingError = - currentHeading - degreesToRotateBase;
+
+	if(fabs(headingError) <= wheelMovementDeadband_deg)
+	{
+		Serial.println("Reached Setpoint\r\n");
+		performingMovement = false;
+		stop();
+		return false;
+	}
+	else{
+		    Serial.println("Heading Error: " + String(headingError) +"\r\n");
+            myleft->setVelocityDegreesPerSecond(-(wheelMovementKp*2.8) * headingError);
+            myright->setVelocityDegreesPerSecond(-(wheelMovementKp*2.8) * headingError);
+	}
+	return true;
 }
 
 /**
diff --git a/DrivingChassis.h b/DrivingChassis.h
index 4943fbc..5905e90 100644
--- a/DrivingChassis.h
+++ b/DrivingChassis.h
@@ -66,7 +66,7 @@ class DrivingChassis {
 public:
 	bool performingMovement = false;
 	unsigned long startTimeOfMovement_ms;
-	float wheelMovementKp = 10;
+	float wheelMovementKp = 20;
 	int wheelMovementDeadband_mm = 1;
 	float wheelMovementDeadband_deg = .5;
 
@@ -149,13 +149,18 @@ class DrivingChassis {
 	/**
 	 * Start a turn action. Will return false when finished moving. Will stop when reached setpoint or timeout.
 	 *
-	 * @param mmDistanceFromCurrent is the distance the mobile base should drive backwards
+	 * 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.
 	 */
-	bool turnDegrees(float degreesToRotateBase, int msDuration);
+	bool turnToHeading(float desiredHeading, int msDuration);
 
 	/**
 	 * Check to see if the chassis is performing an action
diff --git a/StudentsRobot.cpp b/StudentsRobot.cpp
index cfe23a1..14ddc50 100644
--- a/StudentsRobot.cpp
+++ b/StudentsRobot.cpp
@@ -119,11 +119,12 @@ void StudentsRobot::updateStateMachine() {
 		break;
 	case Running:
 		// Set up a non-blocking 1000 ms delay
-		//status = WAIT_FOR_TIME;
-		nextTime = nextTime + 10; // 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)));
@@ -136,7 +137,12 @@ void StudentsRobot::updateStateMachine() {
 #if defined(USE_IR_CAM)
 			IRCamera->print();
 #endif
-        status = TestingBasicMovement;
+		// 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;
+		status = TestingBasicMovement;
 		}
 		break;
 	case WAIT_FOR_TIME:
@@ -166,6 +172,39 @@ void StudentsRobot::updateStateMachine() {
 		// in safe mode
 		break;
 	case TestingBasicMovement:
+		static bool movedForward = false;
+		static bool movedBack    = false;
+	    static bool turnedRight  = false;
+		static bool turnedLeft   = false;
+		static bool backToZero   = false;
+
+		if(!movedForward){
+			if(!robotChassis.driveForward(300, 5000)){
+				movedForward = true;
+			}
+		}
+		else if(movedForward && !movedBack){
+				if(!robotChassis.driveBackwards(300, 5000)){
+					movedBack = true;
+				}
+	    }
+		else if(movedBack && !turnedRight){
+			if(!robotChassis.turnToHeading(-90, 5000)){
+				turnedRight = true;
+			}
+		}
+		else if(turnedRight && !turnedLeft){
+			if(!robotChassis.turnToHeading(90, 5000)){
+				turnedLeft = true;
+			}
+		}
+		else if(turnedLeft && !backToZero){
+			if(!robotChassis.turnToHeading(0, 5000)){
+				backToZero = true;
+				status = Running;
+			}
+		}
+
 //		// Move forward for 300 mm, then turn right
 //		static bool navigatedToBox = false;
 //		static bool turnedToBox    = false;

From 2933735a0152a446b3324cf6950867ac4e536a10 Mon Sep 17 00:00:00 2001
From: gentov <33622808+gentov@users.noreply.github.com>
Date: Wed, 14 Oct 2020 23:39:31 -0400
Subject: [PATCH 05/19] got line following to work backwards

not forwards, but thats ok for us
---
 DrivingChassis.cpp         |   8 ++-
 DrivingChassis.h           |   7 +--
 LineFollower.cpp           |  65 +++++++++++++++++++++++
 LineFollower.h             |  32 +++++++++++
 StudentsRobot.cpp          | 106 +++++++++++++++++--------------------
 StudentsRobot.h            |   4 +-
 src/RobotControlCenter.cpp |   3 +-
 7 files changed, 157 insertions(+), 68 deletions(-)
 create mode 100644 LineFollower.cpp
 create mode 100644 LineFollower.h

diff --git a/DrivingChassis.cpp b/DrivingChassis.cpp
index 66ff585..bf26406 100644
--- a/DrivingChassis.cpp
+++ b/DrivingChassis.cpp
@@ -127,7 +127,6 @@ bool DrivingChassis::driveForward(float mmDistanceFromCurrent, int msDuration){
     	myright -> setVelocityDegreesPerSecond(100*MM_TO_WHEEL_DEGREES);
         myleft -> setVelocityDegreesPerSecond(-100*MM_TO_WHEEL_DEGREES);
     }
-
     return true;
 }
 
@@ -181,8 +180,8 @@ bool DrivingChassis::driveBackwards(float mmDistanceFromCurrent, int msDuration)
 			    return false;
 		    }
 		    else{
-			    //Serial.println("Right Error: " + String(rightWheelError_mm) + "\r\n" );
-			    //Serial.println("Left Error: " + String(leftWheelError_mm) + "\r\n" );
+//			    Serial.println("Right Error: " + String(rightWheelError_mm) + "\r\n" );
+//			    Serial.println("Left Error: " + String(leftWheelError_mm) + "\r\n" );
 			    myright -> setVelocityDegreesPerSecond(wheelMovementKp*rightWheelError_mm);
 			    myleft -> setVelocityDegreesPerSecond(-wheelMovementKp*leftWheelError_mm);
 		    }
@@ -193,7 +192,6 @@ bool DrivingChassis::driveBackwards(float mmDistanceFromCurrent, int msDuration)
 	    	myright -> setVelocityDegreesPerSecond(-100*MM_TO_WHEEL_DEGREES);
 	        myleft -> setVelocityDegreesPerSecond(100*MM_TO_WHEEL_DEGREES);
 	    }
-
 	    return true;
 }
 
@@ -251,7 +249,7 @@ bool DrivingChassis::turnToHeading(float degreesToRotateBase, int msDuration){
 		return false;
 	}
 	else{
-		    Serial.println("Heading Error: " + String(headingError) +"\r\n");
+		    //Serial.println("Heading Error: " + String(headingError) +"\r\n");
             myleft->setVelocityDegreesPerSecond(-(wheelMovementKp*2.8) * headingError);
             myright->setVelocityDegreesPerSecond(-(wheelMovementKp*2.8) * headingError);
 	}
diff --git a/DrivingChassis.h b/DrivingChassis.h
index 5905e90..0b9b75f 100644
--- a/DrivingChassis.h
+++ b/DrivingChassis.h
@@ -31,8 +31,6 @@
  */
 class DrivingChassis {
 private:
-	PIDMotor * myleft;
-	PIDMotor * myright;
 	GetIMU * IMU;
 	float mywheelTrackMM;
 	float mywheelRadiusMM;
@@ -64,10 +62,13 @@ class DrivingChassis {
 	 */
 	float chassisRotationToWheelDistance(float angle);
 public:
+	// moved these over for line following
+	PIDMotor * myleft;
+	PIDMotor * myright;
 	bool performingMovement = false;
 	unsigned long startTimeOfMovement_ms;
 	float wheelMovementKp = 20;
-	int wheelMovementDeadband_mm = 1;
+	int wheelMovementDeadband_mm = 2.5;
 	float wheelMovementDeadband_deg = .5;
 
 	virtual ~DrivingChassis();
diff --git a/LineFollower.cpp b/LineFollower.cpp
new file mode 100644
index 0000000..b81382e
--- /dev/null
+++ b/LineFollower.cpp
@@ -0,0 +1,65 @@
+/*
+ * LineFollower.cpp
+ *
+ *  Created on: Oct 15, 2020
+ *      Author: gentov
+ */
+
+#include "LineFollower.h"
+
+LineFollower::LineFollower(DrivingChassis* myChassis){
+     this->robotChassis = myChassis;
+}
+
+void LineFollower::lineFollow(){
+	// line following. if both sensors are on white full steam ahead but timeout eventually.
+	  // we need the timeout cause otherwise
+	  int leftSensorValue = analogRead(LEFT_LINE_SENSOR);
+	  int rightSensorValue = analogRead(RIGHT_LINE_SENSOR);
+	  int leftCorrection = 0;
+	  int rightCorrection = 0;
+
+	  if(leftSensorValue >= ON_BLACK && rightSensorValue>= ON_BLACK)
+	  {
+	    // reset the timeout timer.
+	    if(canCountLine){
+	      lineCount++;
+	      canCountLine = false;
+	    }
+	    Serial.println("Line Count: " + String(lineCount));
+	  }
+
+	  else if(leftSensorValue < ON_BLACK && rightSensorValue >= ON_BLACK){
+	    // turn right
+	    //Serial.println("Turning Right");
+	    leftCorrection = 50;
+	    rightCorrection = -50;
+	    canCountLine = true;
+	  }
+
+	  else if(leftSensorValue >= ON_BLACK && rightSensorValue < ON_BLACK){
+	    // turn left
+	    //Serial.println("Turning Left");
+	    rightCorrection = 50;
+	    leftCorrection = -50;
+	    canCountLine = true;
+	  }
+
+	  else{
+	    //Serial.println("Straddling Line");
+	    canCountLine = true;
+	  }
+	  //Serial.println("giving vel command");
+	  // Only works backwards
+	  robotChassis->myleft -> setVelocityDegreesPerSecond(100*MM_TO_WHEEL_DEGREES + rightCorrection);
+      robotChassis->myright -> setVelocityDegreesPerSecond(-100*MM_TO_WHEEL_DEGREES - leftCorrection);
+	  //robotChassis->myright -> setVelocityDegreesPerSecond(100*MM_TO_WHEEL_DEGREES + leftCorrection);
+	  //robotChassis->myleft -> setVelocityDegreesPerSecond(-100*MM_TO_WHEEL_DEGREES - rightCorrection);
+	  // if not timeout
+	    // set velocity
+}
+
+void LineFollower::resetLineCount(){
+	lineCount = 0;
+}
+
diff --git a/LineFollower.h b/LineFollower.h
new file mode 100644
index 0000000..e937556
--- /dev/null
+++ b/LineFollower.h
@@ -0,0 +1,32 @@
+/*
+ * 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 = 3500;
+	   int lineCount = 0;
+	   bool canCountLine = true;
+       DrivingChassis* robotChassis;
+
+	   void lineFollow();
+	   void resetLineCount();
+
+    private:
+};
+
+
+
+
+#endif /* LINEFOLLOWER_H_ */
diff --git a/StudentsRobot.cpp b/StudentsRobot.cpp
index 14ddc50..b9cd593 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): robotChassis(motor2, motor1, 230, 30, 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
@@ -142,7 +146,8 @@ void StudentsRobot::updateStateMachine() {
 		//nextTime = millis() + 3000; // ensure no timer drift by incremeting the target
 		// After 1000 ms, come back to this state
 		//nextStatus = TestingBasicMovement;
-		status = TestingBasicMovement;
+	    startTime = millis();
+		status = Testing;
 		}
 		break;
 	case WAIT_FOR_TIME:
@@ -171,72 +176,57 @@ void StudentsRobot::updateStateMachine() {
 	case Halt:
 		// in safe mode
 		break;
-	case TestingBasicMovement:
-		static bool movedForward = false;
-		static bool movedBack    = false;
-	    static bool turnedRight  = false;
-		static bool turnedLeft   = false;
-		static bool backToZero   = false;
-
-		if(!movedForward){
-			if(!robotChassis.driveForward(300, 5000)){
-				movedForward = true;
-			}
-		}
-		else if(movedForward && !movedBack){
-				if(!robotChassis.driveBackwards(300, 5000)){
-					movedBack = true;
-				}
-	    }
-		else if(movedBack && !turnedRight){
-			if(!robotChassis.turnToHeading(-90, 5000)){
-				turnedRight = true;
-			}
-		}
-		else if(turnedRight && !turnedLeft){
-			if(!robotChassis.turnToHeading(90, 5000)){
-				turnedLeft = true;
-			}
+	case Testing:
+		if(millis() - startTime < 20000){
+			lineSensor.lineFollow();
+//		    int leftSensorValue = analogRead(LEFT_LINE_SENSOR);
+//		    int rightSensorValue = analogRead(RIGHT_LINE_SENSOR);
+//
+//		    Serial.println(String(leftSensorValue) + " " + String(rightSensorValue) + "\r\n");
 		}
-		else if(turnedLeft && !backToZero){
-			if(!robotChassis.turnToHeading(0, 5000)){
-				backToZero = true;
-				status = Running;
-			}
+		else{
+			robotChassis.stop();
+		   status = Running;
 		}
 
-//		// Move forward for 300 mm, then turn right
-//		static bool navigatedToBox = false;
-//		static bool turnedToBox    = false;
-//		static bool gotBox         = false;
+//		static bool movedForward = false;
+//		static bool movedBack    = false;
+//	    static bool turnedRight  = false;
+//		static bool turnedLeft   = false;
+//		static bool backToZero   = false;
 //
-//		// if we haven't gotten to the right grid space
-//		if(!navigatedToBox)
-//		{
-//			//Drive until we reach 300 mm
+//		if(!movedForward){
 //			if(!robotChassis.driveForward(300, 5000)){
-//				navigatedToBox = true;
-//				//we found the box, now we have to turn to it
+//				movedForward = true;
 //			}
 //		}
-//		// we've found the box but haven't turned to it
-//		else if(navigatedToBox && !turnedToBox){
-//			//Turn 90 degrees
-//			if(!robotChassis.turnDegrees(90, 1000)){
-//				turnedToBox = true;
-//				//we found the box, now we have to turn to it
+//		else if(movedForward && !movedBack){
+//				if(!robotChassis.driveBackwards(300, 5000)){
+//					movedBack = true;
+//				}
+//	    }
+//		else if(movedBack && !turnedRight){
+//			if(!robotChassis.turnToHeading(-90, 5000)){
+//				turnedRight = true;
 //			}
 //		}
-//
-//		else if (!gotBox && turnedToBox){
-//	        //drive forward until limit switch
-//			robotChassis.driveForward(0, 1000);
-//			if(digitalRead(limitSwitch)){
-//				gotBox = true;
+//		else if(turnedRight && !turnedLeft){
+//			if(!robotChassis.turnToHeading(90, 5000)){
+//				turnedLeft = true;
+//			}
+//		}
+//		else if(turnedLeft && !backToZero){
+//			if(!robotChassis.turnToHeading(0, 5000)){
+//				backToZero = true;
 //				status = Running;
+//				movedBack = false;
+//				movedForward = false;
+//				turnedLeft = false;
+//				turnedRight = false;
+//				backToZero  = false;
 //			}
 //		}
-//
+
 		break;
 
 	}
diff --git a/StudentsRobot.h b/StudentsRobot.h
index dc102eb..14169c6 100644
--- a/StudentsRobot.h
+++ b/StudentsRobot.h
@@ -15,6 +15,7 @@
 #include <ESP32Servo.h>
 
 #include "DrivingChassis.h"
+#include "LineFollower.h"
 #include "src/commands/IRCamSimplePacketComsServer.h"
 #include "src/commands/GetIMU.h"
 
@@ -25,7 +26,7 @@
  */
 enum RobotStateMachine {
 	StartupRobot = 0, StartRunning = 1, Running = 2, Halting = 3, Halt = 4,WAIT_FOR_MOTORS_TO_FINNISH=5,WAIT_FOR_TIME=6,
-	TestingBasicMovement = 7,
+	Testing = 7,
 
 };
 /**
@@ -59,6 +60,7 @@ class StudentsRobot {
 	PIDMotor * motor3;
 	Servo * servo;
 	DrivingChassis robotChassis;
+	LineFollower lineSensor;
 	float lsensorVal=0;
 	float rsensorVal=0;
 	long nextTime =0;
diff --git a/src/RobotControlCenter.cpp b/src/RobotControlCenter.cpp
index b410edc..f60a6d4 100644
--- a/src/RobotControlCenter.cpp
+++ b/src/RobotControlCenter.cpp
@@ -156,6 +156,7 @@ void RobotControlCenter::fastLoop() {
 		return;
 	}
 #endif
+	//uint32_t startTime = micros();
 	robot->updateStateMachine();
-
+	//Serial.println("Time Taken: "  + String(micros() - startTime) + "\r\n" );
 }

From c27cfc5adf7966303a200ea5599a2c25afd5326f Mon Sep 17 00:00:00 2001
From: gentov <33622808+gentov@users.noreply.github.com>
Date: Thu, 15 Oct 2020 19:57:10 -0400
Subject: [PATCH 06/19] PIDMotorTask maybe working

maybe got the PIDMotorTask to work
---
 StudentsRobot.cpp          | 10 +++++-----
 src/RobotControlCenter.cpp | 34 +++++++++++++++++++++++++++++++++-
 src/RobotControlCenter.h   |  8 ++++++++
 3 files changed, 46 insertions(+), 6 deletions(-)

diff --git a/StudentsRobot.cpp b/StudentsRobot.cpp
index b9cd593..d38d3a4 100644
--- a/StudentsRobot.cpp
+++ b/StudentsRobot.cpp
@@ -239,9 +239,9 @@ void StudentsRobot::updateStateMachine() {
  * 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();
-}
+//void StudentsRobot::pidLoop() {
+//	motor1->loop();
+//	motor2->loop();
+//	motor3->loop();
+//}
 
diff --git a/src/RobotControlCenter.cpp b/src/RobotControlCenter.cpp
index f60a6d4..a4e103e 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 * motorPIDTaskMotorList[numberOfPID];
+
 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
@@ -68,6 +74,9 @@ RobotControlCenter::RobotControlCenter(String * mn) {
 	pidList[0] = &motor1;
 	pidList[1] = &motor2;
 	pidList[2] = &motor3;
+	motorPIDTaskMotorList[0] = &motor1;
+	motorPIDTaskMotorList[1] = &motor2;
+	motorPIDTaskMotorList[2] = &motor3;
 	state = Startup;
 	name = mn;
 	robot = NULL;
@@ -97,6 +106,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 +152,34 @@ void RobotControlCenter::setup() {
 
 #endif
 
+	setupMotorPIDTask();
+	Serial.println("Setup PID Task");
 }
 
+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);
+        motorPIDTaskMotorList[0]->loop();
+        motorPIDTaskMotorList[1]->loop();
+        motorPIDTaskMotorList[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();
+	//robot->pidLoop();
 #if defined(USE_WIFI)
 	manager.loop();
 	if (manager.getState() == Connected)
diff --git a/src/RobotControlCenter.h b/src/RobotControlCenter.h
index 76a8ea9..15243cd 100644
--- a/src/RobotControlCenter.h
+++ b/src/RobotControlCenter.h
@@ -99,6 +99,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
 	 *

From f2ac5eac986ebec4d22ced5eb2da1a6a427c2107 Mon Sep 17 00:00:00 2001
From: gentov <33622808+gentov@users.noreply.github.com>
Date: Fri, 16 Oct 2020 17:28:43 -0400
Subject: [PATCH 07/19] addressed code review feedback

---
 StudentsRobot.cpp          | 11 -----------
 src/RobotControlCenter.cpp | 16 ++++++++--------
 src/RobotControlCenter.h   |  6 ++++--
 3 files changed, 12 insertions(+), 21 deletions(-)

diff --git a/StudentsRobot.cpp b/StudentsRobot.cpp
index d38d3a4..767c519 100644
--- a/StudentsRobot.cpp
+++ b/StudentsRobot.cpp
@@ -233,15 +233,4 @@ void StudentsRobot::updateStateMachine() {
 	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/src/RobotControlCenter.cpp b/src/RobotControlCenter.cpp
index a4e103e..ffb69be 100644
--- a/src/RobotControlCenter.cpp
+++ b/src/RobotControlCenter.cpp
@@ -20,7 +20,7 @@ DFRobotIRPosition myDFRobotIRPosition;
 static TaskHandle_t motorPIDTaskHandler;
 
 // This array contains the motors for the PID task to run
-PIDMotor * motorPIDTaskMotorList[numberOfPID];
+PIDMotor * RobotControlCenter::pidList[numberOfPID] = {NULL, };
 
 void RobotControlCenter::loop() {
 	if (state != Startup) {
@@ -74,9 +74,6 @@ RobotControlCenter::RobotControlCenter(String * mn) {
 	pidList[0] = &motor1;
 	pidList[1] = &motor2;
 	pidList[2] = &motor3;
-	motorPIDTaskMotorList[0] = &motor1;
-	motorPIDTaskMotorList[1] = &motor2;
-	motorPIDTaskMotorList[2] = &motor3;
 	state = Startup;
 	name = mn;
 	robot = NULL;
@@ -156,6 +153,10 @@ void RobotControlCenter::setup() {
 	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;
@@ -163,9 +164,9 @@ void motorPIDTask(void *param){
     xLastWakeTime = xTaskGetTickCount();
     while (1) {
         vTaskDelayUntil(&xLastWakeTime, xFrequency);
-        motorPIDTaskMotorList[0]->loop();
-        motorPIDTaskMotorList[1]->loop();
-        motorPIDTaskMotorList[2]->loop();
+        RobotControlCenter::pidList[0]->loop();
+        RobotControlCenter::pidList[1]->loop();
+        RobotControlCenter::pidList[2]->loop();
     }
     ESP_LOGE(TAG, "ERROR PID thread died!");
 }
@@ -179,7 +180,6 @@ void RobotControlCenter::setupMotorPIDTask(){
 void RobotControlCenter::fastLoop() {
 	if (state == Startup)    // Do not run before startp
 		return;
-	//robot->pidLoop();
 #if defined(USE_WIFI)
 	manager.loop();
 	if (manager.getState() == Connected)
diff --git a/src/RobotControlCenter.h b/src/RobotControlCenter.h
index 15243cd..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
@@ -114,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

From cad6ee799df2e4fe8c9b369b2eca84ebbd992881 Mon Sep 17 00:00:00 2001
From: gentov <33622808+gentov@users.noreply.github.com>
Date: Sat, 17 Oct 2020 21:25:23 -0400
Subject: [PATCH 08/19] created enumeration for state of driving action

REACHED_SETPOINT, TIMED_OUT, GOING_TO_SETPOINT instead of just true and false
---
 DrivingChassis.cpp | 25 +++++++++++++------------
 DrivingChassis.h   | 16 +++++++++++++---
 StudentsRobot.cpp  | 14 +++++++-------
 3 files changed, 33 insertions(+), 22 deletions(-)

diff --git a/DrivingChassis.cpp b/DrivingChassis.cpp
index bf26406..bad93e8 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
  *
@@ -83,7 +84,7 @@ void DrivingChassis::driveForwardFromInterpolation(float mmDistanceFromCurrent,
     myright -> startInterpolationDegrees(-mmDistanceFromCurrent * MM_TO_WHEEL_DEGREES, msDuration, LIN);
 }
 
-bool DrivingChassis::driveForward(float mmDistanceFromCurrent, int msDuration){
+DrivingStatus DrivingChassis::driveForward(float mmDistanceFromCurrent, int msDuration){
 	// if we're not performing an action
 	// start a timer, reset encoders
     if(!performingMovement){
@@ -99,7 +100,7 @@ bool DrivingChassis::driveForward(float mmDistanceFromCurrent, int msDuration){
 		Serial.println("Detected Timeout\r\n");
 		stop();
 		performingMovement = false;
-		return false;
+		return TIMED_OUT;
 	}
 
     if(mmDistanceFromCurrent != -1){
@@ -112,7 +113,7 @@ bool DrivingChassis::driveForward(float mmDistanceFromCurrent, int msDuration){
 		    Serial.println("Reached Setpoint \r\n");
 		    stop();
 		    performingMovement = false;
-		    return false;
+		    return REACHED_SETPOINT;
 	    }
 	    else{
 		    //Serial.println("Right Error: " + String(rightWheelError_mm) + "\r\n" );
@@ -127,7 +128,7 @@ bool DrivingChassis::driveForward(float mmDistanceFromCurrent, int msDuration){
     	myright -> setVelocityDegreesPerSecond(100*MM_TO_WHEEL_DEGREES);
         myleft -> setVelocityDegreesPerSecond(-100*MM_TO_WHEEL_DEGREES);
     }
-    return true;
+    return GOING_TO_SETPOINT;
 }
 
 /**
@@ -148,7 +149,7 @@ void DrivingChassis::driveBackwardsFromInterpolation(float mmDistanceFromCurrent
     myright -> startInterpolationDegrees(mmDistanceFromCurrent * MM_TO_WHEEL_DEGREES, msDuration, LIN);
 }
 
-bool DrivingChassis::driveBackwards(float mmDistanceFromCurrent, int msDuration){
+DrivingStatus DrivingChassis::driveBackwards(float mmDistanceFromCurrent, int msDuration){
 		// if we're not performing an action
 		// start a timer, reset encoders
 	    if(!performingMovement){
@@ -164,7 +165,7 @@ bool DrivingChassis::driveBackwards(float mmDistanceFromCurrent, int msDuration)
 			Serial.println("Detected Timeout\r\n");
 			stop();
 			performingMovement = false;
-			return false;
+			return TIMED_OUT;
 		}
 
 	    if(mmDistanceFromCurrent != -1){
@@ -177,7 +178,7 @@ bool DrivingChassis::driveBackwards(float mmDistanceFromCurrent, int msDuration)
 			    Serial.println("Reached Setpoint \r\n");
 			    stop();
 			    performingMovement = false;
-			    return false;
+			    return REACHED_SETPOINT;
 		    }
 		    else{
 //			    Serial.println("Right Error: " + String(rightWheelError_mm) + "\r\n" );
@@ -192,7 +193,7 @@ bool DrivingChassis::driveBackwards(float mmDistanceFromCurrent, int msDuration)
 	    	myright -> setVelocityDegreesPerSecond(-100*MM_TO_WHEEL_DEGREES);
 	        myleft -> setVelocityDegreesPerSecond(100*MM_TO_WHEEL_DEGREES);
 	    }
-	    return true;
+	    return GOING_TO_SETPOINT;
 }
 
 /**
@@ -217,7 +218,7 @@ void DrivingChassis::turnDegreesFromInterpolation(float degreesToRotateBase, int
 	   myright -> startInterpolationDegrees(degreesToRotateBase * WHEEL_DEGREES_TO_BODY_DEGREES, msDuration, LIN);
 }
 
-bool DrivingChassis::turnToHeading(float degreesToRotateBase, int msDuration){
+DrivingStatus DrivingChassis::turnToHeading(float degreesToRotateBase, int msDuration){
 	/* As of 10/4/2020 Gabe doesn't have an IMU... rippu
 	  Two variants, one with IMU and one without IMU.
 	  IMU variant: will use P controller to modulate speed to make the turn based on
@@ -234,7 +235,7 @@ bool DrivingChassis::turnToHeading(float degreesToRotateBase, int msDuration){
 			Serial.println("Detected Timeout\r\n");
 			stop();
 			performingMovement = false;
-			return false;
+			return TIMED_OUT;
     }
 
 	float currentHeading = IMU->getEULER_azimuth();
@@ -246,14 +247,14 @@ bool DrivingChassis::turnToHeading(float degreesToRotateBase, int msDuration){
 		Serial.println("Reached Setpoint\r\n");
 		performingMovement = false;
 		stop();
-		return false;
+		return REACHED_SETPOINT;
 	}
 	else{
 		    //Serial.println("Heading Error: " + String(headingError) +"\r\n");
             myleft->setVelocityDegreesPerSecond(-(wheelMovementKp*2.8) * headingError);
             myright->setVelocityDegreesPerSecond(-(wheelMovementKp*2.8) * headingError);
 	}
-	return true;
+	return GOING_TO_SETPOINT;
 }
 
 /**
diff --git a/DrivingChassis.h b/DrivingChassis.h
index 0b9b75f..a98f8cf 100644
--- a/DrivingChassis.h
+++ b/DrivingChassis.h
@@ -16,6 +16,16 @@
 #define WHEEL_DEGREES_TO_MM .472277F
 
 
+/**
+ * @enum DrivingStatus
+ * States when performing an drive action.
+ */
+enum DrivingStatus {
+	REACHED_SETPOINT = 0,
+	TIMED_OUT = 1,
+	GOING_TO_SETPOINT = 2,
+};
+
 /**
  * DrivingChassis encapsulates a 2 wheel differential steered chassis that drives around
  *
@@ -105,7 +115,7 @@ class DrivingChassis {
 	 * @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.
 	 */
-	bool driveBackwards(float mmDistanceFromCurrent, int msDuration);
+	DrivingStatus driveBackwards(float mmDistanceFromCurrent, int msDuration);
 
 	/**
 	 * Start a drive forward action using the encoders and setpoint interpolation
@@ -128,7 +138,7 @@ class DrivingChassis {
 	 * 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)
 	 */
-	bool driveForward(float mmDistanceFromCurrent, int msDuration);
+	DrivingStatus driveForward(float mmDistanceFromCurrent, int msDuration);
 
 	/**
 	 * Start a turn action using the encoders and setpoint interpolation
@@ -161,7 +171,7 @@ class DrivingChassis {
 	 * @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.
 	 */
-	bool turnToHeading(float desiredHeading, int msDuration);
+	DrivingStatus turnToHeading(float desiredHeading, int msDuration);
 
 	/**
 	 * Check to see if the chassis is performing an action
diff --git a/StudentsRobot.cpp b/StudentsRobot.cpp
index 767c519..fcef97e 100644
--- a/StudentsRobot.cpp
+++ b/StudentsRobot.cpp
@@ -196,27 +196,27 @@ void StudentsRobot::updateStateMachine() {
 //		static bool backToZero   = false;
 //
 //		if(!movedForward){
-//			if(!robotChassis.driveForward(300, 5000)){
+//			if(robotChassis.driveForward(300, 5000) == REACHED_SETPOINT){
 //				movedForward = true;
 //			}
 //		}
 //		else if(movedForward && !movedBack){
-//				if(!robotChassis.driveBackwards(300, 5000)){
+//				if(robotChassis.driveBackwards(300, 5000) == REACHED_SETPOINT){
 //					movedBack = true;
 //				}
 //	    }
 //		else if(movedBack && !turnedRight){
-//			if(!robotChassis.turnToHeading(-90, 5000)){
+//			if(robotChassis.turnToHeading(-90, 5000) == REACHED_SETPOINT){
 //				turnedRight = true;
 //			}
 //		}
 //		else if(turnedRight && !turnedLeft){
-//			if(!robotChassis.turnToHeading(90, 5000)){
+//			if(robotChassis.turnToHeading(90, 5000) == REACHED_SETPOINT){
 //				turnedLeft = true;
 //			}
 //		}
 //		else if(turnedLeft && !backToZero){
-//			if(!robotChassis.turnToHeading(0, 5000)){
+//			if(robotChassis.turnToHeading(0, 5000) == REACHED_SETPOINT){
 //				backToZero = true;
 //				status = Running;
 //				movedBack = false;
@@ -226,8 +226,8 @@ void StudentsRobot::updateStateMachine() {
 //				backToZero  = false;
 //			}
 //		}
-
-		break;
+//
+//		break;
 
 	}
 	digitalWrite(WII_CONTROLLER_DETECT, 0);

From b0ac4483c806158f1be65af2c2ab4d116d089bcb Mon Sep 17 00:00:00 2001
From: gentov <33622808+gentov@users.noreply.github.com>
Date: Tue, 20 Oct 2020 00:58:20 -0400
Subject: [PATCH 09/19] clamped control of basic motion

allows for smoother motion
---
 DrivingChassis.cpp |  56 ++++++++++++++++-----
 DrivingChassis.h   |   5 +-
 LineFollower.cpp   |   2 +-
 StudentsRobot.cpp  | 119 +++++++++++++++++++++++++++------------------
 4 files changed, 122 insertions(+), 60 deletions(-)

diff --git a/DrivingChassis.cpp b/DrivingChassis.cpp
index bad93e8..55b25a8 100644
--- a/DrivingChassis.cpp
+++ b/DrivingChassis.cpp
@@ -116,10 +116,21 @@ DrivingStatus DrivingChassis::driveForward(float mmDistanceFromCurrent, int msDu
 		    return REACHED_SETPOINT;
 	    }
 	    else{
-		    //Serial.println("Right Error: " + String(rightWheelError_mm) + "\r\n" );
-		    //Serial.println("Left Error: " + String(leftWheelError_mm) + "\r\n" );
-		    myright -> setVelocityDegreesPerSecond(-wheelMovementKp*rightWheelError_mm);
-		    myleft -> setVelocityDegreesPerSecond(wheelMovementKp*leftWheelError_mm);
+	    	float rightMotorEffort_deg_per_sec = wheelMovementKp*rightWheelError_mm;
+	    	float leftMotorEffort_deg_per_sec = wheelMovementKp*leftWheelError_mm;
+
+	    	//clamps speed to 20 cm per second
+	    	if(rightMotorEffort_deg_per_sec > (MAX_SPEED_MM_PER_SEC * MM_TO_WHEEL_DEGREES)){
+	    		rightMotorEffort_deg_per_sec = MAX_SPEED_MM_PER_SEC*MM_TO_WHEEL_DEGREES;
+	    	}
+	    	if(leftMotorEffort_deg_per_sec > (MAX_SPEED_MM_PER_SEC * MM_TO_WHEEL_DEGREES)){
+	    		leftMotorEffort_deg_per_sec = MAX_SPEED_MM_PER_SEC*MM_TO_WHEEL_DEGREES;
+	        }
+	    	myright -> setVelocityDegreesPerSecond(-rightMotorEffort_deg_per_sec);
+	    	myleft -> setVelocityDegreesPerSecond(leftMotorEffort_deg_per_sec);
+
+//	    	Serial.println("Left Effort: " + String(leftMotorEffort_deg_per_sec) + "\r\n" );
+//	    	Serial.println("Right Effort: " + String(rightMotorEffort_deg_per_sec) + "\r\n" );
 	    }
     }
 
@@ -181,10 +192,21 @@ DrivingStatus DrivingChassis::driveBackwards(float mmDistanceFromCurrent, int ms
 			    return REACHED_SETPOINT;
 		    }
 		    else{
-//			    Serial.println("Right Error: " + String(rightWheelError_mm) + "\r\n" );
-//			    Serial.println("Left Error: " + String(leftWheelError_mm) + "\r\n" );
-			    myright -> setVelocityDegreesPerSecond(wheelMovementKp*rightWheelError_mm);
-			    myleft -> setVelocityDegreesPerSecond(-wheelMovementKp*leftWheelError_mm);
+		    	float rightMotorEffort_deg_per_sec = wheelMovementKp*rightWheelError_mm;
+		    	float leftMotorEffort_deg_per_sec = wheelMovementKp*leftWheelError_mm;
+
+		    	//clamps speed to 20 cm per second
+		    	if(rightMotorEffort_deg_per_sec > (MAX_SPEED_MM_PER_SEC * MM_TO_WHEEL_DEGREES)){
+		    		rightMotorEffort_deg_per_sec = MAX_SPEED_MM_PER_SEC*MM_TO_WHEEL_DEGREES;
+		    	}
+		    	if(leftMotorEffort_deg_per_sec > (MAX_SPEED_MM_PER_SEC * MM_TO_WHEEL_DEGREES)){
+		    		leftMotorEffort_deg_per_sec = MAX_SPEED_MM_PER_SEC*MM_TO_WHEEL_DEGREES;
+		        }
+		    	myright -> setVelocityDegreesPerSecond(rightMotorEffort_deg_per_sec);
+		    	myleft -> setVelocityDegreesPerSecond(-leftMotorEffort_deg_per_sec);
+
+	//	    	Serial.println("Left Effort: " + String(leftMotorEffort_deg_per_sec) + "\r\n" );
+	//	    	Serial.println("Right Effort: " + String(rightMotorEffort_deg_per_sec) + "\r\n" );
 		    }
 	    }
 
@@ -241,7 +263,7 @@ DrivingStatus DrivingChassis::turnToHeading(float degreesToRotateBase, int msDur
 	float currentHeading = IMU->getEULER_azimuth();
 
 	float headingError = - currentHeading - degreesToRotateBase;
-
+    float motorEffort = (turningMovementKp) * headingError;
 	if(fabs(headingError) <= wheelMovementDeadband_deg)
 	{
 		Serial.println("Reached Setpoint\r\n");
@@ -250,9 +272,19 @@ DrivingStatus DrivingChassis::turnToHeading(float degreesToRotateBase, int msDur
 		return REACHED_SETPOINT;
 	}
 	else{
-		    //Serial.println("Heading Error: " + String(headingError) +"\r\n");
-            myleft->setVelocityDegreesPerSecond(-(wheelMovementKp*2.8) * headingError);
-            myright->setVelocityDegreesPerSecond(-(wheelMovementKp*2.8) * headingError);
+		    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;
+		    	}
+		    }
+		    //Serial.println("Motor Effort: " + String(motorEffort) +"\r\n");
+//		    Serial.println("Heading Error: " + String(headingError) +"\r\n");
+            myleft->setVelocityDegreesPerSecond(-motorEffort);
+            myright->setVelocityDegreesPerSecond(-motorEffort);
 	}
 	return GOING_TO_SETPOINT;
 }
diff --git a/DrivingChassis.h b/DrivingChassis.h
index a98f8cf..c582f57 100644
--- a/DrivingChassis.h
+++ b/DrivingChassis.h
@@ -14,6 +14,8 @@
 #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 200
+#define MAX_MOTOR_EFFORT_DURING_TURN 500
 
 
 /**
@@ -77,7 +79,8 @@ class DrivingChassis {
 	PIDMotor * myright;
 	bool performingMovement = false;
 	unsigned long startTimeOfMovement_ms;
-	float wheelMovementKp = 20;
+	float wheelMovementKp = 3.9;// was 3.5
+	float turningMovementKp = 9;
 	int wheelMovementDeadband_mm = 2.5;
 	float wheelMovementDeadband_deg = .5;
 
diff --git a/LineFollower.cpp b/LineFollower.cpp
index b81382e..a2fa6dc 100644
--- a/LineFollower.cpp
+++ b/LineFollower.cpp
@@ -26,7 +26,7 @@ void LineFollower::lineFollow(){
 	      lineCount++;
 	      canCountLine = false;
 	    }
-	    Serial.println("Line Count: " + String(lineCount));
+	    //Serial.println("Line Count: " + String(lineCount));
 	  }
 
 	  else if(leftSensorValue < ON_BLACK && rightSensorValue >= ON_BLACK){
diff --git a/StudentsRobot.cpp b/StudentsRobot.cpp
index fcef97e..29065b6 100644
--- a/StudentsRobot.cpp
+++ b/StudentsRobot.cpp
@@ -177,57 +177,84 @@ void StudentsRobot::updateStateMachine() {
 		// in safe mode
 		break;
 	case Testing:
-		if(millis() - startTime < 20000){
-			lineSensor.lineFollow();
-//		    int leftSensorValue = analogRead(LEFT_LINE_SENSOR);
-//		    int rightSensorValue = analogRead(RIGHT_LINE_SENSOR);
-//
-//		    Serial.println(String(leftSensorValue) + " " + String(rightSensorValue) + "\r\n");
+// CLAMPED CONTROL
+//		DrivingStatus statusOfForward = robotChassis.turnToHeading(90, 2000);
+//		if(statusOfForward == REACHED_SETPOINT){
+//			status = Running;
+//		}
+//		else if(statusOfForward == TIMED_OUT){
+//			status = Running;
+//		}
+/// LINE FOLLOWING
+		static bool foundCol = false;
+		static bool turnedToBin = false;
+		if(millis() - startTime < 15000){
+			if(!foundCol){
+			    lineSensor.lineFollow();
+			    if(lineSensor.lineCount == 2){
+			    	foundCol = true;
+			    }
+			}
+			if(foundCol && !turnedToBin){
+				if(robotChassis.turnToHeading(90, 1000) == REACHED_SETPOINT){
+					turnedToBin = true;
+				}
+			}
+////		    int leftSensorValue = analogRead(LEFT_LINE_SENSOR);
+////		    int rightSensorValue = analogRead(RIGHT_LINE_SENSOR);
+////
+////		    Serial.println(String(leftSensorValue) + " " + String(rightSensorValue) + "\r\n");
 		}
 		else{
-			robotChassis.stop();
+		   Serial.println("Line Count:" + String(lineSensor.lineCount) + "\r\n");
+		   robotChassis.stop();
+		   lineSensor.resetLineCount();
+		   foundCol = false;
+		   turnedToBin = false;
 		   status = Running;
 		}
 
-//		static bool movedForward = false;
-//		static bool movedBack    = false;
-//	    static bool turnedRight  = false;
-//		static bool turnedLeft   = false;
-//		static bool backToZero   = false;
-//
-//		if(!movedForward){
-//			if(robotChassis.driveForward(300, 5000) == REACHED_SETPOINT){
-//				movedForward = true;
-//			}
-//		}
-//		else if(movedForward && !movedBack){
-//				if(robotChassis.driveBackwards(300, 5000) == REACHED_SETPOINT){
-//					movedBack = true;
-//				}
-//	    }
-//		else if(movedBack && !turnedRight){
-//			if(robotChassis.turnToHeading(-90, 5000) == REACHED_SETPOINT){
-//				turnedRight = true;
-//			}
-//		}
-//		else if(turnedRight && !turnedLeft){
-//			if(robotChassis.turnToHeading(90, 5000) == REACHED_SETPOINT){
-//				turnedLeft = true;
-//			}
-//		}
-//		else if(turnedLeft && !backToZero){
-//			if(robotChassis.turnToHeading(0, 5000) == REACHED_SETPOINT){
-//				backToZero = true;
-//				status = Running;
-//				movedBack = false;
-//				movedForward = false;
-//				turnedLeft = false;
-//				turnedRight = false;
-//				backToZero  = false;
-//			}
-//		}
-//
-//		break;
+// BASIC MOTION
+/*
+		static bool movedForward = false;
+		static bool movedBack    = false;
+	    static bool turnedRight  = false;
+		static bool turnedLeft   = false;
+		static bool backToZero   = false;
+
+		if(!movedForward){
+			if(robotChassis.driveForward(300, 5000) == REACHED_SETPOINT){
+				movedForward = true;
+			}
+		}
+		else if(movedForward && !movedBack){
+				if(robotChassis.driveBackwards(300, 5000) == REACHED_SETPOINT){
+					movedBack = true;
+				}
+	    }
+		else if(movedBack && !turnedRight){
+			if(robotChassis.turnToHeading(-90, 5000) == REACHED_SETPOINT){
+				turnedRight = true;
+			}
+		}
+		else if(turnedRight && !turnedLeft){
+			if(robotChassis.turnToHeading(90, 5000) == REACHED_SETPOINT){
+				turnedLeft = true;
+			}
+		}
+		else if(turnedLeft && !backToZero){
+			if(robotChassis.turnToHeading(0, 5000) == REACHED_SETPOINT){
+				backToZero = true;
+				status = Running;
+				movedBack = false;
+				movedForward = false;
+				turnedLeft = false;
+				turnedRight = false;
+				backToZero  = false;
+			}
+		}
+*/
+		break;
 
 	}
 	digitalWrite(WII_CONTROLLER_DETECT, 0);

From 750d6b7f8acf0f2524a9a1643f286a59dd092cfa Mon Sep 17 00:00:00 2001
From: gentov <33622808+gentov@users.noreply.github.com>
Date: Tue, 20 Oct 2020 19:01:58 -0400
Subject: [PATCH 10/19] cleaned up logic of line following

---
 LineFollower.cpp  | 11 ++++-------
 StudentsRobot.cpp | 25 +++++++++++++------------
 2 files changed, 17 insertions(+), 19 deletions(-)

diff --git a/LineFollower.cpp b/LineFollower.cpp
index a2fa6dc..7285a45 100644
--- a/LineFollower.cpp
+++ b/LineFollower.cpp
@@ -32,8 +32,8 @@ void LineFollower::lineFollow(){
 	  else if(leftSensorValue < ON_BLACK && rightSensorValue >= ON_BLACK){
 	    // turn right
 	    //Serial.println("Turning Right");
-	    leftCorrection = 50;
 	    rightCorrection = -50;
+	    leftCorrection = -50;
 	    canCountLine = true;
 	  }
 
@@ -41,7 +41,7 @@ void LineFollower::lineFollow(){
 	    // turn left
 	    //Serial.println("Turning Left");
 	    rightCorrection = 50;
-	    leftCorrection = -50;
+	    leftCorrection = 50;
 	    canCountLine = true;
 	  }
 
@@ -51,14 +51,11 @@ void LineFollower::lineFollow(){
 	  }
 	  //Serial.println("giving vel command");
 	  // Only works backwards
-	  robotChassis->myleft -> setVelocityDegreesPerSecond(100*MM_TO_WHEEL_DEGREES + rightCorrection);
-      robotChassis->myright -> setVelocityDegreesPerSecond(-100*MM_TO_WHEEL_DEGREES - leftCorrection);
-	  //robotChassis->myright -> setVelocityDegreesPerSecond(100*MM_TO_WHEEL_DEGREES + leftCorrection);
-	  //robotChassis->myleft -> setVelocityDegreesPerSecond(-100*MM_TO_WHEEL_DEGREES - rightCorrection);
+	  robotChassis->myleft -> setVelocityDegreesPerSecond(100*MM_TO_WHEEL_DEGREES + leftCorrection);
+      robotChassis->myright -> setVelocityDegreesPerSecond(-100*MM_TO_WHEEL_DEGREES + rightCorrection);
 	  // if not timeout
 	    // set velocity
 }
-
 void LineFollower::resetLineCount(){
 	lineCount = 0;
 }
diff --git a/StudentsRobot.cpp b/StudentsRobot.cpp
index 29065b6..e5838bd 100644
--- a/StudentsRobot.cpp
+++ b/StudentsRobot.cpp
@@ -188,18 +188,19 @@ void StudentsRobot::updateStateMachine() {
 /// LINE FOLLOWING
 		static bool foundCol = false;
 		static bool turnedToBin = false;
-		if(millis() - startTime < 15000){
-			if(!foundCol){
-			    lineSensor.lineFollow();
-			    if(lineSensor.lineCount == 2){
-			    	foundCol = true;
-			    }
-			}
-			if(foundCol && !turnedToBin){
-				if(robotChassis.turnToHeading(90, 1000) == REACHED_SETPOINT){
-					turnedToBin = true;
-				}
-			}
+		if(millis() - startTime < 10000){
+			lineSensor.lineFollow();
+//			if(!foundCol){
+//			    lineSensor.lineFollow();
+//			    if(lineSensor.lineCount == 2){
+//			    	foundCol = true;
+//			    }
+//			}
+//			if(foundCol && !turnedToBin){
+//				if(robotChassis.turnToHeading(90, 1000) == REACHED_SETPOINT){
+//					turnedToBin = true;
+//				}
+//			}
 ////		    int leftSensorValue = analogRead(LEFT_LINE_SENSOR);
 ////		    int rightSensorValue = analogRead(RIGHT_LINE_SENSOR);
 ////

From e426beba564d691e5ea4a87d81459079cf8e6853 Mon Sep 17 00:00:00 2001
From: gentov <33622808+gentov@users.noreply.github.com>
Date: Wed, 21 Oct 2020 16:48:10 -0400
Subject: [PATCH 11/19] made line following P controlled

Going to delete the comments later
---
 LineFollower.cpp  | 58 ++++++++++++++++++++++++++++++-----------------
 LineFollower.h    |  4 +++-
 StudentsRobot.cpp |  2 +-
 3 files changed, 41 insertions(+), 23 deletions(-)

diff --git a/LineFollower.cpp b/LineFollower.cpp
index 7285a45..d915230 100644
--- a/LineFollower.cpp
+++ b/LineFollower.cpp
@@ -16,8 +16,8 @@ void LineFollower::lineFollow(){
 	  // we need the timeout cause otherwise
 	  int leftSensorValue = analogRead(LEFT_LINE_SENSOR);
 	  int rightSensorValue = analogRead(RIGHT_LINE_SENSOR);
-	  int leftCorrection = 0;
-	  int rightCorrection = 0;
+	  float leftCorrection = 0;
+	  float rightCorrection = 0;
 
 	  if(leftSensorValue >= ON_BLACK && rightSensorValue>= ON_BLACK)
 	  {
@@ -29,30 +29,46 @@ void LineFollower::lineFollow(){
 	    //Serial.println("Line Count: " + String(lineCount));
 	  }
 
-	  else if(leftSensorValue < ON_BLACK && rightSensorValue >= ON_BLACK){
-	    // turn right
-	    //Serial.println("Turning Right");
-	    rightCorrection = -50;
-	    leftCorrection = -50;
-	    canCountLine = true;
-	  }
 
-	  else if(leftSensorValue >= ON_BLACK && rightSensorValue < ON_BLACK){
-	    // turn left
-	    //Serial.println("Turning Left");
-	    rightCorrection = 50;
-	    leftCorrection = 50;
-	    canCountLine = true;
+	  else if(leftSensorValue >= ON_BLACK || rightSensorValue >= ON_BLACK){
+			rightCorrection = (ON_BLACK - rightSensorValue)*lineFollowingKp;
+			leftCorrection =  (leftSensorValue - ON_BLACK)*lineFollowingKp;
 	  }
 
-	  else{
-	    //Serial.println("Straddling Line");
-	    canCountLine = true;
-	  }
+//	  else if(leftSensorValue < ON_BLACK && rightSensorValue >= ON_BLACK){
+//			rightCorrection = -(rightSensorValue - ON_BLACK)*lineFollowingKp;
+//			leftCorrection =  -(ON_BLACK - leftSensorValue)*lineFollowingKp;
+//	  }
+
+//	  else if(leftSensorValue < ON_BLACK && rightSensorValue >= ON_BLACK){
+//	    // turn right
+////		rightCorrection = (ON_BLACK - rightCorrection)*.05;
+////		leftCorrection = (leftSensorValue - ON_BLACK)*.05;
+//	    rightCorrection = -50;
+//	    leftCorrection = -50;
+//	    canCountLine = true;
+//	  }
+////
+//	  else if(leftSensorValue >= ON_BLACK && rightSensorValue < ON_BLACK){
+//	    // turn left
+//	    //Serial.println("Turning Left");
+//	    rightCorrection = 50;
+//	    leftCorrection = 50;
+////		rightCorrection = (rightSensorValue - ON_BLACK)*.05;
+////		leftCorrection = (ON_BLACK - leftSensorValue)*.05;
+//	    canCountLine = true;
+//	  }
+//
+//	  else{
+//	    //Serial.println("Straddling Line");
+//	    canCountLine = true;
+//	  }
 	  //Serial.println("giving vel command");
 	  // Only works backwards
-	  robotChassis->myleft -> setVelocityDegreesPerSecond(100*MM_TO_WHEEL_DEGREES + leftCorrection);
-      robotChassis->myright -> setVelocityDegreesPerSecond(-100*MM_TO_WHEEL_DEGREES + rightCorrection);
+	  Serial.println("Left Correction: " + String(leftCorrection));
+	  Serial.println("Right Correction: " + String(rightCorrection));
+	  robotChassis->myleft -> setVelocityDegreesPerSecond(lineFollowingSpeed_mm_per_sec*MM_TO_WHEEL_DEGREES + leftCorrection);
+      robotChassis->myright -> setVelocityDegreesPerSecond(-lineFollowingSpeed_mm_per_sec*MM_TO_WHEEL_DEGREES + rightCorrection);
 	  // if not timeout
 	    // set velocity
 }
diff --git a/LineFollower.h b/LineFollower.h
index e937556..b973135 100644
--- a/LineFollower.h
+++ b/LineFollower.h
@@ -15,7 +15,9 @@ class LineFollower{
     public:
 	   LineFollower(DrivingChassis* myChassis);
 	   // on black line
-	   const int ON_BLACK = 3500;
+	   const int ON_BLACK = 3625;//3750;
+	   int lineFollowingSpeed_mm_per_sec = 175;
+	   float lineFollowingKp = .002*lineFollowingSpeed_mm_per_sec;
 	   int lineCount = 0;
 	   bool canCountLine = true;
        DrivingChassis* robotChassis;
diff --git a/StudentsRobot.cpp b/StudentsRobot.cpp
index e5838bd..474956d 100644
--- a/StudentsRobot.cpp
+++ b/StudentsRobot.cpp
@@ -188,7 +188,7 @@ void StudentsRobot::updateStateMachine() {
 /// LINE FOLLOWING
 		static bool foundCol = false;
 		static bool turnedToBin = false;
-		if(millis() - startTime < 10000){
+		if(lineSensor.lineCount == 0){
 			lineSensor.lineFollow();
 //			if(!foundCol){
 //			    lineSensor.lineFollow();

From bbc7e7348747e2c13f1061d6d537409d610556e9 Mon Sep 17 00:00:00 2001
From: gentov <33622808+gentov@users.noreply.github.com>
Date: Wed, 21 Oct 2020 17:04:33 -0400
Subject: [PATCH 12/19] a bit of tuning

---
 LineFollower.cpp | 2 --
 LineFollower.h   | 2 +-
 2 files changed, 1 insertion(+), 3 deletions(-)

diff --git a/LineFollower.cpp b/LineFollower.cpp
index d915230..489348b 100644
--- a/LineFollower.cpp
+++ b/LineFollower.cpp
@@ -65,8 +65,6 @@ void LineFollower::lineFollow(){
 //	  }
 	  //Serial.println("giving vel command");
 	  // Only works backwards
-	  Serial.println("Left Correction: " + String(leftCorrection));
-	  Serial.println("Right Correction: " + String(rightCorrection));
 	  robotChassis->myleft -> setVelocityDegreesPerSecond(lineFollowingSpeed_mm_per_sec*MM_TO_WHEEL_DEGREES + leftCorrection);
       robotChassis->myright -> setVelocityDegreesPerSecond(-lineFollowingSpeed_mm_per_sec*MM_TO_WHEEL_DEGREES + rightCorrection);
 	  // if not timeout
diff --git a/LineFollower.h b/LineFollower.h
index b973135..b9e9622 100644
--- a/LineFollower.h
+++ b/LineFollower.h
@@ -15,7 +15,7 @@ class LineFollower{
     public:
 	   LineFollower(DrivingChassis* myChassis);
 	   // on black line
-	   const int ON_BLACK = 3625;//3750;
+	   const int ON_BLACK = 3692;//3750;
 	   int lineFollowingSpeed_mm_per_sec = 175;
 	   float lineFollowingKp = .002*lineFollowingSpeed_mm_per_sec;
 	   int lineCount = 0;

From 8763aaab775b15bef7a0d2740e08cab8a23e88eb Mon Sep 17 00:00:00 2001
From: gentov <33622808+gentov@users.noreply.github.com>
Date: Thu, 22 Oct 2020 01:59:09 -0400
Subject: [PATCH 13/19] started navigation + bug fixes

1) Found bug in drive forward/backward
2) Started navigation routine
3) pose class
---
 DrivingChassis.cpp |  26 +++--
 DrivingChassis.h   |  12 ++-
 LineFollower.cpp   |  54 +++-------
 LineFollower.h     |   2 +-
 Pose.cpp           |  13 +++
 Pose.h             |  26 +++++
 StudentsRobot.cpp  | 254 ++++++++++++++++++++++++++++++++++-----------
 StudentsRobot.h    |  18 ++++
 8 files changed, 287 insertions(+), 118 deletions(-)
 create mode 100644 Pose.cpp
 create mode 100644 Pose.h

diff --git a/DrivingChassis.cpp b/DrivingChassis.cpp
index 55b25a8..2975a0f 100644
--- a/DrivingChassis.cpp
+++ b/DrivingChassis.cpp
@@ -120,15 +120,17 @@ DrivingStatus DrivingChassis::driveForward(float mmDistanceFromCurrent, int msDu
 	    	float leftMotorEffort_deg_per_sec = wheelMovementKp*leftWheelError_mm;
 
 	    	//clamps speed to 20 cm per second
-	    	if(rightMotorEffort_deg_per_sec > (MAX_SPEED_MM_PER_SEC * MM_TO_WHEEL_DEGREES)){
-	    		rightMotorEffort_deg_per_sec = MAX_SPEED_MM_PER_SEC*MM_TO_WHEEL_DEGREES;
+	    	if(fabs(rightMotorEffort_deg_per_sec) > (MAX_SPEED_MM_PER_SEC * MM_TO_WHEEL_DEGREES)){
+	    		rightMotorEffort_deg_per_sec = -MAX_SPEED_MM_PER_SEC*MM_TO_WHEEL_DEGREES;
 	    	}
-	    	if(leftMotorEffort_deg_per_sec > (MAX_SPEED_MM_PER_SEC * MM_TO_WHEEL_DEGREES)){
-	    		leftMotorEffort_deg_per_sec = MAX_SPEED_MM_PER_SEC*MM_TO_WHEEL_DEGREES;
+	    	if(fabs(leftMotorEffort_deg_per_sec) > (MAX_SPEED_MM_PER_SEC * MM_TO_WHEEL_DEGREES)){
+	    		leftMotorEffort_deg_per_sec = -MAX_SPEED_MM_PER_SEC*MM_TO_WHEEL_DEGREES;
 	        }
 	    	myright -> setVelocityDegreesPerSecond(-rightMotorEffort_deg_per_sec);
 	    	myleft -> setVelocityDegreesPerSecond(leftMotorEffort_deg_per_sec);
 
+//	    	Serial.println("Left Error: " + String(leftWheelError_mm) + "\r\n" );
+//	    	Serial.println("Right Error: " + String(rightWheelError_mm) + "\r\n" );
 //	    	Serial.println("Left Effort: " + String(leftMotorEffort_deg_per_sec) + "\r\n" );
 //	    	Serial.println("Right Effort: " + String(rightMotorEffort_deg_per_sec) + "\r\n" );
 	    }
@@ -196,17 +198,19 @@ DrivingStatus DrivingChassis::driveBackwards(float mmDistanceFromCurrent, int ms
 		    	float leftMotorEffort_deg_per_sec = wheelMovementKp*leftWheelError_mm;
 
 		    	//clamps speed to 20 cm per second
-		    	if(rightMotorEffort_deg_per_sec > (MAX_SPEED_MM_PER_SEC * MM_TO_WHEEL_DEGREES)){
-		    		rightMotorEffort_deg_per_sec = MAX_SPEED_MM_PER_SEC*MM_TO_WHEEL_DEGREES;
+		    	if(fabs(rightMotorEffort_deg_per_sec) > (MAX_SPEED_MM_PER_SEC * MM_TO_WHEEL_DEGREES)){
+		    		rightMotorEffort_deg_per_sec = -MAX_SPEED_MM_PER_SEC*MM_TO_WHEEL_DEGREES;
 		    	}
-		    	if(leftMotorEffort_deg_per_sec > (MAX_SPEED_MM_PER_SEC * MM_TO_WHEEL_DEGREES)){
-		    		leftMotorEffort_deg_per_sec = MAX_SPEED_MM_PER_SEC*MM_TO_WHEEL_DEGREES;
+		    	if(fabs(leftMotorEffort_deg_per_sec) > (MAX_SPEED_MM_PER_SEC * MM_TO_WHEEL_DEGREES)){
+		    		leftMotorEffort_deg_per_sec = -MAX_SPEED_MM_PER_SEC*MM_TO_WHEEL_DEGREES;
 		        }
 		    	myright -> setVelocityDegreesPerSecond(rightMotorEffort_deg_per_sec);
 		    	myleft -> setVelocityDegreesPerSecond(-leftMotorEffort_deg_per_sec);
 
-	//	    	Serial.println("Left Effort: " + String(leftMotorEffort_deg_per_sec) + "\r\n" );
-	//	    	Serial.println("Right Effort: " + String(rightMotorEffort_deg_per_sec) + "\r\n" );
+//		    	Serial.println("Left Error: " + String(leftWheelError_mm) + "\r\n" );
+//		    	Serial.println("Right Error: " + String(rightWheelError_mm) + "\r\n" );
+//		    	Serial.println("Left Effort: " + String(leftMotorEffort_deg_per_sec) + "\r\n" );
+//		    	Serial.println("Right Effort: " + String(rightMotorEffort_deg_per_sec) + "\r\n" );
 		    }
 	    }
 
@@ -261,9 +265,9 @@ DrivingStatus DrivingChassis::turnToHeading(float degreesToRotateBase, int msDur
     }
 
 	float currentHeading = IMU->getEULER_azimuth();
-
 	float headingError = - currentHeading - degreesToRotateBase;
     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");
diff --git a/DrivingChassis.h b/DrivingChassis.h
index c582f57..6ecb1ed 100644
--- a/DrivingChassis.h
+++ b/DrivingChassis.h
@@ -10,12 +10,13 @@
 #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 200
-#define MAX_MOTOR_EFFORT_DURING_TURN 500
+#define MAX_SPEED_MM_PER_SEC 180
+#define MAX_MOTOR_EFFORT_DURING_TURN 300 //275 // 500
 
 
 /**
@@ -79,10 +80,11 @@ class DrivingChassis {
 	PIDMotor * myright;
 	bool performingMovement = false;
 	unsigned long startTimeOfMovement_ms;
-	float wheelMovementKp = 3.9;// was 3.5
-	float turningMovementKp = 9;
-	int wheelMovementDeadband_mm = 2.5;
+	float wheelMovementKp = 4.1;// was 3.9
+	float turningMovementKp = 13; //was 9, 11.7
+	float wheelMovementDeadband_mm = 2.5;
 	float wheelMovementDeadband_deg = .5;
+	Pose myChassisPose;
 
 	virtual ~DrivingChassis();
 
diff --git a/LineFollower.cpp b/LineFollower.cpp
index 489348b..da4e364 100644
--- a/LineFollower.cpp
+++ b/LineFollower.cpp
@@ -6,24 +6,29 @@
  */
 
 #include "LineFollower.h"
+#include <math.h>
+
+#define PI 3.14159265f
 
 LineFollower::LineFollower(DrivingChassis* myChassis){
      this->robotChassis = myChassis;
 }
 
 void LineFollower::lineFollow(){
-	// line following. if both sensors are on white full steam ahead but timeout eventually.
-	  // we need the timeout cause otherwise
 	  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)
 	  {
-	    // reset the timeout timer.
-	    if(canCountLine){
+	   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));
@@ -33,44 +38,15 @@ void LineFollower::lineFollow(){
 	  else if(leftSensorValue >= ON_BLACK || rightSensorValue >= ON_BLACK){
 			rightCorrection = (ON_BLACK - rightSensorValue)*lineFollowingKp;
 			leftCorrection =  (leftSensorValue - ON_BLACK)*lineFollowingKp;
+			canCountLine = true;
+	  }
+	  else{
+		  canCountLine = true;
 	  }
-
-//	  else if(leftSensorValue < ON_BLACK && rightSensorValue >= ON_BLACK){
-//			rightCorrection = -(rightSensorValue - ON_BLACK)*lineFollowingKp;
-//			leftCorrection =  -(ON_BLACK - leftSensorValue)*lineFollowingKp;
-//	  }
-
-//	  else if(leftSensorValue < ON_BLACK && rightSensorValue >= ON_BLACK){
-//	    // turn right
-////		rightCorrection = (ON_BLACK - rightCorrection)*.05;
-////		leftCorrection = (leftSensorValue - ON_BLACK)*.05;
-//	    rightCorrection = -50;
-//	    leftCorrection = -50;
-//	    canCountLine = true;
-//	  }
-////
-//	  else if(leftSensorValue >= ON_BLACK && rightSensorValue < ON_BLACK){
-//	    // turn left
-//	    //Serial.println("Turning Left");
-//	    rightCorrection = 50;
-//	    leftCorrection = 50;
-////		rightCorrection = (rightSensorValue - ON_BLACK)*.05;
-////		leftCorrection = (ON_BLACK - leftSensorValue)*.05;
-//	    canCountLine = true;
-//	  }
-//
-//	  else{
-//	    //Serial.println("Straddling Line");
-//	    canCountLine = true;
-//	  }
-	  //Serial.println("giving vel command");
-	  // Only works backwards
 	  robotChassis->myleft -> setVelocityDegreesPerSecond(lineFollowingSpeed_mm_per_sec*MM_TO_WHEEL_DEGREES + leftCorrection);
       robotChassis->myright -> setVelocityDegreesPerSecond(-lineFollowingSpeed_mm_per_sec*MM_TO_WHEEL_DEGREES + rightCorrection);
-	  // if not timeout
-	    // set velocity
 }
 void LineFollower::resetLineCount(){
-	lineCount = 0;
+    lineCount = 0;
 }
 
diff --git a/LineFollower.h b/LineFollower.h
index b9e9622..cda9778 100644
--- a/LineFollower.h
+++ b/LineFollower.h
@@ -19,8 +19,8 @@ class LineFollower{
 	   int lineFollowingSpeed_mm_per_sec = 175;
 	   float lineFollowingKp = .002*lineFollowingSpeed_mm_per_sec;
 	   int lineCount = 0;
-	   bool canCountLine = true;
        DrivingChassis* robotChassis;
+       bool canCountLine = true;
 
 	   void lineFollow();
 	   void resetLineCount();
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..1e38a86
--- /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. I have
+	   // opted to use solid intervals of 90 to make the math cleaner for determining row
+	   // and column.
+	   float heading = 0;
+
+    private:
+};
+
+
+
+#endif /* POSE_H_ */
diff --git a/StudentsRobot.cpp b/StudentsRobot.cpp
index 474956d..565b12c 100644
--- a/StudentsRobot.cpp
+++ b/StudentsRobot.cpp
@@ -186,75 +186,205 @@ void StudentsRobot::updateStateMachine() {
 //			status = Running;
 //		}
 /// LINE FOLLOWING
-		static bool foundCol = false;
-		static bool turnedToBin = false;
-		if(lineSensor.lineCount == 0){
-			lineSensor.lineFollow();
-//			if(!foundCol){
-//			    lineSensor.lineFollow();
-//			    if(lineSensor.lineCount == 2){
-//			    	foundCol = true;
-//			    }
-//			}
-//			if(foundCol && !turnedToBin){
-//				if(robotChassis.turnToHeading(90, 1000) == REACHED_SETPOINT){
-//					turnedToBin = true;
-//				}
-//			}
-////		    int leftSensorValue = analogRead(LEFT_LINE_SENSOR);
-////		    int rightSensorValue = analogRead(RIGHT_LINE_SENSOR);
-////
-////		    Serial.println(String(leftSensorValue) + " " + String(rightSensorValue) + "\r\n");
-		}
-		else{
-		   Serial.println("Line Count:" + String(lineSensor.lineCount) + "\r\n");
-		   robotChassis.stop();
-		   lineSensor.resetLineCount();
-		   foundCol = false;
-		   turnedToBin = false;
-		   status = Running;
-		}
-
-// BASIC MOTION
-/*
-		static bool movedForward = false;
-		static bool movedBack    = false;
-	    static bool turnedRight  = false;
-		static bool turnedLeft   = false;
-		static bool backToZero   = false;
+//		if(lineSensor.lineCount == 0){
+//			lineSensor.lineFollow();
+//		}
+//		else{
+//		   if(robotChassis.turnToHeading(180, 5000) == REACHED_SETPOINT){
+//		   Serial.println("Line Count:" + String(lineSensor.lineCount) + "\r\n");
+//		   Serial.println("Pose Row:" + String(robotChassis.myChassisPose.currentRow) + "\r\n");
+//		   Serial.println("Pose Column:" + String(robotChassis.myChassisPose.currentColumn) + "\r\n");
+//		   Serial.println("Pose Heading:" + String(robotChassis.myChassisPose.heading) + "\r\n");
+//		   robotChassis.stop();
+//		   lineSensor.resetLineCount();
+//		   status = Running;
+//		   }
+//		}
 
-		if(!movedForward){
-			if(robotChassis.driveForward(300, 5000) == REACHED_SETPOINT){
-				movedForward = true;
+// Navigation
+		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(robotChassis.myChassisPose.currentColumn == 0){
+				navState = TURN_TOWARDS_CORRECT_ROW;
 			}
-		}
-		else if(movedForward && !movedBack){
-				if(robotChassis.driveBackwards(300, 5000) == REACHED_SETPOINT){
-					movedBack = true;
+			else{
+				navState = TURN_TOWARDS_CORRECT_COLUMN;
+			}
+			break;
+		case TURN_TOWARDS_CORRECT_COLUMN:
+			Serial.println("TURNING TOWARDS COLUMN");
+			// determine what is our first state
+		    if(robotChassis.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
+			    if(robotChassis.turnToHeading(90, 2500) == REACHED_SETPOINT){
+				     navState = FINDING_OUTER_EDGE;
+			    }
+			}
+			else{
+				// otherwise, we just need to find the right column
+				if(robotChassis.myChassisPose.currentColumn > goalColumn){
+					if(robotChassis.turnToHeading(-90, 2500) == REACHED_SETPOINT){
+						navState = FINDING_COLUMN;
+					}
+				}
+				else{
+					if(robotChassis.turnToHeading(90, 2500) == REACHED_SETPOINT){
+						navState = FINDING_COLUMN;
+					}
 				}
-	    }
-		else if(movedBack && !turnedRight){
-			if(robotChassis.turnToHeading(-90, 5000) == REACHED_SETPOINT){
-				turnedRight = true;
 			}
-		}
-		else if(turnedRight && !turnedLeft){
-			if(robotChassis.turnToHeading(90, 5000) == REACHED_SETPOINT){
-				turnedLeft = true;
+			break;
+		case FINDING_OUTER_EDGE:
+			Serial.println("FINDING COL: 0, CURRENT COL: " + String(robotChassis.myChassisPose.currentColumn));
+		    // navigate until column == 0
+			if(robotChassis.myChassisPose.currentColumn != 0){
+				// if the column is wrong.
+				lineSensor.lineFollow();
 			}
-		}
-		else if(turnedLeft && !backToZero){
-			if(robotChassis.turnToHeading(0, 5000) == REACHED_SETPOINT){
-				backToZero = true;
-				status = Running;
-				movedBack = false;
-				movedForward = false;
-				turnedLeft = false;
-				turnedRight = false;
-				backToZero  = false;
+			else{
+                if(robotChassis.driveBackwards(210, 2500) == REACHED_SETPOINT){
+                	robotChassis.stop();
+                	navState = TURN_TOWARDS_CORRECT_ROW;
+                }
+			}
+			break;
+		case FINDING_ROW:
+			Serial.println("FINDING ROW: " + String(goalRow) +  "CURRENT ROW: " + String(robotChassis.myChassisPose.currentRow));
+			if(robotChassis.myChassisPose.currentRow != goalRow){
+				// if the row is wrong.
+				lineSensor.lineFollow();
+			}
+			else{
+				robotChassis.stop();
+				if(goalColumn != 0){
+				    if(robotChassis.driveBackwards(210, 2500) == REACHED_SETPOINT){
+					    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(robotChassis.myChassisPose.currentRow > goalRow){
+				if(robotChassis.turnToHeading(180, 2500) == REACHED_SETPOINT){
+					navState = FINDING_ROW;
+				}
+			}
+			else{
+				if(robotChassis.turnToHeading(0, 2500) == REACHED_SETPOINT){
+					navState = FINDING_ROW;
+				}
+			}
+			break;
+		case FINDING_COLUMN:
+			Serial.println("FINDING COL: " + String(goalColumn) +  "CURRENT COL: " + String(robotChassis.myChassisPose.currentColumn));
+			if(robotChassis.myChassisPose.currentColumn != goalColumn){
+				// if the row is wrong.
+				lineSensor.lineFollow();
+			}
+			else{
+				if(robotChassis.driveBackwards(210, 2500) == REACHED_SETPOINT){
+					//goalRow = 2;
+					//goalColumn = -1;
+					goalRow = 3;
+					goalColumn = 0;
+					robotChassis.stop();
+				    navState = INITIALIZE_NAVIGATION;
+				}
+			}
+			break;
+		case FINISHED:
+			break;
 		}
-*/
+/// POSE TRACKING
+
+//	   static int testCase = 1;
+//       switch(testCase){
+//          case 1:
+//        	  if(robotChassis.myChassisPose.currentRow != 1){
+//        	      lineSensor.lineFollow();
+//        	  }
+//        	  else{
+//			      robotChassis.stop();
+//			      testCase = 2;
+//			  }
+//        	  break;
+//          case 2:
+//        	  if(robotChassis.driveBackwards(200, 1500) == REACHED_SETPOINT){
+//        		  testCase = 3;
+//        		  lineSensor.canCountLine = true;
+//        	  }
+//        	  break;
+//          case 3:
+//        	  // turn right 90
+//        	  if(robotChassis.turnToHeading(-90, 1500) == REACHED_SETPOINT){
+//        		  testCase = 4;
+//        	  }
+//        	  break;
+//          case 4:
+//        	  // line follow until column
+//        	  if(robotChassis.myChassisPose.currentColumn != -1){
+//        	      lineSensor.lineFollow();
+//        	  }
+//        	  else{
+//        	      robotChassis.stop();
+//        	      testCase = 5;
+//        	  }
+//        	  break;
+//          case 5:
+//        	  if(robotChassis.turnToHeading(90, 1500) == REACHED_SETPOINT){
+//        	      Serial.println("Pose Row:" + String(robotChassis.myChassisPose.currentRow) + "\r\n");
+//        	      Serial.println("Pose Column:" + String(robotChassis.myChassisPose.currentColumn) + "\r\n");
+//        	      Serial.println("Pose Heading:" + String(robotChassis.myChassisPose.heading) + "\r\n");
+//        	      status = Running;
+//        	      testCase = 1;
+//        	  }
+//        	  break;
+//       }
+// BASIC MOTION
+
+//		static bool movedForward = false;
+//		static bool movedBack    = false;
+//	    static bool turnedRight  = false;
+//		static bool turnedLeft   = false;
+//		static bool backToZero   = false;
+//
+//		if(!movedForward){
+//			if(robotChassis.driveForward(300, 5000) == REACHED_SETPOINT){
+//				movedForward = true;
+//			}
+//		}
+//		else if(movedForward && !movedBack){
+//				if(robotChassis.driveBackwards(300, 5000) == REACHED_SETPOINT){
+//					movedBack = true;
+//				}
+//	    }
+//		else if(movedBack && !turnedRight){
+//			if(robotChassis.turnToHeading(-90, 5000) == REACHED_SETPOINT){
+//				turnedRight = true;
+//			}
+//		}
+//		else if(turnedRight && !turnedLeft){
+//			if(robotChassis.turnToHeading(90, 5000) == REACHED_SETPOINT){
+//				turnedLeft = true;
+//			}
+//		}
+//		else if(turnedLeft && !backToZero){
+//			if(robotChassis.turnToHeading(0, 5000) == REACHED_SETPOINT){
+//				backToZero = true;
+//				status = Running;
+//				movedBack = false;
+//				movedForward = false;
+//				turnedLeft = false;
+//				turnedRight = false;
+//				backToZero  = false;
+//			}
+//		}
 		break;
 
 	}
diff --git a/StudentsRobot.h b/StudentsRobot.h
index 14169c6..6ad25e9 100644
--- a/StudentsRobot.h
+++ b/StudentsRobot.h
@@ -16,6 +16,7 @@
 
 #include "DrivingChassis.h"
 #include "LineFollower.h"
+#include "Pose.h"
 #include "src/commands/IRCamSimplePacketComsServer.h"
 #include "src/commands/GetIMU.h"
 
@@ -50,6 +51,17 @@ enum ComStackStatusState {
 	Fault_obstructed_path = 11,
 	Fault_E_Stop_pressed = 12
 };
+
+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,
+	FINISHED = 6,
+};
+
 /**
  * @class StudentsRobot
  */
@@ -93,6 +105,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

From 39370d02c318fb6c39921de076416fce3890cc9e Mon Sep 17 00:00:00 2001
From: gentov <33622808+gentov@users.noreply.github.com>
Date: Thu, 22 Oct 2020 19:44:19 -0400
Subject: [PATCH 14/19] moved navigation to separate file

had to rewire line sensor to move it to the front instead of thw back. Pushing cause sloeber is being slow.

Navigation in its own file but still buggy
---
 LineFollower.cpp      |  36 +++++++++++-
 LineFollower.h        |   3 +-
 NavigationRoutine.cpp | 104 +++++++++++++++++++++++++++++++++
 NavigationRoutine.h   |  28 +++++++++
 StudentsRobot.cpp     | 131 ++++++------------------------------------
 StudentsRobot.h       |  11 +---
 6 files changed, 188 insertions(+), 125 deletions(-)
 create mode 100644 NavigationRoutine.cpp
 create mode 100644 NavigationRoutine.h

diff --git a/LineFollower.cpp b/LineFollower.cpp
index da4e364..94d1ef8 100644
--- a/LineFollower.cpp
+++ b/LineFollower.cpp
@@ -14,7 +14,7 @@ LineFollower::LineFollower(DrivingChassis* myChassis){
      this->robotChassis = myChassis;
 }
 
-void LineFollower::lineFollow(){
+void LineFollower::lineFollowBackwards(){
 	  int leftSensorValue = analogRead(LEFT_LINE_SENSOR);
 	  int rightSensorValue = analogRead(RIGHT_LINE_SENSOR);
 	  float leftCorrection = 0;
@@ -46,6 +46,40 @@ void LineFollower::lineFollow(){
 	  robotChassis->myleft -> setVelocityDegreesPerSecond(lineFollowingSpeed_mm_per_sec*MM_TO_WHEEL_DEGREES + leftCorrection);
       robotChassis->myright -> setVelocityDegreesPerSecond(-lineFollowingSpeed_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
+	      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)*lineFollowingKp;
+			leftCorrection =  (leftSensorValue - ON_BLACK)*lineFollowingKp;
+			canCountLine = true;
+	  }
+	  else{
+		  canCountLine = true;
+	  }
+	  robotChassis->myleft -> setVelocityDegreesPerSecond(-lineFollowingSpeed_mm_per_sec*MM_TO_WHEEL_DEGREES + leftCorrection);
+      robotChassis->myright -> setVelocityDegreesPerSecond(lineFollowingSpeed_mm_per_sec*MM_TO_WHEEL_DEGREES + rightCorrection);
+}
+
 void LineFollower::resetLineCount(){
     lineCount = 0;
 }
diff --git a/LineFollower.h b/LineFollower.h
index cda9778..9e2e31e 100644
--- a/LineFollower.h
+++ b/LineFollower.h
@@ -22,7 +22,8 @@ class LineFollower{
        DrivingChassis* robotChassis;
        bool canCountLine = true;
 
-	   void lineFollow();
+	   void lineFollowBackwards();
+	   void lineFollowForwards();
 	   void resetLineCount();
 
     private:
diff --git a/NavigationRoutine.cpp b/NavigationRoutine.cpp
new file mode 100644
index 0000000..a4e8b97
--- /dev/null
+++ b/NavigationRoutine.cpp
@@ -0,0 +1,104 @@
+/*
+ * NavigationRoutine.cpp
+ *
+ *  Created on: Oct 22, 2020
+ *      Author: gentov
+ */
+#include "NavigationRoutine.h"
+
+bool navigate(int row, int col, DrivingChassis* drivingChassis, LineFollower* lineSensor){
+	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 != row){
+				// 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
+				if(drivingChassis->turnToHeading(90, 2500) == REACHED_SETPOINT){
+					 navState = FINDING_OUTER_EDGE;
+				}
+			}
+			else{
+				// otherwise, we just need to find the right column
+				if(drivingChassis->myChassisPose.currentColumn > col){
+					if(drivingChassis->turnToHeading(-90, 2500) == REACHED_SETPOINT){
+						navState = FINDING_COLUMN;
+					}
+				}
+				else{
+					if(drivingChassis->turnToHeading(90, 2500) == REACHED_SETPOINT){
+						navState = FINDING_COLUMN;
+					}
+				}
+			}
+			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(row) +  "CURRENT ROW: " + String(drivingChassis->myChassisPose.currentRow));
+			if(drivingChassis->myChassisPose.currentRow != row){
+				// if the row is wrong.
+				lineSensor->lineFollowForwards();
+			}
+			else{
+				drivingChassis->stop();
+				if(col != 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 > row){
+				if(drivingChassis->turnToHeading(180, 2500) == REACHED_SETPOINT){
+					navState = FINDING_ROW;
+				}
+			}
+			else{
+				if(drivingChassis->turnToHeading(0, 2500) == REACHED_SETPOINT){
+					navState = FINDING_ROW;
+				}
+			}
+			break;
+		case FINDING_COLUMN:
+			Serial.println("FINDING COL: " + String(col) +  "CURRENT COL: " + String(drivingChassis->myChassisPose.currentColumn));
+			if(drivingChassis->myChassisPose.currentColumn != col){
+				// if the row is wrong.
+				lineSensor->lineFollowForwards();
+			}
+			else{
+			   drivingChassis->stop();
+               navState = FINISHED;
+			}
+			break;
+		case FINISHED:
+			navState = INITIALIZE_NAVIGATION;
+			return true;
+		}
+	return false;
+}
+
+
+
diff --git a/NavigationRoutine.h b/NavigationRoutine.h
new file mode 100644
index 0000000..eb5037b
--- /dev/null
+++ b/NavigationRoutine.h
@@ -0,0 +1,28 @@
+/*
+ * 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,
+	FINISHED = 6,
+};
+
+bool navigate(int row, int col, DrivingChassis* drivingChassis, LineFollower* lineSensor);
+
+static NavigationStates navState = INITIALIZE_NAVIGATION;
+
+#endif /* NAVIGATIONROUTINE_H_ */
diff --git a/StudentsRobot.cpp b/StudentsRobot.cpp
index 565b12c..8bf41c3 100644
--- a/StudentsRobot.cpp
+++ b/StudentsRobot.cpp
@@ -186,121 +186,26 @@ void StudentsRobot::updateStateMachine() {
 //			status = Running;
 //		}
 /// LINE FOLLOWING
-//		if(lineSensor.lineCount == 0){
-//			lineSensor.lineFollow();
-//		}
-//		else{
-//		   if(robotChassis.turnToHeading(180, 5000) == REACHED_SETPOINT){
-//		   Serial.println("Line Count:" + String(lineSensor.lineCount) + "\r\n");
-//		   Serial.println("Pose Row:" + String(robotChassis.myChassisPose.currentRow) + "\r\n");
-//		   Serial.println("Pose Column:" + String(robotChassis.myChassisPose.currentColumn) + "\r\n");
-//		   Serial.println("Pose Heading:" + String(robotChassis.myChassisPose.heading) + "\r\n");
-//		   robotChassis.stop();
-//		   lineSensor.resetLineCount();
-//		   status = Running;
-//		   }
-//		}
+	    if((millis() - startTime) < 3000){
+			lineSensor.lineFollowForwards();
+		}
+		else{
+		   robotChassis.stop();
+		   lineSensor.resetLineCount();
+		   status = Running;
+		}
 
 // Navigation
-		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(robotChassis.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(robotChassis.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
-			    if(robotChassis.turnToHeading(90, 2500) == REACHED_SETPOINT){
-				     navState = FINDING_OUTER_EDGE;
-			    }
-			}
-			else{
-				// otherwise, we just need to find the right column
-				if(robotChassis.myChassisPose.currentColumn > goalColumn){
-					if(robotChassis.turnToHeading(-90, 2500) == REACHED_SETPOINT){
-						navState = FINDING_COLUMN;
-					}
-				}
-				else{
-					if(robotChassis.turnToHeading(90, 2500) == REACHED_SETPOINT){
-						navState = FINDING_COLUMN;
-					}
-				}
-			}
-			break;
-		case FINDING_OUTER_EDGE:
-			Serial.println("FINDING COL: 0, CURRENT COL: " + String(robotChassis.myChassisPose.currentColumn));
-		    // navigate until column == 0
-			if(robotChassis.myChassisPose.currentColumn != 0){
-				// if the column is wrong.
-				lineSensor.lineFollow();
-			}
-			else{
-                if(robotChassis.driveBackwards(210, 2500) == REACHED_SETPOINT){
-                	robotChassis.stop();
-                	navState = TURN_TOWARDS_CORRECT_ROW;
-                }
-			}
-			break;
-		case FINDING_ROW:
-			Serial.println("FINDING ROW: " + String(goalRow) +  "CURRENT ROW: " + String(robotChassis.myChassisPose.currentRow));
-			if(robotChassis.myChassisPose.currentRow != goalRow){
-				// if the row is wrong.
-				lineSensor.lineFollow();
-			}
-			else{
-				robotChassis.stop();
-				if(goalColumn != 0){
-				    if(robotChassis.driveBackwards(210, 2500) == REACHED_SETPOINT){
-					    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(robotChassis.myChassisPose.currentRow > goalRow){
-				if(robotChassis.turnToHeading(180, 2500) == REACHED_SETPOINT){
-					navState = FINDING_ROW;
-				}
-			}
-			else{
-				if(robotChassis.turnToHeading(0, 2500) == REACHED_SETPOINT){
-					navState = FINDING_ROW;
-				}
-			}
-			break;
-		case FINDING_COLUMN:
-			Serial.println("FINDING COL: " + String(goalColumn) +  "CURRENT COL: " + String(robotChassis.myChassisPose.currentColumn));
-			if(robotChassis.myChassisPose.currentColumn != goalColumn){
-				// if the row is wrong.
-				lineSensor.lineFollow();
-			}
-			else{
-				if(robotChassis.driveBackwards(210, 2500) == REACHED_SETPOINT){
-					//goalRow = 2;
-					//goalColumn = -1;
-					goalRow = 3;
-					goalColumn = 0;
-					robotChassis.stop();
-				    navState = INITIALIZE_NAVIGATION;
-				}
-			}
-			break;
-		case FINISHED:
-			break;
-		}
+//   static bool goingToWayPoint1 = true;
+//
+//   if(goingToWayPoint1){
+//        if(navigate(2, -2, &robotChassis, &lineSensor)){
+//        	Serial.println("reached waypoint 1");
+//        	goingToWayPoint1 = false;
+//        }
+//   }
+//   else
+//	   navigate(3, 0, &robotChassis, &lineSensor);
 /// POSE TRACKING
 
 //	   static int testCase = 1;
diff --git a/StudentsRobot.h b/StudentsRobot.h
index 6ad25e9..a763953 100644
--- a/StudentsRobot.h
+++ b/StudentsRobot.h
@@ -13,6 +13,7 @@
 #include "src/pid/HBridgeEncoderPIDMotor.h"
 #include "src/pid/ServoAnalogPIDMotor.h"
 #include <ESP32Servo.h>
+#include "NavigationRoutine.h"
 
 #include "DrivingChassis.h"
 #include "LineFollower.h"
@@ -52,16 +53,6 @@ enum ComStackStatusState {
 	Fault_E_Stop_pressed = 12
 };
 
-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,
-	FINISHED = 6,
-};
-
 /**
  * @class StudentsRobot
  */

From f75e8672a62274cea408856c12ea1899c61965ee Mon Sep 17 00:00:00 2001
From: gentov <33622808+gentov@users.noreply.github.com>
Date: Thu, 22 Oct 2020 20:55:38 -0400
Subject: [PATCH 15/19] line following on the other side

moved line follower onto the other side for tighter navigation
---
 LineFollower.cpp  | 16 ++++++++--------
 LineFollower.h    |  6 ++++--
 StudentsRobot.cpp | 36 ++++++++++++++++++------------------
 3 files changed, 30 insertions(+), 28 deletions(-)

diff --git a/LineFollower.cpp b/LineFollower.cpp
index 94d1ef8..4a31b83 100644
--- a/LineFollower.cpp
+++ b/LineFollower.cpp
@@ -36,15 +36,15 @@ void LineFollower::lineFollowBackwards(){
 
 
 	  else if(leftSensorValue >= ON_BLACK || rightSensorValue >= ON_BLACK){
-			rightCorrection = (ON_BLACK - rightSensorValue)*lineFollowingKp;
-			leftCorrection =  (leftSensorValue - ON_BLACK)*lineFollowingKp;
+			rightCorrection = (ON_BLACK - rightSensorValue)*lineFollowingKpBackwards;
+			leftCorrection =  (leftSensorValue - ON_BLACK)*lineFollowingKpBackwards;
 			canCountLine = true;
 	  }
 	  else{
 		  canCountLine = true;
 	  }
-	  robotChassis->myleft -> setVelocityDegreesPerSecond(lineFollowingSpeed_mm_per_sec*MM_TO_WHEEL_DEGREES + leftCorrection);
-      robotChassis->myright -> setVelocityDegreesPerSecond(-lineFollowingSpeed_mm_per_sec*MM_TO_WHEEL_DEGREES + rightCorrection);
+	  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(){
@@ -69,15 +69,15 @@ void LineFollower::lineFollowForwards(){
 
 
 	  else if(leftSensorValue >= ON_BLACK || rightSensorValue >= ON_BLACK){
-			rightCorrection = (ON_BLACK - rightSensorValue)*lineFollowingKp;
-			leftCorrection =  (leftSensorValue - ON_BLACK)*lineFollowingKp;
+			rightCorrection = (ON_BLACK - rightSensorValue)*lineFollowingKpForwards;
+			leftCorrection =  (leftSensorValue - ON_BLACK)*lineFollowingKpForwards;
 			canCountLine = true;
 	  }
 	  else{
 		  canCountLine = true;
 	  }
-	  robotChassis->myleft -> setVelocityDegreesPerSecond(-lineFollowingSpeed_mm_per_sec*MM_TO_WHEEL_DEGREES + leftCorrection);
-      robotChassis->myright -> setVelocityDegreesPerSecond(lineFollowingSpeed_mm_per_sec*MM_TO_WHEEL_DEGREES + rightCorrection);
+	  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(){
diff --git a/LineFollower.h b/LineFollower.h
index 9e2e31e..7241c66 100644
--- a/LineFollower.h
+++ b/LineFollower.h
@@ -16,8 +16,10 @@ class LineFollower{
 	   LineFollower(DrivingChassis* myChassis);
 	   // on black line
 	   const int ON_BLACK = 3692;//3750;
-	   int lineFollowingSpeed_mm_per_sec = 175;
-	   float lineFollowingKp = .002*lineFollowingSpeed_mm_per_sec;
+	   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;
diff --git a/StudentsRobot.cpp b/StudentsRobot.cpp
index 8bf41c3..375b196 100644
--- a/StudentsRobot.cpp
+++ b/StudentsRobot.cpp
@@ -186,26 +186,26 @@ void StudentsRobot::updateStateMachine() {
 //			status = Running;
 //		}
 /// LINE FOLLOWING
-	    if((millis() - startTime) < 3000){
-			lineSensor.lineFollowForwards();
-		}
-		else{
-		   robotChassis.stop();
-		   lineSensor.resetLineCount();
-		   status = Running;
-		}
+//	    if((millis() - startTime) < 7000){
+//			lineSensor.lineFollowForwards();
+//		}
+//		else{
+//		   robotChassis.stop();
+//		   lineSensor.resetLineCount();
+//		   status = Running;
+//		}
 
 // Navigation
-//   static bool goingToWayPoint1 = true;
-//
-//   if(goingToWayPoint1){
-//        if(navigate(2, -2, &robotChassis, &lineSensor)){
-//        	Serial.println("reached waypoint 1");
-//        	goingToWayPoint1 = false;
-//        }
-//   }
-//   else
-//	   navigate(3, 0, &robotChassis, &lineSensor);
+   static bool goingToWayPoint1 = true;
+
+   if(goingToWayPoint1){
+        if(navigate(2, -2, &robotChassis, &lineSensor)){
+        	Serial.println("reached waypoint 1");
+        	goingToWayPoint1 = false;
+        }
+   }
+   else
+	   navigate(0, 0, &robotChassis, &lineSensor);
 /// POSE TRACKING
 
 //	   static int testCase = 1;

From 681d2aee9944cad56264c0cbb5a4926c13228340 Mon Sep 17 00:00:00 2001
From: gentov <33622808+gentov@users.noreply.github.com>
Date: Thu, 22 Oct 2020 22:04:46 -0400
Subject: [PATCH 16/19] added logic for counting rows in both directions

---
 DrivingChassis.h |  2 +-
 LineFollower.cpp | 13 +++++++++----
 Pose.h           |  6 +++---
 3 files changed, 13 insertions(+), 8 deletions(-)

diff --git a/DrivingChassis.h b/DrivingChassis.h
index 6ecb1ed..4f4c413 100644
--- a/DrivingChassis.h
+++ b/DrivingChassis.h
@@ -81,7 +81,7 @@ class DrivingChassis {
 	bool performingMovement = false;
 	unsigned long startTimeOfMovement_ms;
 	float wheelMovementKp = 4.1;// was 3.9
-	float turningMovementKp = 13; //was 9, 11.7
+	float turningMovementKp = 11; //was 9, 11.7
 	float wheelMovementDeadband_mm = 2.5;
 	float wheelMovementDeadband_deg = .5;
 	Pose myChassisPose;
diff --git a/LineFollower.cpp b/LineFollower.cpp
index 4a31b83..c91c988 100644
--- a/LineFollower.cpp
+++ b/LineFollower.cpp
@@ -57,12 +57,17 @@ void LineFollower::lineFollowForwards(){
 	   if(canCountLine){
 	      lineCount++;
 	      // Mathematically speaking, this should only increment one of the following. Either
-	      // row or column
+	      // 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.currentRow += round(cos(headingInRadians));
-	      robotChassis->myChassisPose.currentColumn += round(sin(headingInRadians));
-	      canCountLine = false;
+	      robotChassis->myChassisPose.rowCount += 1;
+	      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));
 	  }
diff --git a/Pose.h b/Pose.h
index 1e38a86..abc0925 100644
--- a/Pose.h
+++ b/Pose.h
@@ -13,10 +13,10 @@ class 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. I have
-	   // opted to use solid intervals of 90 to make the math cleaner for determining row
-	   // and column.
+	   // current heading will be updated by the IMU during turnToHeading.
 	   float heading = 0;
+	   int rowCount = 0;
+	   int colCount = 0;
 
     private:
 };

From df25c6aeb43139c3009b86f9018fb24b906ab64a Mon Sep 17 00:00:00 2001
From: gentov <33622808+gentov@users.noreply.github.com>
Date: Thu, 22 Oct 2020 23:40:13 -0400
Subject: [PATCH 17/19] fixed some bugs in navigation. Not perfect yet

Will split the navigation routine into two parts
---
 DrivingChassis.h      |  4 ++--
 LineFollower.cpp      |  2 +-
 NavigationRoutine.cpp | 11 ++++++++---
 StudentsRobot.cpp     |  7 +++++--
 4 files changed, 16 insertions(+), 8 deletions(-)

diff --git a/DrivingChassis.h b/DrivingChassis.h
index 4f4c413..16fa700 100644
--- a/DrivingChassis.h
+++ b/DrivingChassis.h
@@ -16,7 +16,7 @@
 #define MM_TO_WHEEL_DEGREES 2.1174F
 #define WHEEL_DEGREES_TO_MM .472277F
 #define MAX_SPEED_MM_PER_SEC 180
-#define MAX_MOTOR_EFFORT_DURING_TURN 300 //275 // 500
+#define MAX_MOTOR_EFFORT_DURING_TURN 260 //300 //275 // 500
 
 
 /**
@@ -81,7 +81,7 @@ class DrivingChassis {
 	bool performingMovement = false;
 	unsigned long startTimeOfMovement_ms;
 	float wheelMovementKp = 4.1;// was 3.9
-	float turningMovementKp = 11; //was 9, 11.7
+	float turningMovementKp = 21; //was 9, 11.7, 17.5
 	float wheelMovementDeadband_mm = 2.5;
 	float wheelMovementDeadband_deg = .5;
 	Pose myChassisPose;
diff --git a/LineFollower.cpp b/LineFollower.cpp
index c91c988..3de635e 100644
--- a/LineFollower.cpp
+++ b/LineFollower.cpp
@@ -60,7 +60,7 @@ void LineFollower::lineFollowForwards(){
 	      // 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 += 1;
+	      robotChassis->myChassisPose.rowCount += abs(round(cos(headingInRadians)));
 	      robotChassis->myChassisPose.colCount += round(sin(headingInRadians));
 	      if(robotChassis->myChassisPose.rowCount == 2){
 	          robotChassis->myChassisPose.currentRow += round(cos(headingInRadians));
diff --git a/NavigationRoutine.cpp b/NavigationRoutine.cpp
index a4e8b97..b19fd49 100644
--- a/NavigationRoutine.cpp
+++ b/NavigationRoutine.cpp
@@ -63,7 +63,10 @@ bool navigate(int row, int col, DrivingChassis* drivingChassis, LineFollower* li
 			}
 			else{
 				drivingChassis->stop();
-				if(col != 0){
+				if(drivingChassis->myChassisPose.currentColumn == col){
+					navState = FINISHED;
+				}
+				else if (col != 0){
 					navState = TURN_TOWARDS_CORRECT_COLUMN;
 				}
 			}
@@ -85,12 +88,14 @@ bool navigate(int row, int col, DrivingChassis* drivingChassis, LineFollower* li
 		case FINDING_COLUMN:
 			Serial.println("FINDING COL: " + String(col) +  "CURRENT COL: " + String(drivingChassis->myChassisPose.currentColumn));
 			if(drivingChassis->myChassisPose.currentColumn != col){
-				// if the row is wrong.
+				// if the column is wrong.
 				lineSensor->lineFollowForwards();
 			}
 			else{
 			   drivingChassis->stop();
-               navState = FINISHED;
+			   if(row == drivingChassis->myChassisPose.currentRow){
+			   					navState = FINISHED;
+			   }
 			}
 			break;
 		case FINISHED:
diff --git a/StudentsRobot.cpp b/StudentsRobot.cpp
index 375b196..2e96436 100644
--- a/StudentsRobot.cpp
+++ b/StudentsRobot.cpp
@@ -199,13 +199,16 @@ void StudentsRobot::updateStateMachine() {
    static bool goingToWayPoint1 = true;
 
    if(goingToWayPoint1){
-        if(navigate(2, -2, &robotChassis, &lineSensor)){
+        if(navigate(2, -1, &robotChassis, &lineSensor)){
         	Serial.println("reached waypoint 1");
         	goingToWayPoint1 = false;
         }
    }
    else
-	   navigate(0, 0, &robotChassis, &lineSensor);
+	   if(navigate(2, -2, &robotChassis, &lineSensor)){
+		   status = Running;
+		   goingToWayPoint1 = true;
+	   }
 /// POSE TRACKING
 
 //	   static int testCase = 1;

From 68df328dbd2041a15c7cad5d5928cb6ede5e115e Mon Sep 17 00:00:00 2001
From: gentov <33622808+gentov@users.noreply.github.com>
Date: Mon, 26 Oct 2020 19:17:07 -0400
Subject: [PATCH 18/19] split nav and motion into seperate functions

re-wrote motion for forward/backwards to drive straight based on IMU.
---
 DrivingChassis.cpp    | 295 +++++++++++++++++++-----------------------
 DrivingChassis.h      |  27 +++-
 NavigationRoutine.cpp |  63 +++++----
 NavigationRoutine.h   |  14 +-
 StudentsRobot.cpp     | 165 +++++++++--------------
 5 files changed, 264 insertions(+), 300 deletions(-)

diff --git a/DrivingChassis.cpp b/DrivingChassis.cpp
index 2975a0f..0e39b6e 100644
--- a/DrivingChassis.cpp
+++ b/DrivingChassis.cpp
@@ -84,64 +84,15 @@ void DrivingChassis::driveForwardFromInterpolation(float mmDistanceFromCurrent,
     myright -> startInterpolationDegrees(-mmDistanceFromCurrent * MM_TO_WHEEL_DEGREES, msDuration, LIN);
 }
 
-DrivingStatus DrivingChassis::driveForward(float mmDistanceFromCurrent, int msDuration){
+void DrivingChassis::driveForward(float mmDistanceFromCurrent, int msDuration){
 	// if we're not performing an action
 	// start a timer, reset encoders
-    if(!performingMovement){
-    	startTimeOfMovement_ms = millis();
-    	performingMovement = true;
-    	myleft -> overrideCurrentPosition(0);
-    	myright -> overrideCurrentPosition(0);
-    	// TODO: call driveStraight method which uses IMU (if we're using an IMU)
-    }
-	// check for timeout
-	if((millis() - startTimeOfMovement_ms) > msDuration){
-		//timeout occured. Stop the robot
-		Serial.println("Detected Timeout\r\n");
-		stop();
-		performingMovement = false;
-		return TIMED_OUT;
-	}
-
-    if(mmDistanceFromCurrent != -1){
-    	float currentDistanceMovedRightWheel_mm = (myright -> getAngleDegrees())*WHEEL_DEGREES_TO_MM;
-	    float currentDistanceMovedLeftWheel_mm = (myleft -> getAngleDegrees())*WHEEL_DEGREES_TO_MM;
-	    float rightWheelError_mm = currentDistanceMovedRightWheel_mm - mmDistanceFromCurrent;
-	    float leftWheelError_mm = - currentDistanceMovedLeftWheel_mm - mmDistanceFromCurrent;
-
-	    if((fabs(rightWheelError_mm) < wheelMovementDeadband_mm) && (fabs(leftWheelError_mm) < wheelMovementDeadband_mm)){
-		    Serial.println("Reached Setpoint \r\n");
-		    stop();
-		    performingMovement = false;
-		    return REACHED_SETPOINT;
-	    }
-	    else{
-	    	float rightMotorEffort_deg_per_sec = wheelMovementKp*rightWheelError_mm;
-	    	float leftMotorEffort_deg_per_sec = wheelMovementKp*leftWheelError_mm;
-
-	    	//clamps speed to 20 cm per second
-	    	if(fabs(rightMotorEffort_deg_per_sec) > (MAX_SPEED_MM_PER_SEC * MM_TO_WHEEL_DEGREES)){
-	    		rightMotorEffort_deg_per_sec = -MAX_SPEED_MM_PER_SEC*MM_TO_WHEEL_DEGREES;
-	    	}
-	    	if(fabs(leftMotorEffort_deg_per_sec) > (MAX_SPEED_MM_PER_SEC * MM_TO_WHEEL_DEGREES)){
-	    		leftMotorEffort_deg_per_sec = -MAX_SPEED_MM_PER_SEC*MM_TO_WHEEL_DEGREES;
-	        }
-	    	myright -> setVelocityDegreesPerSecond(-rightMotorEffort_deg_per_sec);
-	    	myleft -> setVelocityDegreesPerSecond(leftMotorEffort_deg_per_sec);
-
-//	    	Serial.println("Left Error: " + String(leftWheelError_mm) + "\r\n" );
-//	    	Serial.println("Right Error: " + String(rightWheelError_mm) + "\r\n" );
-//	    	Serial.println("Left Effort: " + String(leftMotorEffort_deg_per_sec) + "\r\n" );
-//	    	Serial.println("Right Effort: " + String(rightMotorEffort_deg_per_sec) + "\r\n" );
-	    }
-    }
-
-    else{
-    	// sets speed to 10 cm per second
-    	myright -> setVelocityDegreesPerSecond(100*MM_TO_WHEEL_DEGREES);
-        myleft -> setVelocityDegreesPerSecond(-100*MM_TO_WHEEL_DEGREES);
-    }
-    return GOING_TO_SETPOINT;
+    startTimeOfMovement_ms = millis();
+	myleft -> overrideCurrentPosition(0);
+	myright -> overrideCurrentPosition(0);
+	motionSetpoint = mmDistanceFromCurrent;
+	timeout_ms = msDuration;
+	motionType = DRIVING_FORWARDS;
 }
 
 /**
@@ -162,64 +113,15 @@ void DrivingChassis::driveBackwardsFromInterpolation(float mmDistanceFromCurrent
     myright -> startInterpolationDegrees(mmDistanceFromCurrent * MM_TO_WHEEL_DEGREES, msDuration, LIN);
 }
 
-DrivingStatus DrivingChassis::driveBackwards(float mmDistanceFromCurrent, int msDuration){
+void DrivingChassis::driveBackwards(float mmDistanceFromCurrent, int msDuration){
 		// if we're not performing an action
 		// start a timer, reset encoders
-	    if(!performingMovement){
-	    	startTimeOfMovement_ms = millis();
-	    	performingMovement = true;
-	    	myleft -> overrideCurrentPosition(0);
-	    	myright -> overrideCurrentPosition(0);
-	    	// TODO: call driveStraight method which uses IMU (if we're using an IMU)
-	    }
-		// check for timeout
-		if((millis() - startTimeOfMovement_ms) > msDuration){
-			//timeout occured. Stop the robot
-			Serial.println("Detected Timeout\r\n");
-			stop();
-			performingMovement = false;
-			return TIMED_OUT;
-		}
-
-	    if(mmDistanceFromCurrent != -1){
-	    	float currentDistanceMovedRightWheel_mm = (myright -> getAngleDegrees())*WHEEL_DEGREES_TO_MM;
-		    float currentDistanceMovedLeftWheel_mm = (myleft -> getAngleDegrees())*WHEEL_DEGREES_TO_MM;
-		    float rightWheelError_mm = - currentDistanceMovedRightWheel_mm - mmDistanceFromCurrent;
-		    float leftWheelError_mm = currentDistanceMovedLeftWheel_mm - mmDistanceFromCurrent;
-
-		    if((fabs(rightWheelError_mm) < wheelMovementDeadband_mm) && (fabs(leftWheelError_mm) < wheelMovementDeadband_mm)){
-			    Serial.println("Reached Setpoint \r\n");
-			    stop();
-			    performingMovement = false;
-			    return REACHED_SETPOINT;
-		    }
-		    else{
-		    	float rightMotorEffort_deg_per_sec = wheelMovementKp*rightWheelError_mm;
-		    	float leftMotorEffort_deg_per_sec = wheelMovementKp*leftWheelError_mm;
-
-		    	//clamps speed to 20 cm per second
-		    	if(fabs(rightMotorEffort_deg_per_sec) > (MAX_SPEED_MM_PER_SEC * MM_TO_WHEEL_DEGREES)){
-		    		rightMotorEffort_deg_per_sec = -MAX_SPEED_MM_PER_SEC*MM_TO_WHEEL_DEGREES;
-		    	}
-		    	if(fabs(leftMotorEffort_deg_per_sec) > (MAX_SPEED_MM_PER_SEC * MM_TO_WHEEL_DEGREES)){
-		    		leftMotorEffort_deg_per_sec = -MAX_SPEED_MM_PER_SEC*MM_TO_WHEEL_DEGREES;
-		        }
-		    	myright -> setVelocityDegreesPerSecond(rightMotorEffort_deg_per_sec);
-		    	myleft -> setVelocityDegreesPerSecond(-leftMotorEffort_deg_per_sec);
-
-//		    	Serial.println("Left Error: " + String(leftWheelError_mm) + "\r\n" );
-//		    	Serial.println("Right Error: " + String(rightWheelError_mm) + "\r\n" );
-//		    	Serial.println("Left Effort: " + String(leftMotorEffort_deg_per_sec) + "\r\n" );
-//		    	Serial.println("Right Effort: " + String(rightMotorEffort_deg_per_sec) + "\r\n" );
-		    }
-	    }
-
-	    else{
-	    	// sets speed to 10 cm per second
-	    	myright -> setVelocityDegreesPerSecond(-100*MM_TO_WHEEL_DEGREES);
-	        myleft -> setVelocityDegreesPerSecond(100*MM_TO_WHEEL_DEGREES);
-	    }
-	    return GOING_TO_SETPOINT;
+	    startTimeOfMovement_ms = millis();
+		myleft -> overrideCurrentPosition(0);
+		myright -> overrideCurrentPosition(0);
+		motionSetpoint = mmDistanceFromCurrent;
+		timeout_ms = msDuration;
+		motionType = DRIVING_BACKWARDS;
 }
 
 /**
@@ -244,53 +146,11 @@ void DrivingChassis::turnDegreesFromInterpolation(float degreesToRotateBase, int
 	   myright -> startInterpolationDegrees(degreesToRotateBase * WHEEL_DEGREES_TO_BODY_DEGREES, msDuration, LIN);
 }
 
-DrivingStatus DrivingChassis::turnToHeading(float degreesToRotateBase, int msDuration){
-	/* As of 10/4/2020 Gabe doesn't have an IMU... rippu
-	  Two variants, one with IMU and one without IMU.
-	  IMU variant: will use P controller to modulate speed to make the turn based on
-	  heading. Maybe we can even make this absolute in the future.
-	  Encoder variant: Make the turn based on encoder ticks
-	  */
-	 if(!performingMovement){
-		    	startTimeOfMovement_ms = millis();
-		    	performingMovement = true;
-     }
-	 // check for timeout
-     if((millis() - startTimeOfMovement_ms) > msDuration){
-			//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 - degreesToRotateBase;
-    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");
-		performingMovement = false;
-		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;
-		    	}
-		    }
-		    //Serial.println("Motor Effort: " + String(motorEffort) +"\r\n");
-//		    Serial.println("Heading Error: " + String(headingError) +"\r\n");
-            myleft->setVelocityDegreesPerSecond(-motorEffort);
-            myright->setVelocityDegreesPerSecond(-motorEffort);
-	}
-	return GOING_TO_SETPOINT;
+void DrivingChassis::turnToHeading(float degreesToRotateBase, int msDuration){
+	motionSetpoint = degreesToRotateBase;
+	timeout_ms = msDuration;
+	startTimeOfMovement_ms = millis();
+	motionType = TURNING;
 }
 
 /**
@@ -300,14 +160,127 @@ DrivingStatus DrivingChassis::turnToHeading(float degreesToRotateBase, int msDur
  *
  * @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 10 cm per second
+	    	    	myright -> setVelocityDegreesPerSecond(MAX_SPEED_MM_PER_SEC*MM_TO_WHEEL_DEGREES);
+	    	        myleft -> setVelocityDegreesPerSecond(-MAX_SPEED_MM_PER_SEC*MM_TO_WHEEL_DEGREES);
+	    	    }
+	        }
+	        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
+	    	    	myright -> setVelocityDegreesPerSecond(-MAX_SPEED_MM_PER_SEC*MM_TO_WHEEL_DEGREES);
+	    	        myleft -> setVelocityDegreesPerSecond(MAX_SPEED_MM_PER_SEC*MM_TO_WHEEL_DEGREES);
+	    	    }
+	        }
+	        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 16fa700..2d3a935 100644
--- a/DrivingChassis.h
+++ b/DrivingChassis.h
@@ -15,7 +15,7 @@
 #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 180
+#define MAX_SPEED_MM_PER_SEC 150
 #define MAX_MOTOR_EFFORT_DURING_TURN 260 //300 //275 // 500
 
 
@@ -29,6 +29,16 @@ enum DrivingStatus {
 	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
  *
@@ -79,11 +89,14 @@ class DrivingChassis {
 	PIDMotor * myleft;
 	PIDMotor * myright;
 	bool performingMovement = false;
+	MotionType motionType;
 	unsigned long startTimeOfMovement_ms;
-	float wheelMovementKp = 4.1;// was 3.9
+	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();
@@ -120,7 +133,7 @@ class DrivingChassis {
 	 * @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.
 	 */
-	DrivingStatus driveBackwards(float mmDistanceFromCurrent, int msDuration);
+	void driveBackwards(float mmDistanceFromCurrent, int msDuration);
 
 	/**
 	 * Start a drive forward action using the encoders and setpoint interpolation
@@ -143,7 +156,7 @@ class DrivingChassis {
 	 * 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)
 	 */
-	DrivingStatus driveForward(float mmDistanceFromCurrent, int msDuration);
+	void driveForward(float mmDistanceFromCurrent, int msDuration);
 
 	/**
 	 * Start a turn action using the encoders and setpoint interpolation
@@ -176,7 +189,7 @@ class DrivingChassis {
 	 * @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.
 	 */
-	DrivingStatus turnToHeading(float desiredHeading, int msDuration);
+	void turnToHeading(float desiredHeading, int msDuration);
 
 	/**
 	 * Check to see if the chassis is performing an action
@@ -185,13 +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/NavigationRoutine.cpp b/NavigationRoutine.cpp
index b19fd49..b43cd67 100644
--- a/NavigationRoutine.cpp
+++ b/NavigationRoutine.cpp
@@ -6,7 +6,14 @@
  */
 #include "NavigationRoutine.h"
 
-bool navigate(int row, int col, DrivingChassis* drivingChassis, LineFollower* lineSensor){
+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");
@@ -21,26 +28,24 @@ bool navigate(int row, int col, DrivingChassis* drivingChassis, LineFollower* li
 		case TURN_TOWARDS_CORRECT_COLUMN:
 			Serial.println("TURNING TOWARDS COLUMN");
 			// determine what is our first state
-			if(drivingChassis->myChassisPose.currentRow != row){
+			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
-				if(drivingChassis->turnToHeading(90, 2500) == REACHED_SETPOINT){
-					 navState = FINDING_OUTER_EDGE;
-				}
+				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 > col){
-					if(drivingChassis->turnToHeading(-90, 2500) == REACHED_SETPOINT){
-						navState = FINDING_COLUMN;
-					}
+				if(drivingChassis->myChassisPose.currentColumn > goalCol){
+					drivingChassis->turnToHeading(-90, 5000);
 				}
 				else{
-					if(drivingChassis->turnToHeading(90, 2500) == REACHED_SETPOINT){
-						navState = FINDING_COLUMN;
-					}
+					drivingChassis->turnToHeading(90, 5000);
 				}
+				navStateAfterMotionSetpointReached = FINDING_COLUMN;
+				navState = WAIT_FOR_MOTION_SETPOINT_REACHED;
 			}
 			break;
 		case FINDING_OUTER_EDGE:
@@ -56,17 +61,17 @@ bool navigate(int row, int col, DrivingChassis* drivingChassis, LineFollower* li
 			}
 			break;
 		case FINDING_ROW:
-			Serial.println("FINDING ROW: " + String(row) +  "CURRENT ROW: " + String(drivingChassis->myChassisPose.currentRow));
-			if(drivingChassis->myChassisPose.currentRow != 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 == col){
+				if(drivingChassis->myChassisPose.currentColumn == goalCol){
 					navState = FINISHED;
 				}
-				else if (col != 0){
+				else if (goalCol != 0){
 					navState = TURN_TOWARDS_CORRECT_COLUMN;
 				}
 			}
@@ -74,35 +79,37 @@ bool navigate(int row, int col, DrivingChassis* drivingChassis, LineFollower* li
 		case TURN_TOWARDS_CORRECT_ROW:
 			Serial.println("TURNING TOWARDS ROW");
 			// otherwise, we just need to find the right column
-			if(drivingChassis->myChassisPose.currentRow > row){
-				if(drivingChassis->turnToHeading(180, 2500) == REACHED_SETPOINT){
-					navState = FINDING_ROW;
-				}
+			if(drivingChassis->myChassisPose.currentRow > goalRow){
+				drivingChassis->turnToHeading(180, 5000);
 			}
 			else{
-				if(drivingChassis->turnToHeading(0, 2500) == REACHED_SETPOINT){
-					navState = FINDING_ROW;
-				}
+				drivingChassis->turnToHeading(0, 5000);
 			}
+			navStateAfterMotionSetpointReached = FINDING_ROW;
+			navState = WAIT_FOR_MOTION_SETPOINT_REACHED;
 			break;
 		case FINDING_COLUMN:
-			Serial.println("FINDING COL: " + String(col) +  "CURRENT COL: " + String(drivingChassis->myChassisPose.currentColumn));
-			if(drivingChassis->myChassisPose.currentColumn != col){
+			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(row == drivingChassis->myChassisPose.currentRow){
+			   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 true;
 		}
-	return false;
+	return navState;
 }
 
 
diff --git a/NavigationRoutine.h b/NavigationRoutine.h
index eb5037b..3f074c4 100644
--- a/NavigationRoutine.h
+++ b/NavigationRoutine.h
@@ -18,11 +18,21 @@ enum NavigationStates{
 	FINDING_ROW = 3,
 	TURN_TOWARDS_CORRECT_ROW = 4,
 	FINDING_COLUMN = 5,
-	FINISHED = 6,
+	WAIT_FOR_MOTION_SETPOINT_REACHED = 6,
+	FINISHED = 7,
 };
 
-bool navigate(int row, int col, DrivingChassis* drivingChassis, LineFollower* lineSensor);
+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/StudentsRobot.cpp b/StudentsRobot.cpp
index 2e96436..9c389aa 100644
--- a/StudentsRobot.cpp
+++ b/StudentsRobot.cpp
@@ -177,14 +177,6 @@ void StudentsRobot::updateStateMachine() {
 		// in safe mode
 		break;
 	case Testing:
-// CLAMPED CONTROL
-//		DrivingStatus statusOfForward = robotChassis.turnToHeading(90, 2000);
-//		if(statusOfForward == REACHED_SETPOINT){
-//			status = Running;
-//		}
-//		else if(statusOfForward == TIMED_OUT){
-//			status = Running;
-//		}
 /// LINE FOLLOWING
 //	    if((millis() - startTime) < 7000){
 //			lineSensor.lineFollowForwards();
@@ -196,104 +188,71 @@ void StudentsRobot::updateStateMachine() {
 //		}
 
 // Navigation
-   static bool goingToWayPoint1 = true;
-
-   if(goingToWayPoint1){
-        if(navigate(2, -1, &robotChassis, &lineSensor)){
-        	Serial.println("reached waypoint 1");
-        	goingToWayPoint1 = false;
-        }
-   }
-   else
-	   if(navigate(2, -2, &robotChassis, &lineSensor)){
-		   status = Running;
-		   goingToWayPoint1 = true;
-	   }
-/// POSE TRACKING
-
-//	   static int testCase = 1;
-//       switch(testCase){
-//          case 1:
-//        	  if(robotChassis.myChassisPose.currentRow != 1){
-//        	      lineSensor.lineFollow();
-//        	  }
-//        	  else{
-//			      robotChassis.stop();
-//			      testCase = 2;
-//			  }
-//        	  break;
-//          case 2:
-//        	  if(robotChassis.driveBackwards(200, 1500) == REACHED_SETPOINT){
-//        		  testCase = 3;
-//        		  lineSensor.canCountLine = true;
-//        	  }
-//        	  break;
-//          case 3:
-//        	  // turn right 90
-//        	  if(robotChassis.turnToHeading(-90, 1500) == REACHED_SETPOINT){
-//        		  testCase = 4;
-//        	  }
-//        	  break;
-//          case 4:
-//        	  // line follow until column
-//        	  if(robotChassis.myChassisPose.currentColumn != -1){
-//        	      lineSensor.lineFollow();
-//        	  }
-//        	  else{
-//        	      robotChassis.stop();
-//        	      testCase = 5;
-//        	  }
-//        	  break;
-//          case 5:
-//        	  if(robotChassis.turnToHeading(90, 1500) == REACHED_SETPOINT){
-//        	      Serial.println("Pose Row:" + String(robotChassis.myChassisPose.currentRow) + "\r\n");
-//        	      Serial.println("Pose Column:" + String(robotChassis.myChassisPose.currentColumn) + "\r\n");
-//        	      Serial.println("Pose Heading:" + String(robotChassis.myChassisPose.heading) + "\r\n");
-//        	      status = Running;
-//        	      testCase = 1;
-//        	  }
-//        	  break;
-//       }
+		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 bool movedForward = false;
-//		static bool movedBack    = false;
-//	    static bool turnedRight  = false;
-//		static bool turnedLeft   = false;
-//		static bool backToZero   = false;
 //
-//		if(!movedForward){
-//			if(robotChassis.driveForward(300, 5000) == REACHED_SETPOINT){
-//				movedForward = true;
-//			}
-//		}
-//		else if(movedForward && !movedBack){
-//				if(robotChassis.driveBackwards(300, 5000) == REACHED_SETPOINT){
-//					movedBack = true;
-//				}
-//	    }
-//		else if(movedBack && !turnedRight){
-//			if(robotChassis.turnToHeading(-90, 5000) == REACHED_SETPOINT){
-//				turnedRight = true;
+//	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;
 //			}
-//		}
-//		else if(turnedRight && !turnedLeft){
-//			if(robotChassis.turnToHeading(90, 5000) == REACHED_SETPOINT){
-//				turnedLeft = true;
-//			}
-//		}
-//		else if(turnedLeft && !backToZero){
-//			if(robotChassis.turnToHeading(0, 5000) == REACHED_SETPOINT){
-//				backToZero = true;
-//				status = Running;
-//				movedBack = false;
-//				movedForward = false;
-//				turnedLeft = false;
-//				turnedRight = false;
-//				backToZero  = false;
-//			}
-//		}
-		break;
+//			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);

From 8978a044a11702165caa95dcdaa40dad03f03fd8 Mon Sep 17 00:00:00 2001
From: gentov <33622808+gentov@users.noreply.github.com>
Date: Tue, 27 Oct 2020 13:38:04 -0400
Subject: [PATCH 19/19] makes sure drive straight applied to non-specified
 distances

---
 DrivingChassis.cpp | 8 +++-----
 1 file changed, 3 insertions(+), 5 deletions(-)

diff --git a/DrivingChassis.cpp b/DrivingChassis.cpp
index 0e39b6e..aa8a339 100644
--- a/DrivingChassis.cpp
+++ b/DrivingChassis.cpp
@@ -220,9 +220,8 @@ DrivingStatus DrivingChassis::statusOfChassisDriving() {
 	    	    }
 
 	    	    else{
-	    	    	// sets speed to 10 cm per second
-	    	    	myright -> setVelocityDegreesPerSecond(MAX_SPEED_MM_PER_SEC*MM_TO_WHEEL_DEGREES);
-	    	        myleft -> setVelocityDegreesPerSecond(-MAX_SPEED_MM_PER_SEC*MM_TO_WHEEL_DEGREES);
+	    	    	// sets speed to 20 cm per second
+	    	    	driveStraight(myChassisPose.heading, DRIVING_FORWARDS);
 	    	    }
 	        }
 	        break;
@@ -249,8 +248,7 @@ DrivingStatus DrivingChassis::statusOfChassisDriving() {
 
 	    	    else{
 	    	    	// sets speed to 20 cm per second
-	    	    	myright -> setVelocityDegreesPerSecond(-MAX_SPEED_MM_PER_SEC*MM_TO_WHEEL_DEGREES);
-	    	        myleft -> setVelocityDegreesPerSecond(MAX_SPEED_MM_PER_SEC*MM_TO_WHEEL_DEGREES);
+	    	    	driveStraight(myChassisPose.heading, DRIVING_BACKWARDS);
 	    	    }
 	        }
 	        break;