diff --git a/include/model_car_actuators.h b/include/model_car_actuators.h index 49b48492a0607f343d10e30c4d6c20ac2957a06c..47378aceb5ddefcf54b9ec2861104e6bd2c3a70c 100644 --- a/include/model_car_actuators.h +++ b/include/model_car_actuators.h @@ -1,19 +1,7 @@ #ifndef _MODEL_CAR_ACTUATORS_H #define _MODEL_CAR_ACTUATORS_H -#include "rs232.h" -#include "commexceptions.h" -#include "threadserver.h" #include "model_car_driver_base.h" -#include "model_car_exceptions.h" -#include "model_car_protocol.h" -#include <iostream> -#include <iomanip> -#include <vector> -#include <dirent.h> -#include <chrono> -#include <thread> -#include <map> /** * @class CModelCarActuators @@ -24,18 +12,17 @@ class CModelCarActuators: public CModelCarDriverBase { private: - void process_data_frame(uint8_t id,uint32_t timestamp, TDataUnion data_union); + void process_data_frame(uint8_t id, TDataUnion data_union); protected: - + void send_watchdog(void); + public: CModelCarActuators(std::string name); - void send_speed(float speed); - void send_steering(float angle); - void send_watchdog(); - void send_emergency_stop(); - void send_enable_uss(); - void send_disable_uss(); + void send_command(double speed,double steering); + void emergency_stop(void); + void enable_ultrasounds(void); + void disable_ultrasounds(void); ~CModelCarActuators(void); }; diff --git a/src/model_car_actuators.cpp b/src/model_car_actuators.cpp index 0cdebeba6787ca9deedc2572e45aa187ff57ba86..27a16eb477ef096101441b2617865765e160761e 100644 --- a/src/model_car_actuators.cpp +++ b/src/model_car_actuators.cpp @@ -2,89 +2,68 @@ CModelCarActuators::CModelCarActuators(std::string name) : CModelCarDriverBase(name, ARDUINO_CENTER_ACTUATORS) { - + this->set_rx_timeout(NO_RX_TIMEOUT); } -void CModelCarActuators::process_data_frame(uint8_t id,uint32_t timestamp, TDataUnion data_union) +void CModelCarActuators::process_data_frame(uint8_t id, TDataUnion data_union) { - //std::cout << "CModelCarActuators::process_data_frame: id=" << unsigned(id) << std::endl; +} - //const SENSOR_ID currentSensor = static_cast<SENSOR_ID>(id); - //TODO nothing? +void CModelCarActuators::send_watchdog(void) +{ + uint8_t cmd=1; + this->send_request(ID_ARD_ACT_WATCHDOG,1,&cmd); } -void CModelCarActuators::send_speed(float speed) +void CModelCarActuators::send_command(double speed,double steering) { - uint8_t id = ID_ARD_ACT_SPEED_CONTR; - uint8_t data_length=1; + uint8_t cmd; + + this->send_watchdog(); + // remap speed speed = speed > 100.f ? 100.f : speed; speed = speed < -100.f ? -100.f : speed; speed += 100.f; speed *= 0.9f; // 0...180 + cmd=(uint8_t)speed; - uint8_t value = (uint8_t)speed; - - this->send_request(id, data_length, &value); -} + this->send_request(ID_ARD_ACT_SPEED_CONTR,1, &cmd); -void CModelCarActuators::send_steering(float angle) -{ - uint8_t id = ID_ARD_ACT_STEER_SERVO; - uint8_t data_length=1; - // remap angle - angle = angle > 100.f ? 100.f : angle; - angle = angle < -100.f ? -100.f : angle; - angle += 100.f; - angle *= 0.9f; + // remap steering + steering = steering > 100.f ? 100.f : steering; + steering = steering < -100.f ? -100.f : steering; + steering += 100.f; + steering *= 0.9f; // 0...180 + cmd=(uint8_t)steering; - uint8_t value = (uint8_t)angle; - - this->send_request(id, data_length, &value); + this->send_request(ID_ARD_ACT_STEER_SERVO,1, &cmd); } -void CModelCarActuators::send_watchdog() +void CModelCarActuators::emergency_stop(void) { - //do this every half a second or the motors will stop - uint8_t id = ID_ARD_ACT_WATCHDOG; - uint8_t data_length=1; - uint8_t value=1; + uint8_t cmd=1; - this->send_request(id, data_length, &value); + this->send_request(ID_ARD_ACT_EMERGENCY_STOP,1,&cmd); } -void CModelCarActuators::send_emergency_stop() +void CModelCarActuators::enable_ultrasounds(void) { - //do this every half a second or the motors will stop - uint8_t id = ID_ARD_ACT_EMERGENCY_STOP; - uint8_t data_length=1; - uint8_t value=1; + uint8_t cmd=0; - this->send_request(id, data_length, &value); + this->send_request(ID_ARD_ENABLE_USS,1,&cmd); } -void CModelCarActuators::send_enable_uss() +void CModelCarActuators::disable_ultrasounds(void) { - //do this every half a second or the motors will stop - uint8_t id = ID_ARD_ENABLE_USS; - uint8_t data_length=0; - uint8_t value=0; + uint8_t cmd=0; - this->send_request(id, data_length, &value); + this->send_request(ID_ARD_DISABLE_USS,1,&cmd); } -void CModelCarActuators::send_disable_uss() -{ - //do this every half a second or the motors will stop - uint8_t id = ID_ARD_DISABLE_USS; - uint8_t data_length=0; - uint8_t value=0; - - this->send_request(id, data_length, &value); -} CModelCarActuators::~CModelCarActuators() {