working in scc. added vx check and removed serial encoder from steeri…

…ng estimation
hytech-racing · Jun 10, 2024 · 976b05a · 976b05a
1 parent 2ae2295
commit 976b05a
Show file tree

Hide file tree

Showing 3 changed files with 25 additions and 80 deletions.
diff --git a/lib/systems/include/CASESystem.tpp b/lib/systems/include/CASESystem.tpp
@@ -26,7 +26,22 @@ DrivetrainCommand_s CASESystem<message_queue>::evaluate(
     // in.YawRaterads = 2.5;
 
     // REAL
-    in.Vx_B = vn_data.velocity_x;
+    // if ( (vn_status < 2) || (vn_data.velocity_x < 0) )
+    // {
+    //     if 
+    //     in.Vx_B = 0;
+    // } else {
+    //     in.Vx_B = vn_data.velocity_x;
+    // }
+
+    if (vn_data.velocity_x < 0)
+    {
+        in.Vx_B = 0;
+    } else {
+        in.Vx_B = vn_data.velocity_x;
+    }
+
+
 
     in.TCSVelThreshold = config_.tcsVelThreshold;
 
@@ -168,14 +183,7 @@ DrivetrainCommand_s CASESystem<message_queue>::evaluate(
 
     in.useNL_TCS_SlipSchedule = config_.useNL_TCS_SlipSchedule;
 
-    if ((vn_active_start_time_ == 0) && (vn_status >= 2))
-    {
-        vn_active_start_time_ = tick.millis;
-    }
-    else if (vn_status < 2)
-    {
-        vn_active_start_time_ = 0;
-    }
+
 
     case_.setExternalInputs(&in);
     if ((tick.millis - last_eval_time_) >= 1)

diff --git a/lib/systems/src/SteeringSystem.cpp b/lib/systems/src/SteeringSystem.cpp
@@ -8,88 +8,25 @@ void SteeringSystem::tick(const SteeringSystemTick_s &intake)
     // 2. Computes internal state
     if (intake.tick.triggers.trigger100)
     {
-        // Poll upper steering sensor
-        primarySensor_->sample();
-        primaryConversion_ = primarySensor_->convert();
-
-        // Filter both sensor angle readings
-        filteredAnglePrimary_ = steeringFilters_[0].filtered_result(primaryConversion_.angle);
-        filteredAngleSecondary_ = steeringFilters_[1].filtered_result(intake.secondaryConversion.conversion);        
-
-        // Both sensors are nominal and agree
-        // #ifdef ARDUINO_TEENSY41
-        // if (
-        //     (primaryConversion_.status == SteeringEncoderStatus_e::STEERING_ENCODER_NOMINAL)
-        //     && (intake.secondaryConversion.status == AnalogSensorStatus_e::ANALOG_SENSOR_GOOD)
-        //     && (abs(filteredAnglePrimary_ - filteredAngleSecondary_) < STEERING_DIVERGENCE_WARN_THRESHOLD)
-        // )
-        // #else
-        // if (
-        //     (primaryConversion_.status == SteeringEncoderStatus_e::STEERING_ENCODER_NOMINAL)
-        //     && (intake.secondaryConversion.status == AnalogSensorStatus_e::ANALOG_SENSOR_GOOD)
-        //     && (std::abs(filteredAnglePrimary_ - filteredAngleSecondary_) < STEERING_DIVERGENCE_WARN_THRESHOLD)
-        // )
-        // #endif
-        // {
-        //     steeringData_ = {
-        //         .angle = filteredAnglePrimary_,
-        //         .status = SteeringSystemStatus_e::STEERING_SYSTEM_NOMINAL
-        //     };
-        // }
-        // // One or both sensors are marginal
-        // // Sensors disagree by STEERING_DIVERGENCE_WARN_THRESHOLD degrees and less than STEERING_DIVERGENCE_ERROR_THRESHOLD degrees
-        // #ifdef ARDUINO_TEENSY41
-        // else if (
-        //     (primaryConversion_.status == SteeringEncoderStatus_e::STEERING_ENCODER_MARGINAL)
-        //     || ((primaryConversion_.status == SteeringEncoderStatus_e::STEERING_ENCODER_NOMINAL) && (intake.secondaryConversion.status == AnalogSensorStatus_e::ANALOG_SENSOR_CLAMPED))
-        //     || ((abs(filteredAnglePrimary_ - filteredAngleSecondary_) >= STEERING_DIVERGENCE_WARN_THRESHOLD) && (abs(filteredAnglePrimary_ - filteredAngleSecondary_) < STEERING_DIVERGENCE_ERROR_THRESHOLD))
-        // )
-        // #else
-        // else if (
-        //     (primaryConversion_.status == SteeringEncoderStatus_e::STEERING_ENCODER_MARGINAL)
-        //     || ((primaryConversion_.status == SteeringEncoderStatus_e::STEERING_ENCODER_NOMINAL) && (intake.secondaryConversion.status == AnalogSensorStatus_e::ANALOG_SENSOR_CLAMPED))
-        //     || ((std::abs(filteredAnglePrimary_ - filteredAngleSecondary_) >= STEERING_DIVERGENCE_WARN_THRESHOLD) && (abs(filteredAnglePrimary_ - filteredAngleSecondary_) < STEERING_DIVERGENCE_ERROR_THRESHOLD))
-        // )
-        // #endif
-        // {
-        //     steeringData_ = {
-        //         .angle = filteredAnglePrimary_,
-        //         .status = SteeringSystemStatus_e::STEERING_SYSTEM_MARGINAL
-        //     };
-        // }
-        // Upper steering sensor reports error or is not detected, lower sensor is nominal
-        if ((intake.secondaryConversion.status == AnalogSensorStatus_e::ANALOG_SENSOR_GOOD))
-        {
-            steeringData_ = {
-                .angle = filteredAngleSecondary_,
-                .status = SteeringSystemStatus_e::STEERING_SYSTEM_DEGRADED
-            };
-        }
-        // Fall through case
-        // Complete failure of steering sensing
-        else
-        {
-            steeringData_ = {
-                .angle = 0.0,
-                .status = SteeringSystemStatus_e::STEERING_SYSTEM_ERROR
-            };
-        }
-
+        filteredAngleSecondary_ = steeringFilters_[1].filtered_result(intake.secondaryConversion.conversion);
+        primaryConversion_.status = SteeringEncoderStatus_e::STEERING_ENCODER_ERROR;
+        steeringData_.status = SteeringSystemStatus_e::STEERING_SYSTEM_NOMINAL;
+        steeringData_ = {
+            .angle = filteredAngleSecondary_,
+            .status = SteeringSystemStatus_e::STEERING_SYSTEM_DEGRADED};
     }
 
     // Report at 50Hz
     if (intake.tick.triggers.trigger50)
     {
         reportSteeringStatus(
             steeringData_.angle,
-            filteredAnglePrimary_,
+            0,
             filteredAngleSecondary_,
             static_cast<uint8_t>(steeringData_.status),
             static_cast<uint8_t>(primaryConversion_.status),
             static_cast<uint8_t>(intake.secondaryConversion.status));
     }
-
-
 }
 
 void SteeringSystem::reportSteeringStatus(const float angle,

diff --git a/src/main.cpp b/src/main.cpp
@@ -556,7 +556,7 @@ void tick_all_systems(const SysTick_s &current_system_tick)
         0,
         fsm.get_state(),
         dashboard.startButtonPressed(),
-        3);
+        vn_interface.get_vn_struct().vn_status);
 
     // case_system.update_config_from_param_interface(param_interface);