Add lower bounds to speeds (#104)

Co-authored-by: Mihir Laud <[email protected]>

include/ARMS/chassis.h

@@ -1,14 +1,16 @@
 #ifndef _ARMS_CHASSIS_H_
 #define _ARMS_CHASSIS_H_
 
+#include "../api.h"
 #include "ARMS/flags.h"
 #include "ARMS/point.h"
 #include <memory>
-#include "../api.h"
 
 namespace arms::chassis {
 
 extern double maxSpeed;
+extern double min_linear_speed;
+extern double min_angular_speed;
 extern std::shared_ptr<pros::Motor_Group> leftMotors;
 extern std::shared_ptr<pros::Motor_Group> rightMotors;
 
@@ -30,8 +32,8 @@ void waitUntilFinished(double exit_error);
 /**
  * Perform 2D chassis movement
  */
-void move(std::vector<double> target, double max, double exit_error,
-          double lp, double ap, MoveFlags = NONE);
+void move(std::vector<double> target, double max, double exit_error, double lp,
+          double ap, MoveFlags = NONE);
 void move(std::vector<double> target, double max, double exit_error,
           MoveFlags = NONE);
 void move(std::vector<double> target, double max, MoveFlags = NONE);
@@ -40,10 +42,9 @@ void move(std::vector<double> target, MoveFlags = NONE);
 /**
  * Perform 1D chassis movement
  */
-void move(double target, double max, double exit_error,
-          double lp, double ap, MoveFlags = NONE);
-void move(double target, double max, double exit_error,
+void move(double target, double max, double exit_error, double lp, double ap,
           MoveFlags = NONE);
+void move(double target, double max, double exit_error, MoveFlags = NONE);
 void move(double target, double max, MoveFlags = NONE);
 void move(double target, MoveFlags = NONE);
 
@@ -95,10 +96,11 @@ void arcade(double vertical, double horizontal, bool velocity = false);
  * initialize the chassis
  */
 void init(std::initializer_list<int8_t> leftMotors,
-          std::initializer_list<int8_t> rightMotors, pros::motor_gearset_e_t gearset,
-          double slew_step, double linear_exit_error, double angular_exit_error, 
+          std::initializer_list<int8_t> rightMotors,
+          pros::motor_gearset_e_t gearset, double slew_step,
+          double linear_exit_error, double angular_exit_error,
           double settle_thresh_linear, double settle_thresh_angular,
-          int settle_time);
+          int settle_time, double min_linear_speed, double min_angular_speed);
 
 } // namespace arms::chassis
 

include/ARMS/config.h

@@ -14,12 +14,14 @@ namespace arms {
 #define GEARSET pros::E_MOTOR_GEAR_200 // RPM of chassis motors
 
 // Ticks per inch
-#define TPI 1      			  // Encoder ticks per inch of forward robot movement
-#define MIDDLE_TPI 1          // Ticks per inch for the middle wheel
+#define TPI 1        // Encoder ticks per inch of forward robot movement
+#define MIDDLE_TPI 1 // Ticks per inch for the middle wheel
 
 // Tracking wheel distances
-#define TRACK_WIDTH 0 		  // The distance between left and right wheels (or tracker wheels)
-#define MIDDLE_DISTANCE 0     // Distance from middle wheel to the robot turning center
+#define TRACK_WIDTH                                                            \
+	0 // The distance between left and right wheels (or tracker wheels)
+#define MIDDLE_DISTANCE                                                        \
+	0 // Distance from middle wheel to the robot turning center
 
 // Sensors
 #define IMU_PORT 0                           // Port 0 for disabled
@@ -28,21 +30,24 @@ namespace arms {
 #define ENCODER_TYPE arms::odom::ENCODER_ADI // The type of encoders
 
 // Movement tuning
-#define SLEW_STEP 8          // Smaller number = more slew
-#define LINEAR_EXIT_ERROR 1  // default exit distance for linear movements
-#define ANGULAR_EXIT_ERROR 1 // default exit distance for angular movements
-#define SETTLE_THRESH_LINEAR .5      // amount of linear movement for settling
-#define SETTLE_THRESH_ANGULAR 1      // amount of angular movement for settling
-#define SETTLE_TIME 250      // amount of time to count as settled
+#define SLEW_STEP 8             // Smaller number = more slew
+#define LINEAR_EXIT_ERROR 1     // default exit distance for linear movements
+#define ANGULAR_EXIT_ERROR 1    // default exit distance for angular movements
+#define SETTLE_THRESH_LINEAR .5 // amount of linear movement for settling
+#define SETTLE_THRESH_ANGULAR 1 // amount of angular movement for settling
+#define SETTLE_TIME 250         // amount of time to count as settled
 #define LINEAR_KP 1
 #define LINEAR_KI 0
 #define LINEAR_KD 0
-#define TRACKING_KP 60		 // point tracking turning strength
+#define TRACKING_KP 60 // point tracking turning strength
 #define ANGULAR_KP 1
 #define ANGULAR_KI 0
 #define ANGULAR_KD 0
-#define MIN_ERROR 5          // Minimum distance to target before angular componenet is disabled
-#define LEAD_PCT .6			 // Go-to-pose lead distance ratio (0-1)
+#define MIN_ERROR                                                              \
+	5 // Minimum distance to target before angular componenet is disabled
+#define LEAD_PCT .6         // Go-to-pose lead distance ratio (0-1)
+#define MIN_LINEAR_SPEED 0  // Minimum speed for linear movements
+#define MIN_ANGULAR_SPEED 0 // Minimum speed for angular movements
 
 // Auton selector configuration constants
 #define AUTONS "Front", "Back", "Do Nothing" // Names of autonomi, up to 10
@@ -52,18 +57,19 @@ namespace arms {
 // Initializer
 inline void init() {
 
-	chassis::init({LEFT_MOTORS}, {RIGHT_MOTORS}, GEARSET, SLEW_STEP, LINEAR_EXIT_ERROR,
-	              ANGULAR_EXIT_ERROR, SETTLE_THRESH_LINEAR, SETTLE_THRESH_ANGULAR, SETTLE_TIME);
+	chassis::init({LEFT_MOTORS}, {RIGHT_MOTORS}, GEARSET, SLEW_STEP,
+	              LINEAR_EXIT_ERROR, ANGULAR_EXIT_ERROR, SETTLE_THRESH_LINEAR,
+	              SETTLE_THRESH_ANGULAR, SETTLE_TIME, MIN_LINEAR_SPEED,
+	              MIN_ANGULAR_SPEED);
 
 	odom::init(ODOM_DEBUG, ENCODER_TYPE, {ENCODER_PORTS}, EXPANDER_PORT, IMU_PORT,
-	           TRACK_WIDTH, MIDDLE_DISTANCE, TPI,
-	           MIDDLE_TPI);
+	           TRACK_WIDTH, MIDDLE_DISTANCE, TPI, MIDDLE_TPI);
 
-	pid::init(LINEAR_KP, LINEAR_KI, LINEAR_KD, ANGULAR_KP, ANGULAR_KI, ANGULAR_KD, TRACKING_KP, MIN_ERROR, LEAD_PCT);
+	pid::init(LINEAR_KP, LINEAR_KI, LINEAR_KD, ANGULAR_KP, ANGULAR_KI, ANGULAR_KD,
+	          TRACKING_KP, MIN_ERROR, LEAD_PCT);
 
 	const char* b[] = {AUTONS, ""};
 	selector::init(HUE, DEFAULT, b);
-
 }
 
 } // namespace arms

src/ARMS/chassis.cpp

@@ -24,6 +24,8 @@ int settle_time;
 
 // chassis variables
 double maxSpeed = 100;
+double min_linear_speed;
+double min_angular_speed;
 double leftPrev = 0;
 double rightPrev = 0;
 double leftDriveSpeed = 0;
@@ -394,7 +396,7 @@ void init(std::initializer_list<int8_t> leftMotors,
           pros::motor_gearset_e_t gearset, double slew_step,
           double linear_exit_error, double angular_exit_error,
           double settle_thresh_linear, double settle_thresh_angular,
-          int settle_time) {
+          int settle_time, double min_linear_speed, double min_angular_speed) {
 
 	// assign constants
 	chassis::slew_step = slew_step;
@@ -413,6 +415,9 @@ void init(std::initializer_list<int8_t> leftMotors,
 	chassis::rightMotors->set_gearing(gearset);
 
 	pros::Task chassis_task(chassisTask);
+
+	chassis::min_linear_speed = min_linear_speed;
+	chassis::min_angular_speed = min_angular_speed;
 }
 
 /**************************************************/

src/ARMS/pid.cpp

@@ -102,6 +102,9 @@ std::array<double, 2> translational() {
 	if (lin_speed > chassis::maxSpeed)
 		lin_speed = chassis::maxSpeed;
 
+	if (lin_speed < chassis::min_linear_speed)
+		lin_speed = chassis::min_linear_speed;
+
 	// apply direction
 	if (reverse)
 		lin_speed = -lin_speed;
@@ -120,6 +123,11 @@ std::array<double, 2> translational() {
 			while (fabs(poseError) > M_PI)
 				poseError -= 2 * M_PI * poseError / fabs(poseError);
 			ang_speed = pid(poseError, &pe_ang, &in_ang, trackingKP, 0, 0);
+
+			if (fabs(ang_speed) < chassis::min_angular_speed) {
+				ang_speed =
+				    (std::signbit(ang_speed) ? -1 : 1) * chassis::min_angular_speed;
+			}
 		}
 
 		// reduce the linear speed if the bot is tangent to the target