Merge pull request #1 from TourGuideSeniorDesign/feature/joystick-logs

Implemented direct joystick logs, will change detected errors in followup commit

Author: alexanderlindquis

MicrocontrollerCode/include/JoystickFunctions.h

@@ -15,13 +15,28 @@ struct RefSpeed {
     int8_t rightSpeed;     ///< Speed of the right wheel.
 };
 
+/**
+ * Struct representing the reference displace
+ */
+struct RefDisplacement {
+    int8_t longDisp;      ///< Forward/Backward displacement with + indicating forward
+    int8_t latDisp;     ///< Side to side displacement with + indicating right
+};
+
 /**
  * Reads a value from the joystick connected to the ADC and returns the reference speeds
  * @param adc An instance of the adc the joystick is connected to
  * @return A RefSpeed for the wheelchair containing the wheel speeds and the direction
  */
 RefSpeed joystickToSpeed(Adafruit_ADS1115 &adc);
 
+/**
+ * Reads a value from the joystick connected to the ADC and returns the reference displacement
+ * @param adc An instance of the adc the joystick is connected to
+ * @return A RefDisplacement for the joystick
+ */
+RefDisplacement joystickToDisplacement(Adafruit_ADS1115 &adc);
+
 template <typename T>
 /**
  * Custom clamp function to keep a value between to values

MicrocontrollerCode/src/JoystickFunctions.cpp

@@ -9,6 +9,28 @@
 int diffParam = 30;
 int deadzoneParam = 30;
 
+RefDisplacement joystickToDisplacement(Adafruit_ADS1115 &adc){
+    int forwardJoystick = adc.readADC_SingleEnded(0); //a0 is forward/backward
+    int sidewaysJoystick = adc.readADC_SingleEnded(1); //a1 is left/right
+
+    RefDisplacement displacements;
+    /*
+     * Joystick middle values: ~8500
+     * a0 middle value: ~8500
+     * a1 middle value: ~8300
+     * a0 deadzone 10000 - 6500
+     * a1 deadzone 11000 - 6000
+     * Joystick Min: 0
+     * Joystick Max: 17390
+     * Output is a value -100 to 100 for the speed of the motor
+     */
+
+    //Converting the speeds so they start around 0 and then go positive and negative
+    displacements.longDisp = forwardJoystick - (8500+4400); //The second value is used to zero it out when the ADC gain is set to 0 instead of the default (2/3)
+    displacements.latDisp = sidewaysJoystick - (8400+4400);
+    return displacements;
+}
+
 RefSpeed joystickToSpeed(Adafruit_ADS1115 &adc){
     int forwardJoystick = adc.readADC_SingleEnded(0); //a0 is forward/backward
     int sidewaysJoystick = adc.readADC_SingleEnded(1); //a1 is left/right

MicrocontrollerCode/src/main.cpp

@@ -104,10 +104,14 @@ void loop() {
     //TODO might want to figure out how to put these on core1 so that they can run in parallel
     //uint32_t start = millis();
     RefSpeed omegaRef{};
+    RefDisplacement thetaRef{};
     if (!joystick_adc_error) {
         omegaRef = joystickToSpeed(joystickAdc);
+        thetaRef = joystickToDisplacement(joystickAdc);
     }
 
+    
+
 
     //uint32_t joystickTime = millis() - start;
     USData usDistances{};
@@ -147,7 +151,7 @@ void loop() {
 
     microRosTick();
 
-    transmitMsg(omegaRef, usDistances, pirSensors, fanSpeeds, imuData);
+    transmitMsg(thetaRef,omegaRef, usDistances, pirSensors, fanSpeeds, imuData);
 
     if (currentMillis - lastErrorTime >= error_timer) {
         lastErrorTime = currentMillis;
@@ -160,7 +164,7 @@ void loop() {
     #elif ROS_DEBUG
 
 
-    transmitMsg(omegaRef);
+    transmitMsg(thetaRef,omegaRef);
 
 
     #elif DEBUG

MicrocontrollerCode/src/microRosFunctions.cpp

@@ -373,7 +373,9 @@ static void lidar_subscription_callback(const void *msgin){
     }
 
 #ifdef ROS
-void transmitMsg(RefSpeed omegaRef, USData ultrasonicData, PIRSensors pirSensors, FanSpeeds fanSpeeds, IMUData imuData) {
+void transmitMsg(RefDisplacement thetaRef, RefSpeed omegaRef, USData ultrasonicData, PIRSensors pirSensors, FanSpeeds fanSpeeds, IMUData imuData) {
+    sensorMsg.long_disp = thetaRef.longDisp;
+    sensorMsg.lat_disp = thetaRef.latDisp;
     sensorMsg.left_speed = omegaRef.leftSpeed;
     sensorMsg.right_speed = omegaRef.rightSpeed;
     sensorMsg.ultrasonic_front_0 = ultrasonicData.us_front_0;
@@ -406,9 +408,11 @@ void transmitMsg(RefSpeed omegaRef, USData ultrasonicData, PIRSensors pirSensors
 
 #elif ROS_DEBUG
 
-void transmitMsg(RefSpeed omegaRef){
+void transmitMsg(RefDisplacement thetaRef, RefSpeed omegaRef){
     msg.left_speed = omegaRef.leftSpeed;
     msg.right_speed = omegaRef.rightSpeed;
+    msg.long_disp = thetaRef.longDisp;
+    msg.lat_disp = thetaRef.latDisp;
 
     RCSOFTCHECK(rclc_executor_spin_some(&executor, RCL_MS_TO_NS(10)));
 }