From a7a6fb21edf6a2ab8c058d151c6b959e83640100 Mon Sep 17 00:00:00 2001 From: David Date: Wed, 20 Sep 2023 08:41:02 +0100 Subject: [PATCH] 2023 competition changes --- CMakeLists.txt | 3 + demo.cpp | 54 ++++++++++++ include/control/axle_control_frame.hpp | 7 +- include/simple_adas.hpp | 4 + setup_can.sh | 0 setup_vcan.sh | 0 src/control/axle_control_frame.cpp | 117 ++++++++----------------- src/example_threaded.cpp | 114 ++++++++++++++++++++++++ src/simple_adas.cpp | 18 ++-- 9 files changed, 230 insertions(+), 87 deletions(-) create mode 100644 demo.cpp mode change 100755 => 100644 setup_can.sh mode change 100755 => 100644 setup_vcan.sh create mode 100644 src/example_threaded.cpp diff --git a/CMakeLists.txt b/CMakeLists.txt index e3acab0..93d4a72 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -144,6 +144,9 @@ target_link_libraries( example_frame_reader PRIVATE candata status_frames ) add_executable( example_straight_line src/example_straight_line.cpp ) target_link_libraries( example_straight_line PUBLIC simple_adas ) +#add_executable( example_threaded src/example_threaded.cpp ) +#target_link_libraries( example_threaded PUBLIC simple_adas ) + add_executable( example_wiggle src/example_wiggle.cpp ) target_link_libraries( example_wiggle PUBLIC simple_adas ) diff --git a/demo.cpp b/demo.cpp new file mode 100644 index 0000000..c11f779 --- /dev/null +++ b/demo.cpp @@ -0,0 +1,54 @@ +#include "include/can_wrap.hpp" +#include "include/candata.h" + +#include + +int main() +{ + float f = 0.0E-1f; + + std::cout << std::fixed << f << "\n"; + + return 0; + + const std::string device = "vcan0"; + const int canSocket = can::connect( device ); + + while( true ) + { + try + { + const can_frame frame = can::read( canSocket ); + + if( frame.can_id == CANDATA_VCU2_AI_STATUS_FRAME_ID ) + { + candata_vcu2_ai_status_t data; + candata_vcu2_ai_status_unpack( &data, frame.data, frame.can_dlc ); + std::cout << "status: " << + candata_vcu2_ai_status_as_state_decode( data.as_state ) << "\n"; + } + else if( frame.can_id == CANDATA_VCU2_AI_BRAKE_FRAME_ID ) + { + candata_vcu2_ai_brake_t data; + candata_vcu2_ai_brake_unpack( &data, frame.data, frame.can_dlc ); + std::cout << "brake: " << std::fixed << std::setprecision( 10 ) << + candata_vcu2_ai_brake_hyd_press_f_pct_decode( data.hyd_press_f_pct ) << " " << + candata_vcu2_ai_brake_hyd_press_r_pct_decode( data.hyd_press_r_pct ) << "\n"; + } + else if( frame.can_id == CANDATA_VCU2_AI_DRIVE_F_FRAME_ID ) + { + candata_vcu2_ai_drive_f_t data; + candata_vcu2_ai_drive_f_unpack( &data, frame.data, frame.can_dlc ); + std::cout << "axle: " << + candata_vcu2_ai_drive_f_front_axle_trq_decode( data.front_axle_trq ) << "\n"; + } + // std::cout << "status frame" << "\n"; + } + catch( const can::error& ) + { + // no more frames to read + } + } + + return 0; +} \ No newline at end of file diff --git a/include/control/axle_control_frame.hpp b/include/control/axle_control_frame.hpp index ad16b70..4ec672b 100644 --- a/include/control/axle_control_frame.hpp +++ b/include/control/axle_control_frame.hpp @@ -30,6 +30,8 @@ namespace adas_api{ void set_torque( const float torque ); + void set_plaid( const bool plaid ); + /** * @brief Limit torque based on rpm. * @@ -40,7 +42,7 @@ namespace adas_api{ * @param rpm * @return float Limited torque. */ - static float torque_rpm_limit( const float rpm ); + float torque_rpm_limit( const float rpm ) const; constexpr static float motor_ratio = 3.5f; @@ -60,6 +62,8 @@ namespace adas_api{ virtual can_frame _process() const override; virtual void _print( std::ostream& os ) const override; + static float _plaid_mode( const float rpm ); + const AxleTorqueFrame &_torqueFrame; const BrakeFrame &_brakeFrame; const WheelSpeedsFrame &_speedsFrame; @@ -69,6 +73,7 @@ namespace adas_api{ const bool _front; float _speed = 0.f; float _torque = 0.f; + bool _plaidMode = false; }; }; diff --git a/include/simple_adas.hpp b/include/simple_adas.hpp index e883725..af4eeae 100644 --- a/include/simple_adas.hpp +++ b/include/simple_adas.hpp @@ -3,6 +3,7 @@ #include +#include #include namespace adas_api @@ -66,6 +67,8 @@ class SimpleAdas : protected Adas */ void set_angle( const float degrees ); + void set_plaid( const bool plaid ); + /** status control */ bool go() const; void finish(); @@ -90,6 +93,7 @@ class SimpleAdas : protected Adas private: static void _set_rpm( AxleControlFrame &axle, const float rpm ); + std::mutex _mutex; }; }; // namespace adas_api diff --git a/setup_can.sh b/setup_can.sh old mode 100755 new mode 100644 diff --git a/setup_vcan.sh b/setup_vcan.sh old mode 100755 new mode 100644 diff --git a/src/control/axle_control_frame.cpp b/src/control/axle_control_frame.cpp index b567dbc..b136e48 100644 --- a/src/control/axle_control_frame.cpp +++ b/src/control/axle_control_frame.cpp @@ -11,87 +11,9 @@ void adas_api::AxleControlFrame::set_torque( const float torque ) _torque = torque; } -float adas_api::AxleControlFrame::torque_rpm_limit( const float rpm ) +void adas_api::AxleControlFrame::set_plaid( const bool plaid ) { - /*if( rpm > 700 ) - return 50.f; - else if( rpm > 600 ) - return 85.f; - else if( rpm > 500 ) - return 100.f; - else if( rpm > 400 ) - return 120.f; - else if( rpm > 300 ) - return 150.f; - - return 200.f;*/ - - // Cavern Taylor's Code Potential Optimum from Ian's Limits (SET 21.9kW Motor Power Limit) - if ( rpm < 305 ) { - return 195.f; - } else if ( rpm > 800 ){ - return -0.02 + 59596 * pow(rpm,-0.999); - } else { - return 59596 * pow(rpm,-0.999); - } - - //***Cavern Taylor's Code Values using code from ideal curve smoothing (SET 24.95kW Motor Power Limit) - /*if ( rpm > 345 ) - return 194.5f; - else if ( rpm > 350 ) { - return 67896 * pow(rpm,-1); - } else { - return 195.f; - }*/ - - // ***07-03-2023 TESTING DATA POINTS*** - /*if( rpm > 470 ) - return 150.f; - else if( rpm > 430 ) - return 160.f; - else if( rpm > 390 ) - return 170.f; - - return 192.f;*/ - - //***Cavern Taylor's TESTING DATA POINT OPTIMISATION 1 (Straight line connecting data points) (wheel) - /*if ( rpm > 500 ) - return 150.f; - else if ( rpm > 470 ) - return -0.33 * rpm + 316.6; - else if ( rpm > 430 ) - return -0.25 * rpm + 277.5; - else if ( rpm > 380 ) - return -0.55 * rpm + 406.5; - else - return 190.f;*/ - - - - /*float e = ( -0.3055 * rpm ) + 284.05; - if( e > 200.f ) e = 200.f; - if( e < 0 ) e = 0; - return e;*/ - - /*if( rpm < 390 ) - return 195.f; - return 76169 * powf( rpm, -1 );*/ - - /*if( rpm <390 )return 192.f; - if( rpm < 430 ) return 170.f; - if( rpm < 470 ) return 160.f; - return 150.f;*/ - - // Cavern Taylor's Code to check potential maximum TORQUE limits - // if ( rpm > 305 ) { - // return 59596 * pow(rpm,-0.999); - // } else { - // return 195.f - // } // if ( rpm > 305 ) { - // return 59596 * pow(rpm,-0.999); - // } else { - // return 195.f - // } + _plaidMode = plaid; } can_frame adas_api::AxleControlFrame::_process() const @@ -168,6 +90,41 @@ void adas_api::AxleControlFrame::_print( std::ostream& os ) const { } +float adas_api::AxleControlFrame::_plaid_mode( const float rpm ) +{ + // cavern opt F + /*if( rpm > 500 ) return 150; + else if( rpm > 470 ) return -0.33 * rpm + 316.6; + else if( rpm > 430 ) return -0.25 * rpm + 277.5; + else if( rpm > 390 ) return -0.55 * rpm + 406.5; + else return 192.f;*/ + + // cavern opt H + /*if( rpm > 510 ) return 150; + else if( rpm > 398 ) return 75096 * pow( rpm, -0.998 ); + return 195.f;*/ + + return std::min( 200.f, std::max( 50.f, 309.25f - ( 0.2875f * rpm ) ) ); +}// so the second number of 306.25 | safe 286.25 is definetly safe | can change it to 309.25 + +float adas_api::AxleControlFrame::torque_rpm_limit( const float rpm ) const +{ + if( _plaidMode ) return _plaid_mode( rpm ); + + if( rpm > 700 ) + return 50.f; + else if( rpm > 600 ) + return 85.f; + else if( rpm > 500 ) + return 100.f; + else if( rpm > 400 ) + return 120.f; + else if( rpm > 300 ) + return 150.f; + + return 200.f; +} + adas_api::AxleControlFrame::AxleControlFrame( const adas_api::AxleTorqueFrame &torqueFrame, const adas_api::BrakeFrame &brakeFrame, const adas_api::WheelSpeedsFrame &speedsFrame, diff --git a/src/example_threaded.cpp b/src/example_threaded.cpp new file mode 100644 index 0000000..ef9b5b5 --- /dev/null +++ b/src/example_threaded.cpp @@ -0,0 +1,114 @@ +#include +#include +#include +#include +#include + +#include +#include +#include +#include +#include + +#include "simple_adas.hpp" + +int main( int argc, char* argv[] ) +{ + adas_api::SimpleAdas adas { argc >= 2 ? argv[1] : "can0" }; // was "vcan0" + + bool running = true; + std::mutex adasMutex; + std::thread processThread( [&]() + { + while( running ) + { + adasMutex.lock(); + adas.read(); + adas.write(); + adasMutex.unlock(); + std::this_thread::sleep_for( std::chrono::milliseconds(5) ); + } + } ); + + enum States + { + IDLE, + START, + LEFT, + RIGHT, + CENTER, + INCREMENT, + DONE + } state = IDLE; + + using clock = std::chrono::steady_clock; + clock::time_point timer; + + const auto timeout = std::chrono::seconds( 3 ); + + int count = 0; + + while( true ) + { + States prev = state; + + if( !adas.go() ) state = IDLE; + + switch( state ) + { + case IDLE: + if( adas.go() ) + state = START; + break; + + case START: + adas.set_rpm( 100 ); + adas.set_brakes( 0 ); + state = LEFT; + break; + + case LEFT: + if( clock::now() - timer >= timeout ) + state = RIGHT; + adas.set_angle( 20.f ); + break; + + case RIGHT: + if( clock::now() - timer >= timeout ) + state = CENTER; + adas.set_angle( -20.f ); + break; + + case CENTER: + if( clock::now() - timer >= timeout ) + state = INCREMENT; + adas.set_angle( 0.f ); + break; + + case INCREMENT: + count += 1; + if( count < 20 ) + state = IDLE; + else + state = DONE; + break; + + case DONE: + adas.finish(); + break; + } + + if( state != prev ) + timer = clock::now(); + + adas.read(); + adas.write(); + + std::this_thread::sleep_for( std::chrono::milliseconds(10) ); + } + + running = false; + processThread.join(); + + return 0; +} diff --git a/src/simple_adas.cpp b/src/simple_adas.cpp index 634da08..147526c 100644 --- a/src/simple_adas.cpp +++ b/src/simple_adas.cpp @@ -41,6 +41,12 @@ void adas_api::SimpleAdas::set_rpm( const float rpm ) this->set_rear_rpm( rpm ); } +void adas_api::SimpleAdas::set_plaid( const bool plaid ) +{ + this->frontControl.set_plaid( plaid ); + this->rearControl.set_plaid( plaid ); +} + void adas_api::SimpleAdas::_set_rpm( AxleControlFrame &axle, const float rpm ) { axle.set_speed( rpm ); @@ -157,7 +163,7 @@ void adas_api::SimpleAdas::write() switch( this->status().as_state() ) { case StatusFrame::AsState::OFF: - std::cout << "AS OFF" << std::endl; + //std::cout << "AS OFF" << std::endl; if( this->status().ami_state() >= StatusFrame::AmiState::ACCELERATION && this->status().ami_state() <= StatusFrame::AmiState::AUTONOMOUS_DEMO ) { @@ -171,32 +177,32 @@ void adas_api::SimpleAdas::write() //this->statusControl.set_estop( false ); break; case StatusFrame::AsState::READY: - std::cout << "AS READY" << std::endl; + //std::cout << "AS READY" << std::endl; this->statusControl.set_mission_status( StatusControlFrame::MissionStatus::SELECTED ); this->statusControl.set_drive( false ); //this->statusControl.set_estop( false ); break; case StatusFrame::AsState::DRIVING: - std::cout << "AS DRIVING" << std::endl; + //std::cout << "AS DRIVING" << std::endl; if( this->statusControl.get_mission_status() != StatusControlFrame::MissionStatus::FINISHED ) this->statusControl.set_mission_status( StatusControlFrame::MissionStatus::SELECTED ); this->statusControl.set_drive( true ); //this->statusControl.set_estop( false ); break; case StatusFrame::AsState::FINISHED: - std::cout << "AS FINISHED" << std::endl; + //std::cout << "AS FINISHED" << std::endl; this->statusControl.set_mission_status( StatusControlFrame::MissionStatus::NOT_SELECTED ); this->statusControl.set_drive( false ); //this->statusControl.set_estop( false ); break; case StatusFrame::AsState::EMERGENCY: - std::cout << "AS EMERGENCY" << std::endl; + //std::cout << "AS EMERGENCY" << std::endl; this->statusControl.set_mission_status( StatusControlFrame::MissionStatus::NOT_SELECTED ); this->statusControl.set_drive( false ); //this->statusControl.set_estop( true ); break; default: - std::cout << "AS default" << std::endl; + //std::cout << "AS default" << std::endl; this->statusControl.set_mission_status( StatusControlFrame::MissionStatus::NOT_SELECTED ); this->statusControl.set_drive( false ); //this->statusControl.set_estop( false );