diff --git a/include/adas.hpp b/include/adas.hpp index 39b3d5c..d334ebc 100644 --- a/include/adas.hpp +++ b/include/adas.hpp @@ -33,9 +33,9 @@ namespace adas_api{ WheelSpeedsFrame wheelSpeeds; // outgoing frames - AxleControlFrame frontControl { this->frontAxle, this->brakes, this->wheelSpeeds, this->status }; - AxleControlFrame rearControl { this->rearAxle, this->brakes, this->wheelSpeeds, this->status, false }; BrakeControlFrame brakeControl; + AxleControlFrame frontControl { this->frontAxle, this->brakes, this->wheelSpeeds, this->status, this->brakeControl }; + AxleControlFrame rearControl { this->rearAxle, this->brakes, this->wheelSpeeds, this->status, this->brakeControl, false }; StatusControlFrame statusControl { this->frontAxle, this->rearAxle, this->status }; SteeringControlFrame steeringControl { this->steering, this->status }; diff --git a/include/control/axle_control_frame.hpp b/include/control/axle_control_frame.hpp index 211567c..0f83adf 100644 --- a/include/control/axle_control_frame.hpp +++ b/include/control/axle_control_frame.hpp @@ -21,6 +21,7 @@ namespace adas_api{ const BrakeFrame &brakeFrame, const WheelSpeedsFrame &speedsFrame, const StatusFrame &statusFrame, + const BrakeControlFrame &brakeControl, const bool front=true ); virtual ~AxleControlFrame() = default; @@ -62,6 +63,7 @@ namespace adas_api{ const BrakeFrame &_brakeFrame; const WheelSpeedsFrame &_speedsFrame; const StatusFrame &_statusFrame; + const BrakeControlFrame &_brakeControl; const bool _front; float _speed = 0.f; diff --git a/include/control/brake_control_frame.hpp b/include/control/brake_control_frame.hpp index d7efdc3..f903189 100644 --- a/include/control/brake_control_frame.hpp +++ b/include/control/brake_control_frame.hpp @@ -16,6 +16,7 @@ namespace adas_api{ const float get_front() const; const float get_rear() const; + const bool on() const; private: /** diff --git a/include/control/status_control_frame.hpp b/include/control/status_control_frame.hpp index 4b080da..e6348d9 100644 --- a/include/control/status_control_frame.hpp +++ b/include/control/status_control_frame.hpp @@ -36,6 +36,11 @@ namespace adas_api{ _mission_status = status; } + const MissionStatus get_mission_status() const + { + return this->_mission_status; + } + private: /** * @brief Generate can_frame for brake diff --git a/src/control/axle_control_frame.cpp b/src/control/axle_control_frame.cpp index e7ae893..c147322 100644 --- a/src/control/axle_control_frame.cpp +++ b/src/control/axle_control_frame.cpp @@ -32,7 +32,8 @@ can_frame adas_api::AxleControlFrame::_process() const float torque = 0.f; /* check that we have up to date information otherwise set safe values */ - if( this->_statusFrame.is_recent() && + if( this->_brakeControl.on() == false && + this->_statusFrame.is_recent() && this->_statusFrame.as_state() == adas_api::StatusFrame::AsState::DRIVING && // in driving mode this->_brakeFrame.is_recent() && this->_brakeFrame.on() == false && // brakes are off @@ -103,12 +104,14 @@ adas_api::AxleControlFrame::AxleControlFrame( const adas_api::AxleTorqueFrame &t const adas_api::BrakeFrame &brakeFrame, const adas_api::WheelSpeedsFrame &speedsFrame, const adas_api::StatusFrame &statusFrame, + const adas_api::BrakeControlFrame &brakeControl, const bool front ) : ControlFrame( front ? CANDATA_AI2_VCU_DRIVE_F_FRAME_ID : CANDATA_AI2_VCU_DRIVE_R_FRAME_ID ), _torqueFrame( torqueFrame ), _brakeFrame( brakeFrame ), _speedsFrame( speedsFrame ), _statusFrame( statusFrame ), + _brakeControl( brakeControl ), _front( front ) { if( front != torqueFrame.is_front() ) diff --git a/src/control/brake_control_frame.cpp b/src/control/brake_control_frame.cpp index 645f777..638fd6d 100644 --- a/src/control/brake_control_frame.cpp +++ b/src/control/brake_control_frame.cpp @@ -29,6 +29,11 @@ const float adas_api::BrakeControlFrame::get_rear() const return this->_rear; } +const bool adas_api::BrakeControlFrame::on() const +{ + return this->_front > 0.f || this->_rear > 0.f; +} + can_frame adas_api::BrakeControlFrame::_process() const { float front = this->_front; diff --git a/src/example_straight_line.cpp b/src/example_straight_line.cpp index cfa2700..5b90330 100644 --- a/src/example_straight_line.cpp +++ b/src/example_straight_line.cpp @@ -27,14 +27,16 @@ int main( int argc, char* argv[] ) switch( state ) { case IDLE: + std::cout << "IDLE" << std::endl; adas.set_rpm( 0 ); // don't move - adas.set_brakes( 100 ); + adas.set_brakes( 0 ); if( adas.go() ) // wait for mission to start state = SAVE_START; break; case SAVE_START: + std::cout << "SAVE_START" << std::endl; for( int i=0; i<4; ++i ) // save starting wheel counts startingCount[i] = adas.wheel_counts().count( i ); @@ -42,15 +44,21 @@ int main( int argc, char* argv[] ) break; case ACCELERATE: - adas.set_rpm( 10 ); // move + std::cout << "ACCELERATE" << std::endl; + adas.set_rpm( 250 ); // move adas.set_brakes( 0 ); for( int i=0; i<4; ++i ) // check distance traveled - if( std::abs( adas.wheel_counts().count( i ) - startingCount[i] ) >= 100 ) + { + std::cout << adas.wheel_counts().count( i ) - startingCount[i] << " "; + if( std::abs( adas.wheel_counts().count( i ) - startingCount[i] ) >= 5000 ) state = DECELERATE; // start stopping when distance is reached + } + std::cout << std::endl; break; case DECELERATE: + std::cout << "DECELERATE" << std::endl; adas.set_rpm( 0 ); // stop adas.set_brakes( 100 ); @@ -59,6 +67,7 @@ int main( int argc, char* argv[] ) break; case DONE: + std::cout << "DONE" << std::endl; adas.finish(); // car in safe state break; } diff --git a/src/simple_adas.cpp b/src/simple_adas.cpp index 05e98b7..103034a 100644 --- a/src/simple_adas.cpp +++ b/src/simple_adas.cpp @@ -7,11 +7,13 @@ void adas_api::SimpleAdas::set_front_rpm( const float rpm ) { this->frontControl.set_speed( rpm ); + this->frontControl.set_torque( 200 ); } void adas_api::SimpleAdas::set_rear_rpm( const float rpm ) { this->rearControl.set_speed( rpm ); + this->rearControl.set_torque( 200 ); } void adas_api::SimpleAdas::set_rpm( const float rpm ) @@ -130,72 +132,53 @@ void adas_api::SimpleAdas::write() { case StatusFrame::AsState::OFF: std::cout << "AS OFF" << std::endl; + if( this->status().ami_state() >= StatusFrame::AmiState::ACCELERATION && + this->status().ami_state() <= StatusFrame::AmiState::AUTONOMOUS_DEMO ) + { + this->statusControl.set_mission_status( StatusControlFrame::MissionStatus::SELECTED ); + } + else + { + this->statusControl.set_mission_status( StatusControlFrame::MissionStatus::NOT_SELECTED ); + } + this->statusControl.set_drive( false ); + this->statusControl.set_estop( false ); break; case StatusFrame::AsState::READY: 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; + 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; + 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; + 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; - break; - } - - /* mission status */ - switch( this->status().as_state() ) - { - case StatusFrame::AsState::OFF: - [[fallthrough]]; - case StatusFrame::AsState::READY: - [[fallthrough]]; - case StatusFrame::AsState::DRIVING: - this->statusControl.set_mission_status( this->status().ami_state() == StatusFrame::AmiState::NOT_SELECTED ? - StatusControlFrame::MissionStatus::NOT_SELECTED : - StatusControlFrame::MissionStatus::SELECTED ); - // possible bug here regarding getting the vehicle into finished state - break; - - case StatusFrame::AsState::FINISHED: - this->statusControl.set_mission_status( StatusControlFrame::MissionStatus::FINISHED ); - break; - - case StatusFrame::AsState::EMERGENCY: - [[fallthrough]]; - default: - this->statusControl.set_mission_status( StatusControlFrame::MissionStatus::FINISHED ); - break; - } - - switch( this->status().as_state() ) - { - case StatusFrame::AsState::OFF: - [[fallthrough]]; - case StatusFrame::AsState::READY: - [[fallthrough]]; - case StatusFrame::AsState::FINISHED: + this->statusControl.set_mission_status( StatusControlFrame::MissionStatus::NOT_SELECTED ); this->statusControl.set_drive( false ); this->statusControl.set_estop( false ); break; - - case StatusFrame::AsState::DRIVING: - this->statusControl.set_drive( this->status().go() ); - this->statusControl.set_estop( false ); - break; - - case StatusFrame::AsState::EMERGENCY: - [[fallthrough]]; - default: - this->statusControl.set_drive( false ); - this->statusControl.set_estop( true ); - break; } + std::cout << "AMI state: " << this->status().ami_state() << std::endl; + Adas::write(); } \ No newline at end of file