Testing updates

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>

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;

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;

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;

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;

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)

src/model_car_driver_base.cpp

 #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;
 }