diff --git a/lib/systems/include/CASESystem.tpp b/lib/systems/include/CASESystem.tpp index 56ccd2994..3a6db48c7 100644 --- a/lib/systems/include/CASESystem.tpp +++ b/lib/systems/include/CASESystem.tpp @@ -26,7 +26,22 @@ DrivetrainCommand_s CASESystem::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::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 index ee54af775..e074cf57d 100644 --- a/lib/systems/src/SteeringSystem.cpp +++ b/lib/systems/src/SteeringSystem.cpp @@ -8,73 +8,12 @@ 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 @@ -82,14 +21,12 @@ void SteeringSystem::tick(const SteeringSystemTick_s &intake) { reportSteeringStatus( steeringData_.angle, - filteredAnglePrimary_, + 0, filteredAngleSecondary_, static_cast(steeringData_.status), static_cast(primaryConversion_.status), static_cast(intake.secondaryConversion.status)); } - - } void SteeringSystem::reportSteeringStatus(const float angle, diff --git a/src/main.cpp b/src/main.cpp index 36146165a..ee2cbdab4 100644 --- a/src/main.cpp +++ b/src/main.cpp @@ -556,7 +556,7 @@ void tick_all_systems(const SysTick_s ¤t_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);