diff --git a/include/model_car_driver_base.h b/include/model_car_driver_base.h
index e0dd5a3df0cdf96150fbd9cca26cd1a4c3e6a91c..0ff074a285e02124d1503fdbe49ba855b66765da 100644
--- a/include/model_car_driver_base.h
+++ b/include/model_car_driver_base.h
@@ -5,6 +5,7 @@
#include "commexceptions.h"
#include "threadserver.h"
// #include "eventserver.h"
+#include "eventexceptions.h"
#include "model_car_exceptions.h"
#include "model_car_protocol.h"
#include <iostream>
diff --git a/src/examples/model_car_actuators_test.cpp b/src/examples/model_car_actuators_test.cpp
index ef43b865c4f47a440abfd30e6bab47d65ee1a12f..ab72ed7bce37ac350e0e0103bd6a74c4734ea95e 100644
--- a/src/examples/model_car_actuators_test.cpp
+++ b/src/examples/model_car_actuators_test.cpp
@@ -4,98 +4,143 @@
int main(int argc, char *argv[])
{
- std::string name = "model_car_driver_act";
+ std::string name = "model_car_driver_actuators";
CModelCarActuators *act_driver;
try
{
- std::cout << "model_car_drivers_test_act::main: Creating object..." << std::endl;
+ std::cout << "model_car_actuators_test::main: Creating object..." << std::endl;
act_driver = new CModelCarActuators(name);
- std::cout << "model_car_drivers_test_act::main: Object created" << std::endl;
+ std::cout << "model_car_actuators_test::main: Object created" << std::endl;
act_driver->open();
bool test_steering = true;
- bool test_speed = false;
- bool test_watchdog = true;
- bool test_emergency_stop = true;
- bool test_enable_disable_uss = true;
+ bool test_speed = true;
+ bool test_watchdog = false;
+ bool test_emergency_stop = false;
+ bool test_enable_disable_uss = false;
+
+ float t=500e3;
if(test_steering)
{
- std::cout << "model_car_drivers_test_act::main: about to test steering, in 3..." << std::endl;
+ std::cout << "model_car_actuators_test::main: about to test steering, in 3..." << std::endl;
sleep(1);
- std::cout << "model_car_drivers_test_act::main: about to test steering, 2..." << std::endl;
+ std::cout << "model_car_actuators_test::main: about to test steering, 2..." << std::endl;
sleep(1);
- std::cout << "model_car_drivers_test_act::main: about to test steering, 1..." << std::endl;
+ std::cout << "model_car_actuators_test::main: about to test steering, 1..." << std::endl;
sleep(1);
- std::cout << "model_car_drivers_test_act::main: sending steering 0" << std::endl;
- act_driver->send_steering(0.0); //center
- usleep(500e3);
- std::cout << "model_car_drivers_test_act::main: sending steering max left" << std::endl;
- act_driver->send_steering(100.0); //max left (30º)
- usleep(500e3);
- std::cout << "model_car_drivers_test_act::main: sending steering 0" << std::endl;
- act_driver->send_steering(0.0); //center
- usleep(500e3);
- std::cout << "model_car_drivers_test_act::main: sending steering max right" << std::endl;
- act_driver->send_steering(-100.0); //max right (30º)
- usleep(500e3);
- std::cout << "model_car_drivers_test_act::main: sending steering 0" << std::endl;
- act_driver->send_steering(0.0); //center
+ std::cout << "model_car_actuators_test::main: sending steering 0" << std::endl;
+
+ act_driver->send_watchdog();
+ act_driver->send_speed(0.0); //stop
+ act_driver->send_steering(0.0);
+
+ for(int i=0; i<100; i+=10)
+ {
+ act_driver->send_watchdog();
+ act_driver->send_steering(i);
+ std::cout << "model_car_actuators_test::main: sending steering "<< i << std::endl;
+ usleep(t);
+ }
+ for(int i=100; i>=-100; i-=10)
+ {
+ act_driver->send_watchdog();
+ act_driver->send_steering(i);
+ std::cout << "model_car_actuators_test::main: sending steering "<< i << std::endl;
+ usleep(t);
+ }
+ for(int i=-100; i<=0; i+=10)
+ {
+ act_driver->send_watchdog();
+ act_driver->send_steering(i);
+ std::cout << "model_car_actuators_test::main: sending steering "<< i << std::endl;
+ usleep(t);
+ }
+
+ act_driver->send_watchdog();
+ std::cout << "model_car_actuators_test::main: sending steering 0"<< std::endl;
+ act_driver->send_steering(0.0);
}
if(test_speed)
{
- std::cout << "model_car_drivers_test_act::main: about to test speed, in 3..." << std::endl;
+ float v=10;
+ std::cout << "model_car_actuators_test::main: about to test speed, in 3..." << std::endl;
sleep(1);
- std::cout << "model_car_drivers_test_act::main: about to test speed, 2..." << std::endl;
+ std::cout << "model_car_actuators_test::main: about to test speed, 2..." << std::endl;
sleep(1);
- std::cout << "model_car_drivers_test_act::main: about to test speed, 1..." << std::endl;
+ std::cout << "model_car_actuators_test::main: about to test speed, 1..." << std::endl;
sleep(1);
- std::cout << "model_car_drivers_test_act::main: sending speed 0" << std::endl;
- act_driver->send_speed(0.0); //stop
- usleep(500e3);
- std::cout << "model_car_drivers_test_act::main: sending speed 10%" << std::endl;
- act_driver->send_speed(10.0); //10% of full gas
- usleep(500e3);
- std::cout << "model_car_drivers_test_act::main: sending speed 0" << std::endl;
- act_driver->send_speed(0.0); //stop
- usleep(500e3);
- std::cout << "model_car_drivers_test_act::main: sending speed -10%" << std::endl;
- act_driver->send_speed(-10.0); //10% of reverse
- usleep(500e3);
- std::cout << "model_car_drivers_test_act::main: sending speed 0" << std::endl;
- act_driver->send_speed(0.0); //stop
+
+ v=0;
+ std::cout << "model_car_actuators_test::main: sending speed "<< v << std::endl;
+ act_driver->send_watchdog();
+ act_driver->send_speed(v);
+ act_driver->send_steering(0.0); //center
+ usleep(t);
+ v=10;
+ std::cout << "model_car_actuators_test::main: sending speed " << v << std::endl;
+ act_driver->send_speed(v);
+ act_driver->send_watchdog();
+ usleep(t);
+ act_driver->send_watchdog();
+ usleep(t);
+ act_driver->send_watchdog();
+ usleep(t);
+ act_driver->send_watchdog();
+ usleep(t);
+ act_driver->send_watchdog();
+ usleep(t);
+ act_driver->send_watchdog();
+ v=0;
+ std::cout << "model_car_actuators_test::main: sending speed " << v << std::endl;
+ act_driver->send_watchdog();
+ act_driver->send_speed(v);
+ usleep(t);
+ v=-10;
+ std::cout << "model_car_actuators_test::main: sending speed " << v << std::endl;
+ act_driver->send_watchdog();
+ act_driver->send_speed(v);
+ usleep(t);
+ act_driver->send_watchdog();
+ usleep(t);
+ act_driver->send_watchdog();
+ usleep(t);
+ v=0;
+ std::cout << "model_car_actuators_test::main: sending speed "<< v << std::endl;
+ act_driver->send_watchdog();
+ act_driver->send_speed(v);
}
if(test_watchdog)
{
- std::cout << "model_car_drivers_test_act::main: sending watchdog" << std::endl;
- act_driver->send_watchdog();
+ std::cout << "model_car_actuators_test::main: sending watchdog" << std::endl;
+ act_driver->send_watchdog();
}
if(test_emergency_stop)
{
- std::cout << "model_car_drivers_test_act::main: sending emergency stop" << std::endl;
+ std::cout << "model_car_actuators_test::main: sending emergency stop" << std::endl;
act_driver->send_emergency_stop();
}
if(test_enable_disable_uss)
{
- std::cout << "model_car_drivers_test_act::main: sending enable uss" << std::endl;
+ std::cout << "model_car_actuators_test::main: sending enable uss" << std::endl;
act_driver->send_enable_uss();
- std::cout << "model_car_drivers_test_act::main: sending disable uss" << std::endl;
+ std::cout << "model_car_actuators_test::main: sending disable uss" << std::endl;
act_driver->send_disable_uss();
}
- std::cout << "model_car_drivers_test_act::main: END OF TESTING" << std::endl;
+ std::cout << "model_car_actuators_test::main: END OF TESTING" << std::endl;
- std::cout << "model_car_drivers_test_act::main: stopping act_driver" << std::endl;
+ std::cout << "model_car_actuators_test::main: stopping act_driver" << std::endl;
act_driver->close();
- std::cout << "model_car_drivers_test_act::main: act_driver stopped" << std::endl;
+ std::cout << "model_car_actuators_test::main: act_driver stopped" << std::endl;
}
catch (CException &e)
{
- std::cout << e.what() << std::endl;
+ std::cout << "model_car_actuators_test: " << e.what() << std::endl;
}
delete act_driver;
diff --git a/src/examples/model_car_batteries_test.cpp b/src/examples/model_car_batteries_test.cpp
index 24d610c5fc109627ae3d26318fac61f8afa5f281..426fe5dd58d22bc995a7ff491bf3e7de04e6b286 100644
--- a/src/examples/model_car_batteries_test.cpp
+++ b/src/examples/model_car_batteries_test.cpp
@@ -15,32 +15,31 @@ int main(int argc, char *argv[])
events.push_back(bat_driver->get_new_data_event_id());
std::cout << "model_car_drivers_test_bat::main: reading BAT values" << std::endl;
- for(unsigned int i=0; i<10; i++)
+ //for(unsigned int i=0; i<10; i++)
+ while(true)
{
event_server->wait_all(events,1000);
float value;
if(bat_driver->get_bat(ID_ARD_SENS_VOLT_ACTUATOR, value))
- std::cout << " ID_ARD_SENS_VOLT_ACTUATOR value: " << value << std::endl;
+ std::cout << " ID_ARD_SENS_VOLT_ACTUATOR: " << value << std::endl;
if(bat_driver->get_bat(ID_ARD_SENS_VOLT_ACTUATOR_CELL1, value))
- std::cout << " ID_ARD_SENS_VOLT_ACTUATOR_CELL1 value: " << value << std::endl;
+ std::cout << " ID_ARD_SENS_VOLT_ACTUATOR_CELL1: " << value << std::endl;
if(bat_driver->get_bat(ID_ARD_SENS_VOLT_ACTUATOR_CELL2, value))
- std::cout << " ID_ARD_SENS_VOLT_ACTUATOR_CELL2 value: " << value << std::endl;
- if(bat_driver->get_bat(ID_ARD_SENS_VOLT_ACTUATOR_CELL2, value))
- std::cout << " ID_ARD_SENS_VOLT_ACTUATOR_CELL2 value: " << value << std::endl;
+ std::cout << " ID_ARD_SENS_VOLT_ACTUATOR_CELL2: " << value << std::endl;
if(bat_driver->get_bat(ID_ARD_SENS_VOLT_SENSORS, value))
- std::cout << " ID_ARD_SENS_VOLT_SENSORS value: " << value << std::endl;
+ std::cout << " ID_ARD_SENS_VOLT_SENSORS: " << value << std::endl;
if(bat_driver->get_bat(ID_ARD_SENS_VOLT_SENSORS_CELL1, value))
- std::cout << " ID_ARD_SENS_VOLT_SENSORS_CELL1 value: " << value << std::endl;
+ std::cout << " ID_ARD_SENS_VOLT_SENSORS_CELL1: " << value << std::endl;
if(bat_driver->get_bat(ID_ARD_SENS_VOLT_SENSORS_CELL2, value))
- std::cout << " ID_ARD_SENS_VOLT_SENSORS_CELL2 value: " << value << std::endl;
+ std::cout << " ID_ARD_SENS_VOLT_SENSORS_CELL2: " << value << std::endl;
if(bat_driver->get_bat(ID_ARD_SENS_VOLT_SENSORS_CELL3, value))
- std::cout << " ID_ARD_SENS_VOLT_SENSORS_CELL3 value: " << value << std::endl;
+ std::cout << " ID_ARD_SENS_VOLT_SENSORS_CELL3: " << value << std::endl;
if(bat_driver->get_bat(ID_ARD_SENS_VOLT_SENSORS_CELL4, value))
- std::cout << " ID_ARD_SENS_VOLT_SENSORS_CELL4 value: " << value << std::endl;
+ std::cout << " ID_ARD_SENS_VOLT_SENSORS_CELL4: " << value << std::endl;
if(bat_driver->get_bat(ID_ARD_SENS_VOLT_SENSORS_CELL5, value))
- std::cout << " ID_ARD_SENS_VOLT_SENSORS_CELL5 value: " << value << std::endl;
+ std::cout << " ID_ARD_SENS_VOLT_SENSORS_CELL5: " << value << std::endl;
if(bat_driver->get_bat(ID_ARD_SENS_VOLT_SENSORS_CELL6, value))
- std::cout << " ID_ARD_SENS_VOLT_SENSORS_CELL6 value: " << value << std::endl;
+ std::cout << " ID_ARD_SENS_VOLT_SENSORS_CELL6: " << value << std::endl;
std::cout <<" ---" <<std::endl;
}
std::cout << "model_car_drivers_test_bat::main: stopping bat_driver" << std::endl;
diff --git a/src/examples/model_car_egomotion_test.cpp b/src/examples/model_car_egomotion_test.cpp
index dafd7a87f988d3bf7c329a20707f6a809c7805c5..2b38c233852175c29f546e507bd6d322ccc20cc9 100644
--- a/src/examples/model_car_egomotion_test.cpp
+++ b/src/examples/model_car_egomotion_test.cpp
@@ -17,49 +17,57 @@ int main(int argc, char *argv[])
events.push_back(egomotion_driver->get_new_data_event_id());
std::cout << "model_car_drivers_test_egomotion::main: reading EGOMOTION values" << std::endl;
- for(unsigned int i=0; i<10; i++)
+ //for(unsigned int i=0; i<1000; i++)
+ while(true)
{
- event_server->wait_all(events,1000);
- int lwheel_tach=0;
- bool lwheel_dir=false;
- if(egomotion_driver->get_left_wheel(lwheel_tach, lwheel_dir))
+ try
{
- std::cout << " Left wheel tachometer: " << lwheel_tach << std::endl;
- std::cout << " Left wheel direction: " << lwheel_dir << std::endl;
- }
+ event_server->wait_all(events,1000);
+ int lwheel_tach=0;
+ bool lwheel_dir=false;
+ if(egomotion_driver->get_left_wheel(lwheel_tach, lwheel_dir))
+ {
+ std::cout << " Left wheel tachometer: " << lwheel_tach << std::endl;
+ std::cout << " Left wheel direction: " << lwheel_dir << std::endl;
+ }
- int rwheel_tach=0;
- bool rwheel_dir=false;
- if(egomotion_driver->get_right_wheel(rwheel_tach,rwheel_dir))
- {
- std::cout << " Right wheel tachometer: " << rwheel_tach << std::endl;
- std::cout << " Right wheel direction: " << rwheel_dir << std::endl;
- }
+ int rwheel_tach=0;
+ bool rwheel_dir=false;
+ if(egomotion_driver->get_right_wheel(rwheel_tach,rwheel_dir))
+ {
+ std::cout << " Right wheel tachometer: " << rwheel_tach << std::endl;
+ std::cout << " Right wheel direction: " << rwheel_dir << std::endl;
+ }
- float ax=0.0 ,ay=.0 ,az=0.0 ,gx=0.0 ,gy=0.0 ,gz=0.0 ,mx=0.0 ,my=0.0 ,mz=0.0;
- if(egomotion_driver->get_imu_accel(ax,ay,az))
- {
- std::cout << " Imu ax: " << ax << std::endl;
- std::cout << " Imu ay: " << ay << std::endl;
- std::cout << " Imu az: " << az << std::endl;
- }
+ float ax=0.0 ,ay=.0 ,az=0.0 ,gx=0.0 ,gy=0.0 ,gz=0.0 ,mx=0.0 ,my=0.0 ,mz=0.0;
+ if(egomotion_driver->get_imu_accel(ax,ay,az))
+ {
+ std::cout << " Imu ax: " << ax << std::endl;
+ std::cout << " Imu ay: " << ay << std::endl;
+ std::cout << " Imu az: " << az << std::endl;
+ }
- if(egomotion_driver->get_imu_gyro(gx,gy,gz))
- {
- std::cout << " Imu gx: " << gx << std::endl;
- std::cout << " Imu gy: " << gy << std::endl;
- std::cout << " Imu gz: " << gz << std::endl;
- }
+ if(egomotion_driver->get_imu_gyro(gx,gy,gz))
+ {
+ std::cout << " Imu gx: " << gx << std::endl;
+ std::cout << " Imu gy: " << gy << std::endl;
+ std::cout << " Imu gz: " << gz << std::endl;
+ }
+
+ if(egomotion_driver->get_imu_magn(mx,my,mz))
+ {
+ std::cout << " Imu mx: " << mx << std::endl;
+ std::cout << " Imu my: " << my << std::endl;
+ std::cout << " Imu mz: " << mz << std::endl;
+ }
- if(egomotion_driver->get_imu_magn(mx,my,mz))
+ std::cout <<" ---" <<std::endl;
+ usleep(1e4);
+ }
+ catch(CEventTimeoutException &e)
{
- std::cout << " Imu mx: " << mx << std::endl;
- std::cout << " Imu my: " << my << std::endl;
- std::cout << " Imu mz: " << mz << std::endl;
+ std::cout << e.what() << std::endl;
}
-
- std::cout <<" ---" <<std::endl;
- usleep(1e4);
}
std::cout << "model_car_drivers_test_egomotion::main: stopping egomotion_driver" << std::endl;
diff --git a/src/examples/model_car_ultrasounds_test.cpp b/src/examples/model_car_ultrasounds_test.cpp
index cfb9cf71806edd63f5f71e8497ad729caa6b2708..3f8fb29fef23ffc8916d6fdd18b1b0199aae0978 100644
--- a/src/examples/model_car_ultrasounds_test.cpp
+++ b/src/examples/model_car_ultrasounds_test.cpp
@@ -17,7 +17,8 @@ int main(int argc, char *argv[])
events.push_back(uss_driver->get_new_data_event_id());
std::cout << "model_car_drivers_test_uss::main: reading USS values" << std::endl;
- for(unsigned int i=0; i<10; i++)
+ //for(unsigned int i=0; i<1000; i++)
+ while(true)
{
event_server->wait_all(events,1000);
float us_value;
diff --git a/src/model_car_batteries.cpp b/src/model_car_batteries.cpp
index f32a37762c14492c8133bc121bb28f549eefd51c..8e90f574b8e57fee0d87ef0ab456f2c845a5a23f 100644
--- a/src/model_car_batteries.cpp
+++ b/src/model_car_batteries.cpp
@@ -19,9 +19,12 @@ CModelCarBatteries::CModelCarBatteries(std::string name) : CModelCarDriverBase(n
void CModelCarBatteries::process_data_frame(uint8_t id,uint32_t timestamp, TDataUnion data_union)
{
//std::cout << "CModelCarBatteries::process_data_frame: id=" << unsigned(id) << std::endl;
-
- const SENSOR_ID currentSensor = static_cast<SENSOR_ID>(id);
+ const SENSOR_ID currentSensor = static_cast<SENSOR_ID>(id);
+
+ //m_bat_values[currentSensor]=data_union.voltage;
+
+
auto it = m_bat_values.find(currentSensor);
if(it==m_bat_values.end())
{
@@ -32,7 +35,7 @@ void CModelCarBatteries::process_data_frame(uint8_t id,uint32_t timestamp, TData
it->second = data_union.voltage;
//m_bat_values[currentSensor] = data_union.us;
}
-
+
}
bool CModelCarBatteries::get_bat(SENSOR_ID id, float & value)
diff --git a/src/model_car_driver_base.cpp b/src/model_car_driver_base.cpp
index f3c49e9c14e3098bc2f82db0348648fb08c2efe6..931eec545d7c0ea6e5ad7376eb7260eaa345e642 100644
--- a/src/model_car_driver_base.cpp
+++ b/src/model_car_driver_base.cpp
@@ -1,5 +1,5 @@
#include "model_car_driver_base.h"
-#include "eventexceptions.h"
+//#include "eventexceptions.h"
struct HexCharStruct
{
@@ -181,7 +181,8 @@ void *CModelCarDriverBase::data_thread(void *param)
try
{
driver->port_mutex.enter();
- if(driver->serial_port->get_num_data()==0)
+ num=driver->serial_port->get_num_data();
+ if(num==0)
{
driver->port_mutex.exit();
driver->event_server->wait_all(events,1000);
@@ -212,10 +213,11 @@ void *CModelCarDriverBase::data_thread(void *param)
catch(CEventTimeoutException &e)
{
//do nothing;
+ //std::cout << "CModelCarDriverBase::data_thread: " << e.what() << std::endl;
}
catch(CException &e)
{
- std::cout << e.what() << std::endl;
+ std::cout << "CModelCarDriverBase::data_thread: " << e.what() << std::endl;
}
if(driver->event_server->event_is_set(driver->finish_thread_event_id))
@@ -494,6 +496,7 @@ void CModelCarDriverBase::send_request(uint8_t id, uint8_t data_length, uint8_t
this->port_mutex.enter();
this->serial_port->write(stuffed_frame,stuffed_frame_size);
+ //cout_data(stuffed_frame,stuffed_frame_size,"stuffed_frame");
this->port_mutex.exit();
delete []stuffed_frame;
}