Merge pull request #143 from CU-Robotics/feature-damage-status-sending

Feature damage status sending

Author: Rishi-H

.gitignore

@@ -32,6 +32,7 @@ tools/*.out
 tools/git_scraper
 tools/custom_monitor
 tools/compiler
+tools/tools
 
 # Ignore the output from the git scraper
 src/git_info.h

src/controls/estimator.cpp

@@ -147,11 +147,6 @@ void GimbalEstimator::step_states(float output[STATE_LEN][3], float curr_state[S
     while (yaw_angle <= -PI)
         yaw_angle += 2 * PI;
 
-    while (chassis_angle >= PI)
-        chassis_angle -= 2 * PI;
-    while (chassis_angle <= -PI)
-        chassis_angle += 2 * PI;
-
     // output[2][0] = chassis_angle;
     // output[2][1] = 0;
     // output[2][2] = 0;
@@ -174,6 +169,10 @@ void GimbalEstimator::step_states(float output[STATE_LEN][3], float curr_state[S
     }
 
     chassis_angle = yaw_angle - yaw_enc_angle;
+    while (chassis_angle >= PI)
+        chassis_angle -= 2 * PI;
+    while (chassis_angle <= -PI)
+        chassis_angle += 2 * PI;
     // chassis_angle = -(total_odom_pos[0] + total_odom_pos[2])/(2*odom_axis_offset_x)+initial_chassis_angle;  
     float d_chassis_heading = (chassis_angle - prev_chassis_angle);
     if (d_chassis_heading > PI) d_chassis_heading -= 2 * PI;
@@ -573,6 +572,6 @@ void NewFeederEstimator::step_states(float output[CAN_MAX_MOTORS][MICRO_STATE_LE
     ball_count += diff/(M_PI/2.0);
     output[0][0] = ball_count;
     output[0][1] = feeder_velocity;
-    output[0][2] = 0;
+    output[0][2] = feeder_angle; // this is not the acceleration just the encoder value for debugging
 }
 

src/main.cpp

@@ -448,4 +448,4 @@ int main() {
     }
 
     return 0;
-}
+}

src/sensors/RefSystem.cpp

@@ -1,4 +1,5 @@
 #include "RefSystem.hpp"
+#include "RefSystemPacketDefs.hpp"
 
 uint8_t generateCRC8(uint8_t* data, uint32_t len) {
     uint8_t CRC8 = 0xFF;
@@ -90,12 +91,19 @@ CommsRefData RefSystem::get_data_for_comms() {
     memcpy(output_array.raw + REF_COMMS_ROBOT_POWER_HEAT_OFFSET, ref_data.robot_power_heat.raw, ref_data.robot_power_heat.packet_size);
     memcpy(output_array.raw + REF_COMMS_ROBOT_POSITION_OFFSET, ref_data.robot_position.raw, ref_data.robot_position.packet_size);
     memcpy(output_array.raw + REF_COMMS_ROBOT_BUFF_OFFSET, ref_data.robot_buff.raw, ref_data.robot_buff.packet_size);
-    memcpy(output_array.raw + REF_COMMS_DAMAGE_STATUS_OFFSET, ref_data.damage_status.raw, ref_data.damage_status.packet_size);
+
+    if(damage_status_changed) {
+        memcpy(output_array.raw + REF_COMMS_DAMAGE_STATUS_OFFSET, ref_data.damage_status.raw, ref_data.damage_status.packet_size);
+        damage_status_changed = false; // reset the flag
+    } else {
+        //if the damage status has not changed, send (15, 15) which means invalid
+        memset(output_array.raw + REF_COMMS_DAMAGE_STATUS_OFFSET, 255, ref_data.damage_status.packet_size);
+    }
+
     memcpy(output_array.raw + REF_COMMS_LAUNCHING_STATUS_OFFSET, ref_data.launching_status.raw, ref_data.launching_status.packet_size);
     memcpy(output_array.raw + REF_COMMS_PROJECTILE_ALLOWANCE_OFFSET, ref_data.projectile_allowance.raw, ref_data.projectile_allowance.packet_size);
     memcpy(output_array.raw + REF_COMMS_RFID_STATUS_OFFSET, ref_data.rfid_status.raw, ref_data.rfid_status.packet_size);
     memcpy(output_array.raw + REF_COMMS_KBM_INTERACTION_OFFSET, ref_data.kbm_interaction.raw, ref_data.kbm_interaction.packet_size);
-
     return output_array;
 }
 
@@ -280,6 +288,7 @@ void RefSystem::set_ref_data(Frame& frame, uint8_t raw_buffer[REF_MAX_PACKET_SIZ
         break;
     case FrameType::DAMAGE_STATUS:
         ref_data.damage_status.set_data(frame.data);
+        damage_status_changed = true;
         break;
     case FrameType::LAUNCHING_STATUS:
         ref_data.launching_status.set_data(frame.data);

src/sensors/RefSystem.hpp

@@ -148,6 +148,9 @@ class RefSystem {
     /// @brief Current sequence number. Used to send packets
     uint8_t seq = 0;
 
+    /// @brief whether we have a new damage status since we last sent ref data to comms
+    bool damage_status_changed = false;
+
     /// @brief Internal data struct for the RefSystem. This contains all the meta data required to read a frame
     struct RefInternalData {
         /// @brief Raw buffer to store incoming data