Thread Safety added to different subsystems

include/ARMS/chassis.h

@@ -8,7 +8,7 @@
 
 namespace arms::chassis {
 
-extern double maxSpeed;
+extern std::atomic<double> maxSpeed;
 extern std::shared_ptr<pros::Motor_Group> leftMotors;
 extern std::shared_ptr<pros::Motor_Group> rightMotors;
 

include/ARMS/odom.h

@@ -6,7 +6,10 @@
 
 namespace arms::odom {
 
-typedef enum EncoderType { ENCODER_ADI, ENCODER_ROTATION } EncoderType_e_t;
+typedef enum EncoderType { 
+    ENCODER_ADI = 0, 
+    ENCODER_ROTATION
+} EncoderType_e_t;
 
 // Odom Configuration
 typedef struct config_data_s {

include/ARMS/pid.h

@@ -2,6 +2,8 @@
 #define _ARMS_PID_H_
 
 #include <array>
+#include <mutex>
+#include <atomic>
 
 namespace arms::pid {
 
@@ -12,21 +14,21 @@ extern int mode;
 #define ANGULAR 2
 
 // pid constants
-extern double linearKP;
-extern double linearKI;
-extern double linearKD;
-extern double angularKP;
-extern double angularKI;
-extern double angularKD;
-extern double trackingKP;
-extern double minError;
+extern std::atomic<double> linearKP;
+extern std::atomic<double> linearKI;
+extern std::atomic<double> linearKD;
+extern std::atomic<double> angularKP;
+extern std::atomic<double> angularKI;
+extern std::atomic<double> angularKD;
+extern std::atomic<double> trackingKP;
+extern std::atomic<double> minError;
 
 // integral
-extern double in_lin;
-extern double in_ang;
+extern std::atomic<double> in_lin;
+extern std::atomic<double> in_ang;
 
 // targets
-extern double angularTarget;
+extern std::atomic<double> angularTarget;
 extern Point pointTarget;
 
 // flags

include/ARMS/point.h

@@ -3,7 +3,7 @@
 
 #include <array>
 #include <cmath>
-
+#include <atomic> 
 namespace arms {
 
 /**

src/ARMS/chassis.cpp

@@ -1,38 +1,45 @@
 #include "ARMS/api.h"
 #include "api.h"
+#include "chassis.h"
 #include "pros/motors.h"
-
+#include "pros/rtos.hpp"
 #include <tuple>
+#include <mutex>
+#include <atomic>
 
 namespace arms::chassis {
 
 // chassis motors
 std::shared_ptr<pros::Motor_Group> leftMotors;
 std::shared_ptr<pros::Motor_Group> rightMotors;
 
+// thread safety mutex
+pros::Mutex chassisMutex;
+
 // slew control (autonomous only)
 double slew_step; // smaller number = more slew
 
 // default exit error
-double linear_exit_error;
-double angular_exit_error;
+std::atomic<double> linear_exit_error;
+std::atomic<double> angular_exit_error;
 
 // settling
-double settle_thresh_linear;
-double settle_thresh_angular;
-int settle_time;
+std::atomic<double> settle_thresh_linear;
+std::atomic<double> settle_thresh_angular;
+std::atomic<int> settle_time;
 
 // chassis variables
-double maxSpeed = 100;
-double leftPrev = 0;
-double rightPrev = 0;
-double leftDriveSpeed = 0;
-double rightDriveSpeed = 0;
+std::atomic<double> maxSpeed = 100;
+std::atomic<double> leftPrev = 0;
+std::atomic<double> rightPrev = 0;
+std::atomic<double> leftDriveSpeed = 0;
+std::atomic<double> rightDriveSpeed = 0;
 
 /**************************************************/
 // motor control
 void motorMove(std::shared_ptr<pros::Motor_Group> motor, double speed,
                bool velocity) {
+	const std::lock_guard<pros::Mutex> lock(chassisMutex);
 	if (velocity)
 		motor->move_velocity(speed * (double)motor->get_gearing()[0] / 100);
 	else
@@ -54,6 +61,7 @@ void setBrakeMode(pros::motor_brake_mode_e_t b) {
 /**************************************************/
 // speed control
 double limitSpeed(double speed, double max) {
+	const std::lock_guard<pros::Mutex> lock(chassisMutex);
 	if (speed > max)
 		speed = max;
 	if (speed < -max)
@@ -63,7 +71,7 @@ double limitSpeed(double speed, double max) {
 }
 
 double slew(double target_speed, double step, double current_speed) {
-
+	const std::lock_guard<pros::Mutex> lock(chassisMutex);
 	if (fabs(current_speed) > fabs(target_speed))
 		step = 200;
 
@@ -80,6 +88,8 @@ double slew(double target_speed, double step, double current_speed) {
 /**************************************************/
 // settling
 bool settled() {
+	const std::lock_guard<pros::Mutex> lock(chassisMutex);
+
 	// previous position values
 	static Point p_pos = {0, 0};
 	static double p_ang = 0;
@@ -137,6 +147,8 @@ void waitUntilFinished(double exit_error) {
 // 2D movement
 void move(std::vector<double> target, double max, double exit_error, double lp,
           double ap, MoveFlags flags) {
+	chassisMutex.take();
+
 	pid::mode = TRANSLATIONAL;
 
 	double x = target.at(0);
@@ -170,11 +182,19 @@ void move(std::vector<double> target, double max, double exit_error, double lp,
 	pid::in_ang = 0;
 
 	if (!(flags & ASYNC)) {
+
+		chassisMutex.give();
 		waitUntilFinished(exit_error);
 		pid::mode = DISABLE;
-		if (!(flags & THRU))
+		chassisMutex.take();
+
+		if (!(flags & THRU)) {
+			chassisMutex.give();
 			chassis::setBrakeMode(pros::E_MOTOR_BRAKE_BRAKE);
+			chassisMutex.take();
+		}
 	}
+	chassisMutex.give();
 }
 
 void move(std::vector<double> target, double max, double exit_error,
@@ -208,6 +228,7 @@ void move(double target, MoveFlags flags) {
 // rotational movement
 void turn(double target, double max, double exit_error, double ap,
           MoveFlags flags) {
+	chassisMutex.take();	
 	pid::mode = ANGULAR;
 
 	double bounded_heading = (int)(odom::getHeading()) % 360;
@@ -231,11 +252,19 @@ void turn(double target, double max, double exit_error, double ap,
 	pid::in_ang = 0; // reset the integral value to zero
 
 	if (!(flags & ASYNC)) {
+
+		chassisMutex.give();
 		waitUntilFinished(exit_error);
 		pid::mode = DISABLE;
-		if (!(flags & THRU))
+		chassisMutex.take();
+
+		if (!(flags & THRU)) {
+			chassisMutex.give();
 			chassis::setBrakeMode(pros::E_MOTOR_BRAKE_BRAKE);
+			chassisMutex.take();
+		}
 	}
+	chassisMutex.give();
 }
 
 void turn(double target, double max, double exit_error, MoveFlags flags) {
@@ -276,6 +305,7 @@ int chassisTask() {
 	while (1) {
 		pros::delay(10);
 
+		chassisMutex.take();
 		std::array<double, 2> speeds = {0, 0}; // left, right
 
 		if (pid::mode == TRANSLATIONAL)
@@ -284,6 +314,7 @@ int chassisTask() {
 			speeds = pid::angular();
 		else
 			speeds = {leftDriveSpeed, rightDriveSpeed};
+		chassisMutex.give();
 
 		// speed limiting
 		speeds[0] = limitSpeed(speeds[0], maxSpeed);
@@ -307,7 +338,7 @@ void init(std::initializer_list<int8_t> leftMotors,
           double linear_exit_error, double angular_exit_error,
           double settle_thresh_linear, double settle_thresh_angular,
           int settle_time) {
-
+	const std::lock_guard<pros::Mutex> lock(chassisMutex);
 	// assign constants
 	chassis::slew_step = slew_step;
 	chassis::linear_exit_error = linear_exit_error;
@@ -330,13 +361,15 @@ void init(std::initializer_list<int8_t> leftMotors,
 /**************************************************/
 // operator control
 void tank(double left_speed, double right_speed, bool velocity) {
+	const std::lock_guard<pros::Mutex> lock(chassisMutex);
 	pid::mode = DISABLE; // turns off autonomous tasks
 	maxSpeed = 100;
 	chassis::leftDriveSpeed = left_speed;
 	chassis::rightDriveSpeed = right_speed;
 }
 
 void arcade(double vertical, double horizontal, bool velocity) {
+	const std::lock_guard<pros::Mutex> lock(chassisMutex);
 	pid::mode = DISABLE; // turns off autonomous task
 	maxSpeed = 100;
 	chassis::leftDriveSpeed = vertical + horizontal;