diff --git a/src/CMakeLists.txt b/src/CMakeLists.txt
index ee1fd313ca61959da16fa1b4e141ff7b504d2b27..a50bed2b854b52285ae9239a3a8b103918dc2dbe 100644
--- a/src/CMakeLists.txt
+++ b/src/CMakeLists.txt
@@ -1,6 +1,5 @@
# driver source files
SET(sources model_car_driver_base.cpp
- model_car_exceptions.cpp
model_car_actuators.cpp
model_car_ultrasounds.cpp
model_car_egomotion.cpp
@@ -8,7 +7,6 @@ SET(sources model_car_driver_base.cpp
# application header files
SET(headers ../include/model_car_driver_base.h
- ../include/model_car_exceptions.h
../include/model_car_protocol.h
../include/model_car_actuators.h
../include/model_car_ultrasounds.h
diff --git a/src/examples/CMakeLists.txt b/src/examples/CMakeLists.txt
index 0fc08b468507bbdcce7a7154fa40844a82d4bc4d..75133f2ff7bc511b1b3eef70b22139e59345f5d9 100644
--- a/src/examples/CMakeLists.txt
+++ b/src/examples/CMakeLists.txt
@@ -1,7 +1,3 @@
-SET(name model_car_drivers_test)
-ADD_EXECUTABLE(${name} ${name}.cpp)
-TARGET_LINK_LIBRARIES(${name} ${PROJECT_NAME})
-
SET(name model_car_ultrasounds_test)
ADD_EXECUTABLE(${name} ${name}.cpp)
TARGET_LINK_LIBRARIES(${name} ${PROJECT_NAME})
diff --git a/src/examples/model_car_actuators_test.cpp b/src/examples/model_car_actuators_test.cpp
index ab72ed7bce37ac350e0e0103bd6a74c4734ea95e..45590722a31c7d432e368a7ddb54be37abc9f9e2 100644
--- a/src/examples/model_car_actuators_test.cpp
+++ b/src/examples/model_car_actuators_test.cpp
@@ -1,117 +1,39 @@
#include "model_car_actuators.h"
-
-
+#include <iostream>
+#include <math.h>
int main(int argc, char *argv[])
{
std::string name = "model_car_driver_actuators";
CModelCarActuators *act_driver;
+ double vx,steer;
+
try
{
- std::cout << "model_car_actuators_test::main: Creating object..." << std::endl;
act_driver = new CModelCarActuators(name);
- std::cout << "model_car_actuators_test::main: Object created" << std::endl;
act_driver->open();
- bool test_steering = true;
- bool test_speed = true;
- bool test_watchdog = false;
- bool test_emergency_stop = false;
- bool test_enable_disable_uss = false;
+ act_driver->disable_ultrasounds();
- float t=500e3;
+ std::cout << "model_car_actuators_test::main: about to test speed, in 3..." << std::endl;
+ sleep(1);
+ std::cout << "model_car_actuators_test::main: about to test speed, 2..." << std::endl;
+ sleep(1);
+ std::cout << "model_car_actuators_test::main: about to test speed, 1..." << std::endl;
+ sleep(1);
- if(test_steering)
+ std::cout << "model_car_actuators_test::main: sending speed 10" << std::endl;
+ for(unsigned int i=0;i<100;i++)
{
- std::cout << "model_car_actuators_test::main: about to test steering, in 3..." << std::endl;
- sleep(1);
- std::cout << "model_car_actuators_test::main: about to test steering, 2..." << std::endl;
- sleep(1);
- std::cout << "model_car_actuators_test::main: about to test steering, 1..." << std::endl;
- sleep(1);
- 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)
- {
- float v=10;
- std::cout << "model_car_actuators_test::main: about to test speed, in 3..." << std::endl;
- sleep(1);
- std::cout << "model_car_actuators_test::main: about to test speed, 2..." << std::endl;
- sleep(1);
- std::cout << "model_car_actuators_test::main: about to test speed, 1..." << std::endl;
- sleep(1);
-
- 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);
+ usleep(100000);
+ vx=20.0*sin((2.0*3.14159*i)/100.0);
+ steer=50.0*sin((2.0*3.14159*i)/100.0);
+ std::cout << vx << "," << steer << std::endl;
+ act_driver->send_command(vx,steer);
}
+ act_driver->send_command(0.0,0.0);
+ /*
if(test_watchdog)
{
std::cout << "model_car_actuators_test::main: sending watchdog" << std::endl;
@@ -131,12 +53,8 @@ int main(int argc, char *argv[])
std::cout << "model_car_actuators_test::main: sending disable uss" << std::endl;
act_driver->send_disable_uss();
}
-
- std::cout << "model_car_actuators_test::main: END OF TESTING" << std::endl;
-
- std::cout << "model_car_actuators_test::main: stopping act_driver" << std::endl;
+ */
act_driver->close();
- std::cout << "model_car_actuators_test::main: act_driver stopped" << std::endl;
}
catch (CException &e)
{
diff --git a/src/examples/model_car_batteries_test.cpp b/src/examples/model_car_batteries_test.cpp
index 426fe5dd58d22bc995a7ff491bf3e7de04e6b286..7f2892b31ed391ffcd4a19d23c9b2bfc854ee955 100644
--- a/src/examples/model_car_batteries_test.cpp
+++ b/src/examples/model_car_batteries_test.cpp
@@ -1,4 +1,5 @@
#include "model_car_batteries.h"
+#include <iostream>
int main(int argc, char *argv[])
{
@@ -8,43 +9,26 @@ int main(int argc, char *argv[])
CModelCarBatteries *bat_driver;
try
{
- std::cout << "model_car_drivers_test_bat::main: Creating object..." << std::endl;
bat_driver = new CModelCarBatteries(name);
- std::cout << "model_car_drivers_test_bat::main: Object created" << std::endl;
bat_driver->open();
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++)
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 << std::endl;
- if(bat_driver->get_bat(ID_ARD_SENS_VOLT_ACTUATOR_CELL1, value))
- 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 << std::endl;
- if(bat_driver->get_bat(ID_ARD_SENS_VOLT_SENSORS, value))
- 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 << std::endl;
- if(bat_driver->get_bat(ID_ARD_SENS_VOLT_SENSORS_CELL2, value))
- 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 << std::endl;
- if(bat_driver->get_bat(ID_ARD_SENS_VOLT_SENSORS_CELL4, value))
- 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 << std::endl;
- if(bat_driver->get_bat(ID_ARD_SENS_VOLT_SENSORS_CELL6, value))
- std::cout << " ID_ARD_SENS_VOLT_SENSORS_CELL6: " << value << std::endl;
- std::cout <<" ---" <<std::endl;
+ event_server->wait_all(events,2000);
+ std::cout << " motors battery voltage: " << bat_driver->get_motors_battery_voltage() << std::endl;
+ std::cout << " motors battery cell1 voltage: " << bat_driver->get_motors_cell1_voltage() << std::endl;
+ std::cout << " motors battery cell2 voltage: " << bat_driver->get_motors_cell2_voltage() << std::endl;
+ std::cout << " payload battery voltage: " << bat_driver->get_payload_battery_voltage() << std::endl;
+ std::cout << " payload battery cell1 voltage: " << bat_driver->get_payload_cell1_voltage() << std::endl;
+ std::cout << " payload battery cell2 voltage: " << bat_driver->get_payload_cell2_voltage() << std::endl;
+ std::cout << " payload battery cell3 voltage: " << bat_driver->get_payload_cell3_voltage() << std::endl;
+ std::cout << " payload battery cell4 voltage: " << bat_driver->get_payload_cell4_voltage() << std::endl;
+ std::cout << " payload battery cell5 voltage: " << bat_driver->get_payload_cell5_voltage() << std::endl;
+ std::cout << " payload battery cell6 voltage: " << bat_driver->get_payload_cell6_voltage() << std::endl;
+ std::cout << std::endl;
}
- std::cout << "model_car_drivers_test_bat::main: stopping bat_driver" << std::endl;
bat_driver->close();
- std::cout << "model_car_drivers_test_bat::main: bat_driver stopped" << std::endl;
}
catch (CException &e)
{
diff --git a/src/examples/model_car_egomotion_test.cpp b/src/examples/model_car_egomotion_test.cpp
index 2b38c233852175c29f546e507bd6d322ccc20cc9..0a2ca943dff1fa4aeb2def6dcf3e98f0587e542b 100644
--- a/src/examples/model_car_egomotion_test.cpp
+++ b/src/examples/model_car_egomotion_test.cpp
@@ -1,6 +1,5 @@
#include "model_car_egomotion.h"
-
-
+#include <iostream>
int main(int argc, char *argv[])
{
@@ -8,61 +7,47 @@ int main(int argc, char *argv[])
std::list<std::string> events;
std::string name = "model_car_driver_egomotion";
CModelCarEgomotion *egomotion_driver;
+ double ax,ay,az,gx,gy,gz,mx,my,mz;
+ unsigned int timestamp;
+ int tach;
+ bool dir;
+
try
{
- std::cout << "model_car_drivers_test_egomotion::main: Creating object..." << std::endl;
egomotion_driver = new CModelCarEgomotion(name);
- std::cout << "model_car_drivers_test_egomotion::main: Object created" << std::endl;
egomotion_driver->open();
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<1000; i++)
while(true)
{
try
{
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;
- }
-
- 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_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;
- }
-
- std::cout <<" ---" <<std::endl;
- usleep(1e4);
+ egomotion_driver->get_left_wheel(tach,dir,timestamp);
+ std::cout << " Left Wheel: " << std::endl;
+ std::cout << " tachometer: " << tach << std::endl;
+ std::cout << " direction: " << dir << std::endl;
+ std::cout << " timestamp: " << timestamp << std::endl;
+ egomotion_driver->get_right_wheel(tach,dir,timestamp);
+ std::cout << " Right Wheel: " << std::endl;
+ std::cout << " tachometer: " << tach << std::endl;
+ std::cout << " direction: " << dir << std::endl;
+ std::cout << " timestamp: " << timestamp << std::endl;
+ egomotion_driver->get_imu_accelerometer(ax,ay,az,timestamp);
+ std::cout << " IMU: " << std::endl;
+ std::cout << " ax: " << ax << std::endl;
+ std::cout << " ay: " << ay << std::endl;
+ std::cout << " az: " << az << std::endl;
+ egomotion_driver->get_imu_gyroscope(gx,gy,gz,timestamp);
+ std::cout << " gx: " << gx << std::endl;
+ std::cout << " gy: " << gy << std::endl;
+ std::cout << " gz: " << gz << std::endl;
+ egomotion_driver->get_imu_magnetometer(mx,my,mz,timestamp);
+ std::cout << " mx: " << mx << std::endl;
+ std::cout << " my: " << my << std::endl;
+ std::cout << " mz: " << mz << std::endl;
+ std::cout << " timestamp: " << timestamp << std::endl;
+ std::cout << std::endl;
}
catch(CEventTimeoutException &e)
{
diff --git a/src/examples/model_car_ultrasounds_test.cpp b/src/examples/model_car_ultrasounds_test.cpp
index 3f8fb29fef23ffc8916d6fdd18b1b0199aae0978..9816451a7ed08c2cb322c657064b5a592daee478 100644
--- a/src/examples/model_car_ultrasounds_test.cpp
+++ b/src/examples/model_car_ultrasounds_test.cpp
@@ -1,6 +1,5 @@
#include "model_car_ultrasounds.h"
-
-
+#include <iostream>
int main(int argc, char *argv[])
{
@@ -10,34 +9,22 @@ int main(int argc, char *argv[])
CModelCarUltrasounds *uss_driver;
try
{
- std::cout << "model_car_drivers_test_uss::main: Creating object..." << std::endl;
uss_driver = new CModelCarUltrasounds(name);
- std::cout << "model_car_drivers_test_uss::main: Object created" << std::endl;
uss_driver->open();
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<1000; i++)
while(true)
{
event_server->wait_all(events,1000);
- float us_value;
- if(uss_driver->get_uss(ID_ARD_SENS_US_SIDE_RIGHT, us_value))
- std::cout << " ID_ARD_SENS_US_SIDE_RIGHT: " << us_value << std::endl;
- if(uss_driver->get_uss(ID_ARD_SENS_US_REAR_CENTER_RIGHT, us_value))
- std::cout << " ID_ARD_SENS_US_REAR_CENTER_RIGHT: " << us_value << std::endl;
- if(uss_driver->get_uss(ID_ARD_SENS_US_REAR_CENTER, us_value))
- std::cout << " ID_ARD_SENS_US_REAR_CENTER: " << us_value << std::endl;
- if(uss_driver->get_uss(ID_ARD_SENS_US_REAR_CENTER_LEFT, us_value))
- std::cout << " ID_ARD_SENS_US_REAR_CENTER_LEFT: " << us_value << std::endl;
- if(uss_driver->get_uss(ID_ARD_SENS_US_SIDE_LEFT, us_value))
- std::cout << " ID_ARD_SENS_US_SIDE_LEFT: " << us_value << std::endl;
- std::cout <<" ---" <<std::endl;
+ std::cout << " Side right ultrasound distance: " << uss_driver->get_side_right_distance() << std::endl;
+ std::cout << " Side left ultrasound distance: " << uss_driver->get_side_left_distance() << std::endl;
+ std::cout << " Rear right ultrasound distance: " << uss_driver->get_rear_right_distance() << std::endl;
+ std::cout << " Rear center ultrasound distance: " << uss_driver->get_rear_center_distance() << std::endl;
+ std::cout << " Rear left ultrasound distance: " << uss_driver->get_rear_left_distance() << std::endl;
+ std::cout << std::endl;
}
- std::cout << "model_car_drivers_test_uss::main: stopping uss_driver" << std::endl;
uss_driver->close();
- std::cout << "model_car_drivers_test_uss::main: uss_driver stopped" << std::endl;
}
catch (CException &e)
{