Basic movement firmware Changelog

DrivingChassis.cpp

@@ -7,6 +7,7 @@
 
 #include "DrivingChassis.h"
 
+
 /**
  * Compute a delta in wheel angle to traverse a specific distance
  *
@@ -56,7 +57,11 @@ DrivingChassis::~DrivingChassis() {
  */
 DrivingChassis::DrivingChassis(PIDMotor * left, PIDMotor * right,
 		float wheelTrackMM, float wheelRadiusMM,GetIMU * imu) {
-
+	myleft = left;
+	myright = right;
+	mywheelTrackMM = wheelTrackMM;
+	mywheelRadiusMM = wheelRadiusMM;
+	IMU = imu;
 }
 
 /**
@@ -69,7 +74,54 @@ DrivingChassis::DrivingChassis(PIDMotor * left, PIDMotor * right,
  * @note pidmotorInstance->overrideCurrentPosition(0); can be used to "zero out" the motor to
  * 		 allow for relative moves. Otherwise the motor is always in ABSOLUTE mode
  */
-void DrivingChassis::driveForward(float mmDistanceFromCurrent, int msDuration) {
+void DrivingChassis::driveForwardFromInterpolation(float mmDistanceFromCurrent, int msDuration) {
+	// We should also have a "drive straight" method that uses the IMU to actually drive straight
+	// Although we'll have a linefollow state so... idk maybe not
+
+	myleft -> overrideCurrentPosition(0);
+	myright -> overrideCurrentPosition(0);
+	myleft -> startInterpolationDegrees(mmDistanceFromCurrent * MM_TO_WHEEL_DEGREES, msDuration, LIN);
+    myright -> startInterpolationDegrees(-mmDistanceFromCurrent * MM_TO_WHEEL_DEGREES, msDuration, LIN);
+}
+
+void DrivingChassis::driveForward(float mmDistanceFromCurrent, int msDuration){
+	// if we're not performing an action
+	// start a timer, reset encoders
+    startTimeOfMovement_ms = millis();
+	myleft -> overrideCurrentPosition(0);
+	myright -> overrideCurrentPosition(0);
+	motionSetpoint = mmDistanceFromCurrent;
+	timeout_ms = msDuration;
+	motionType = DRIVING_FORWARDS;
+}
+
+/**
+	 * Start a drive backwards action
+	 *
+	 * @param mmDistanceFromCurrent is the distance the mobile base should drive backwards
+	 * @param msDuration is the time in miliseconds that the drive action should take
+	 *
+	 * @note this function is fast-return and should not block
+*/
+void DrivingChassis::driveBackwardsFromInterpolation(float mmDistanceFromCurrent, int msDuration) {
+	// We should also have a "drive straight" method that uses the IMU to actually drive straight
+	// Although we'll have a linefollow state so... idk maybe not
+	myleft -> overrideCurrentPosition(0);
+	myright -> overrideCurrentPosition(0);
+
+	myleft -> startInterpolationDegrees(-mmDistanceFromCurrent * MM_TO_WHEEL_DEGREES, msDuration, LIN);
+    myright -> startInterpolationDegrees(mmDistanceFromCurrent * MM_TO_WHEEL_DEGREES, msDuration, LIN);
+}
+
+void DrivingChassis::driveBackwards(float mmDistanceFromCurrent, int msDuration){
+		// if we're not performing an action
+		// start a timer, reset encoders
+	    startTimeOfMovement_ms = millis();
+		myleft -> overrideCurrentPosition(0);
+		myright -> overrideCurrentPosition(0);
+		motionSetpoint = mmDistanceFromCurrent;
+		timeout_ms = msDuration;
+		motionType = DRIVING_BACKWARDS;
 }
 
 /**
@@ -87,8 +139,18 @@ void DrivingChassis::driveForward(float mmDistanceFromCurrent, int msDuration) {
  *  @note pidmotorInstance->overrideCurrentPosition(0); can be used to "zero out" the motor to
  * 		  allow for relative moves. Otherwise the motor is always in ABSOLUTE mode
  */
-void DrivingChassis::turnDegrees(float degreesToRotateBase, int msDuration) {
+void DrivingChassis::turnDegreesFromInterpolation(float degreesToRotateBase, int msDuration) {
+	   myleft -> overrideCurrentPosition(0);
+	   myright -> overrideCurrentPosition(0);
+	   myleft -> startInterpolationDegrees(degreesToRotateBase * WHEEL_DEGREES_TO_BODY_DEGREES, msDuration, LIN);
+	   myright -> startInterpolationDegrees(degreesToRotateBase * WHEEL_DEGREES_TO_BODY_DEGREES, msDuration, LIN);
+}
 
+void DrivingChassis::turnToHeading(float degreesToRotateBase, int msDuration){
+	motionSetpoint = degreesToRotateBase;
+	timeout_ms = msDuration;
+	startTimeOfMovement_ms = millis();
+	motionType = TURNING;
 }
 
 /**
@@ -98,8 +160,124 @@ void DrivingChassis::turnDegrees(float degreesToRotateBase, int msDuration) {
  *
  * @note this function is fast-return and should not block
  */
-bool DrivingChassis::isChassisDoneDriving() {
-	return false;
+DrivingStatus DrivingChassis::statusOfChassisDriving() {
+	switch(motionType){
+
+	    case TURNING:{
+			 // check for timeout
+			 if((millis() - startTimeOfMovement_ms) > timeout_ms){
+					//timeout occured. Stop the robot
+					Serial.println("Detected Timeout\r\n");
+					stop();
+					performingMovement = false;
+					return TIMED_OUT;
+			   }
+
+			float currentHeading = IMU->getEULER_azimuth();
+			float headingError = - currentHeading - motionSetpoint;
+			float motorEffort = (turningMovementKp) * headingError;
+			myChassisPose.heading = -currentHeading; // - to account for what is considered a "positive" rotation
+			if(fabs(headingError) <= wheelMovementDeadband_deg)
+			{
+				Serial.println("Reached Setpoint\r\n");
+				stop();
+				return REACHED_SETPOINT;
+			}
+			else{
+					if(fabs(motorEffort) > MAX_MOTOR_EFFORT_DURING_TURN)
+					{
+						if(motorEffort < 0){
+							motorEffort = -MAX_MOTOR_EFFORT_DURING_TURN;
+						}
+						else if(motorEffort > 0){
+							motorEffort = MAX_MOTOR_EFFORT_DURING_TURN;
+						}
+					}
+					   myleft->setVelocityDegreesPerSecond(-motorEffort);
+					   myright->setVelocityDegreesPerSecond(-motorEffort);
+			}
+	    }
+		    break;
+
+	    case DRIVING_FORWARDS:{
+	    		// check for timeout
+	    		if((millis() - startTimeOfMovement_ms) > timeout_ms){
+	    			//timeout occured. Stop the robot
+	    			Serial.println("Detected Timeout\r\n");
+	    			stop();
+	    			return TIMED_OUT;
+	    		}
+
+	    	    if(motionSetpoint != -1){
+	    	    	float currentDistanceMovedRightWheel_mm = (myright -> getAngleDegrees())*WHEEL_DEGREES_TO_MM;
+	    	    	float rightWheelError_mm = currentDistanceMovedRightWheel_mm - motionSetpoint;
+	    	    	driveStraight(myChassisPose.heading, DRIVING_FORWARDS);
+	    	    	if((fabs(rightWheelError_mm) < wheelMovementDeadband_mm)){
+						Serial.println("Reached Setpoint \r\n");
+						stop();
+						return REACHED_SETPOINT;
+					}
+	    	    }
+
+	    	    else{
+	    	    	// sets speed to 20 cm per second
+	    	    	driveStraight(myChassisPose.heading, DRIVING_FORWARDS);
+	    	    }
+	        }
+	        break;
+
+	    case DRIVING_BACKWARDS:{
+	    	// check for timeout
+	    		if((millis() - startTimeOfMovement_ms) > timeout_ms){
+	    			//timeout occured. Stop the robot
+	    			Serial.println("Detected Timeout\r\n");
+	    			stop();
+	    			return TIMED_OUT;
+	    		}
+
+	    	    if(motionSetpoint != -1){
+	    	    	float currentDistanceMovedRightWheel_mm = (myright -> getAngleDegrees())*WHEEL_DEGREES_TO_MM;
+	    	    	float rightWheelError_mm = - currentDistanceMovedRightWheel_mm - motionSetpoint;
+	    	    	driveStraight(myChassisPose.heading, DRIVING_BACKWARDS);
+	    	    	if((fabs(rightWheelError_mm) < wheelMovementDeadband_mm)){
+						Serial.println("Reached Setpoint \r\n");
+						stop();
+						return REACHED_SETPOINT;
+					}
+	    	    }
+
+	    	    else{
+	    	    	// sets speed to 20 cm per second
+	    	    	driveStraight(myChassisPose.heading, DRIVING_BACKWARDS);
+	    	    }
+	        }
+	        break;
+
+	    default:
+	    	 break;
+	}
+	return GOING_TO_SETPOINT;
+}
+
+void DrivingChassis::stop(){
+	myleft -> stop();
+	myright -> stop();
+}
+
+void DrivingChassis::driveStraight(float targetHeading, MotionType direction){
+	float currentHeading = IMU->getEULER_azimuth();
+	float headingError = - currentHeading - targetHeading;
+	float motorEffort = (turningMovementKp) * headingError;
+
+	if(direction == DRIVING_BACKWARDS){
+		myleft->setVelocityDegreesPerSecond((MAX_SPEED_MM_PER_SEC - motorEffort)*MM_TO_WHEEL_DEGREES);
+		myright->setVelocityDegreesPerSecond((-MAX_SPEED_MM_PER_SEC - motorEffort)*MM_TO_WHEEL_DEGREES);
+	}
+
+	else if(direction == DRIVING_FORWARDS){
+		myright->setVelocityDegreesPerSecond((MAX_SPEED_MM_PER_SEC - motorEffort)*MM_TO_WHEEL_DEGREES);
+	    myleft->setVelocityDegreesPerSecond((-MAX_SPEED_MM_PER_SEC - motorEffort)*MM_TO_WHEEL_DEGREES);
+	}
 }
 /**
  * loop()

DrivingChassis.h

@@ -10,6 +10,35 @@
 #include "src/pid/PIDMotor.h"
 #include "src/commands/GetIMU.h"
 #include "config.h"
+#include "Pose.h"
+
+#define WHEEL_DEGREES_TO_BODY_DEGREES 4.25F
+#define MM_TO_WHEEL_DEGREES 2.1174F
+#define WHEEL_DEGREES_TO_MM .472277F
+#define MAX_SPEED_MM_PER_SEC 150
+#define MAX_MOTOR_EFFORT_DURING_TURN 260 //300 //275 // 500
+
+
+/**
+ * @enum DrivingStatus
+ * States when performing an drive action.
+ */
+enum DrivingStatus {
+	REACHED_SETPOINT = 0,
+	TIMED_OUT = 1,
+	GOING_TO_SETPOINT = 2,
+};
+
+/**
+ * @enum MotionType
+ * States when performing an drive action.
+ */
+enum MotionType {
+	DRIVING_FORWARDS = 0,
+	DRIVING_BACKWARDS = 1,
+	TURNING = 2,
+};
+
 /**
  * DrivingChassis encapsulates a 2 wheel differential steered chassis that drives around
  *
@@ -25,8 +54,6 @@
  */
 class DrivingChassis {
 private:
-	PIDMotor * myleft;
-	PIDMotor * myright;
 	GetIMU * IMU;
 	float mywheelTrackMM;
 	float mywheelRadiusMM;
@@ -58,6 +85,20 @@ class DrivingChassis {
 	 */
 	float chassisRotationToWheelDistance(float angle);
 public:
+	// moved these over for line following
+	PIDMotor * myleft;
+	PIDMotor * myright;
+	bool performingMovement = false;
+	MotionType motionType;
+	unsigned long startTimeOfMovement_ms;
+	float wheelMovementKp = 3.5;// was 3.9
+	float turningMovementKp = 21; //was 9, 11.7, 17.5
+	float wheelMovementDeadband_mm = 2.5;
+	float wheelMovementDeadband_deg = .5;
+	float motionSetpoint;
+	float timeout_ms;
+	Pose myChassisPose;
+
 	virtual ~DrivingChassis();
 
 	/**
@@ -73,18 +114,52 @@ class DrivingChassis {
 			float wheelRadiusMM,GetIMU * imu);
 
 	/**
-	 * Start a drive forward action
+	 * Start a drive backwards action using the encoders and setpoint interpolation
+	 *
+	 * @param mmDistanceFromCurrent is the distance the mobile base should drive backwards
+	 * @param msDuration is the time in miliseconds that the drive action should take
+	 *
+	 * @note this function is fast-return and should not block
+	 */
+	void driveBackwardsFromInterpolation(float mmDistanceFromCurrent, int msDuration);
+
+	/**
+	 * Start a drive backwards action. Will return false when finished moving. Will stop when reached setpoint or timeout.
+	 * Specifying a mmDistanceFromCurrent of '0' will move the robot indefinitely.
+	 *
+	 * @param mmDistanceFromCurrent is the distance the mobile base should drive backwards
+	 * @param msDuration is the time in miliseconds that the drive action should take (this is a timeout)
+	 *
+	 * @note this function is fast-return and should not block. Whatever is calling this should repeatedly do so
+	 * until this function returns false due to reaching the set-point or timing out.
+	 */
+	void driveBackwards(float mmDistanceFromCurrent, int msDuration);
+
+	/**
+	 * Start a drive forward action using the encoders and setpoint interpolation
 	 *
 	 * @param mmDistanceFromCurrent is the distance the mobile base should drive forward
 	 * @param msDuration is the time in miliseconds that the drive action should take
 	 *
 	 * @note this function is fast-return and should not block
-	 * @note pidmotorInstance->overrideCurrentPosition(0); can be used to "zero out" the motor to
-	 * 		 allow for relative moves. Otherwise the motor is always in ABSOLUTE mode
+	 */
+	void driveForwardFromInterpolation(float mmDistanceFromCurrent, int msDuration);
+
+	/**
+	 * Start a drive forwards action. Will return false when finished moving. Will stop when reached setpoint or timeout.
+	 * Specifying a mmDistanceFromCurrent of '0' will move the robot indefinitely.
+	 *
+	 * @param mmDistanceFromCurrent is the distance the mobile base should drive forward
+	 * @param msDuration is the time in miliseconds that the drive action should take (this is a timeout)
+	 *
+	 * @note this function is fast-return and should not block. Whatever is calling this should repeatedly do so
+	 * until this function returns false due to reaching the set-point or timing out. Make sure to change state when this reaches a setpoint.
+	 * Otherwise, this will try to reach a new setpoint (imagine driving indefinitely)
 	 */
 	void driveForward(float mmDistanceFromCurrent, int msDuration);
+
 	/**
-	 * Start a turn action
+	 * Start a turn action using the encoders and setpoint interpolation
 	 *
 	 * This action rotates the robot around the center line made up by the contact points of the left and right wheels.
 	 * Positive angles should rotate to the left
@@ -98,15 +173,40 @@ class DrivingChassis {
 	 *  @note pidmotorInstance->overrideCurrentPosition(0); can be used to "zero out" the motor to
 	 * 		  allow for relative moves. Otherwise the motor is always in ABSOLUTE mode
 	 */
-	void turnDegrees(float degreesToRotateBase, int msDuration);
+	void turnDegreesFromInterpolation(float degreesToRotateBase, int msDuration);
+
+	/**
+	 * Start a turn action. Will return false when finished moving. Will stop when reached setpoint or timeout.
+	 *
+	 * This action rotates the robot around the center line made up by the contact points of the left and right wheels.
+	 * Positive angles should rotate to the left
+	 *
+	 * This rotation is a positive rotation about the Z axis of the robot.
+	 *
+	 * @param desiredHeading is the desired angle the robot should reach
+	 * @param msDuration is the time in miliseconds that the drive action should take (this is a timeout)
+	 *
+	 * @note this function is fast-return and should not block. Whatever is calling this should repeatedly do so
+	 * until this function returns false due to reaching the set-point or timing out.
+	 */
+	void turnToHeading(float desiredHeading, int msDuration);
+
 	/**
 	 * Check to see if the chassis is performing an action
 	 *
 	 * @return false is the chassis is driving, true is the chassis msDuration has elapsed
 	 *
 	 * @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()
 	 *