diff --git a/src/CMakeLists.txt b/src/CMakeLists.txt index a9586aa6e302241aa89a21882beceaf964b7ce6e..a6fbf6e1d0815b9285f6691a3d0e0c056343d09a 100644 --- a/src/CMakeLists.txt +++ b/src/CMakeLists.txt @@ -29,3 +29,4 @@ INSTALL(TARGETS bioloid_robot INSTALL(FILES ${headers} DESTINATION include/iridrivers) INSTALL(FILES ../Findbioloid_robot.cmake DESTINATION ${CMAKE_ROOT}/Modules/) ADD_SUBDIRECTORY(examples) +ADD_SUBDIRECTORY(tools) diff --git a/src/bioloid_robot.cpp b/src/bioloid_robot.cpp index 49fd95cda2c09dca36d3826cd2b8a7d5f01f5821..f7fffba0ab0d296c510e8b08cd6945064c32f769 100644 --- a/src/bioloid_robot.cpp +++ b/src/bioloid_robot.cpp @@ -707,7 +707,7 @@ double CBioloidRobot::mm_get_servo_angle(unsigned char servo_id) throw CBioloidRobotException(_HERE_,"Invalid robot device"); } -void CBioloidRobot::mm_set_offset(unsigned char servo_id,double offset) +void CBioloidRobot::mm_set_servo_offset(unsigned char servo_id,double offset) { if(offset<-9.0 || offset>9.0) throw CBioloidRobotException(_HERE_,"Desired offset out of range"); @@ -723,7 +723,7 @@ void CBioloidRobot::mm_set_offset(unsigned char servo_id,double offset) } } -double CBioloidRobot::mm_get_offset(unsigned char servo_id) +double CBioloidRobot::mm_get_servo_offset(unsigned char servo_id) { unsigned char value; double offset; @@ -739,6 +739,24 @@ double CBioloidRobot::mm_get_offset(unsigned char servo_id) } } +std::vector<double> CBioloidRobot::mm_get_servo_offsets(void) +{ + int i=0; + signed char values[MAX_NUM_SERVOS]; + std::vector<double> offsets(MAX_NUM_SERVOS); + + if(this->robot_device!=NULL) + { + this->robot_device->read_registers(BIOLOID_MM_SERVO0_OFFSET,(unsigned char *)values,MAX_NUM_SERVOS); + + for(i=0;i<MAX_NUM_SERVOS;i++) + offsets[i]=((double)values[i])/16.0; + + return offsets; + } + else + throw CBioloidRobotException(_HERE_,"Invalid robot device"); +} // motion action interface void CBioloidRobot::action_load_page(unsigned char page_id) { diff --git a/src/bioloid_robot.h b/src/bioloid_robot.h index 2e997f2c50af6f7ada1efec13548e3e343c51730..320ee929462e79180db72fe5c61416d460741825 100644 --- a/src/bioloid_robot.h +++ b/src/bioloid_robot.h @@ -158,8 +158,9 @@ class CBioloidRobot mm_mode_t mm_get_module(unsigned char servo_id); std::vector<double> mm_get_servo_angles(void); double mm_get_servo_angle(unsigned char servo_id); - void mm_set_offset(unsigned char servo_id,double offset); - double mm_get_offset(unsigned char servo_id); + void mm_set_servo_offset(unsigned char servo_id,double offset); + double mm_get_servo_offset(unsigned char servo_id); + std::vector<double> mm_get_servo_offsets(void); // motion action interface void action_load_page(unsigned char page_id); unsigned char action_get_current_page(void); diff --git a/src/examples/CMakeLists.txt b/src/examples/CMakeLists.txt index a5b05ecf70bdf338d9e45ee0194eb1b324a398f3..79349548b02bf8837d0eea20b582f3ef050f4cd8 100644 --- a/src/examples/CMakeLists.txt +++ b/src/examples/CMakeLists.txt @@ -1,4 +1,24 @@ # create an example application -ADD_EXECUTABLE(bioloid_robot_test bioloid_robot_test.cpp) +ADD_EXECUTABLE(bioloid_robot_gpio_test bioloid_robot_gpio_test.cpp) # link necessary libraries -TARGET_LINK_LIBRARIES(bioloid_robot_test bioloid_robot) +TARGET_LINK_LIBRARIES(bioloid_robot_gpio_test bioloid_robot) + +# create an example application +ADD_EXECUTABLE(bioloid_robot_adc_test bioloid_robot_adc_test.cpp) +# link necessary libraries +TARGET_LINK_LIBRARIES(bioloid_robot_adc_test bioloid_robot) + +# create an example application +ADD_EXECUTABLE(bioloid_robot_zigbee_test bioloid_robot_zigbee_test.cpp) +# link necessary libraries +TARGET_LINK_LIBRARIES(bioloid_robot_zigbee_test bioloid_robot) + +# create an example application +ADD_EXECUTABLE(bioloid_robot_mm_status_test bioloid_robot_mm_status_test.cpp) +# link necessary libraries +TARGET_LINK_LIBRARIES(bioloid_robot_mm_status_test bioloid_robot) + +# create an example application +ADD_EXECUTABLE(bioloid_robot_mm_action_test bioloid_robot_mm_action_test.cpp) +# link necessary libraries +TARGET_LINK_LIBRARIES(bioloid_robot_mm_action_test bioloid_robot) diff --git a/src/examples/bioloid_robot_adc_test.cpp b/src/examples/bioloid_robot_adc_test.cpp new file mode 100644 index 0000000000000000000000000000000000000000..9ad5b1849b20909ab757b09a521eb1c59dbb3460 --- /dev/null +++ b/src/examples/bioloid_robot_adc_test.cpp @@ -0,0 +1,31 @@ +#include "bioloid_robot.h" +#include "bioloid_robot_exceptions.h" +#include "action_id.h" + +#include <iostream> +#include <stdint.h> + +std::string robot_device="/dev/ttyO1"; + +int main(int argc, char *argv[]) +{ + unsigned int i=0; + + try{ + CBioloidRobot tina("Tina",robot_device,921600,0x02,GPIO2,25); + for(i=0;i<100;i++) + { + std::cout << "temperature: " << tina.adc_get_temperature() << std::endl; + std::cout << "Vref_int: " << tina.adc_get_vrefint() << std::endl; + std::cout << "Channel 1: " << tina.adc_get_value(ADC_CH1) << std::endl; + std::cout << "Channel 2: " << tina.adc_get_value(ADC_CH2) << std::endl; + std::cout << "Channel 3: " << tina.adc_get_value(ADC_CH3) << std::endl; + std::cout << "Channel 4: " << tina.adc_get_value(ADC_CH4) << std::endl; + std::cout << "Channel 5: " << tina.adc_get_value(ADC_CH5) << std::endl; + std::cout << "Channel 6: " << tina.adc_get_value(ADC_CH6) << std::endl; +// usleep(100000); + } + }catch(CException &e){ + std::cout << e.what() << std::endl; + } +} diff --git a/src/examples/bioloid_robot_gpio_test.cpp b/src/examples/bioloid_robot_gpio_test.cpp new file mode 100644 index 0000000000000000000000000000000000000000..c1fca46ed7e2389064274f28a90b77c9069a46a9 --- /dev/null +++ b/src/examples/bioloid_robot_gpio_test.cpp @@ -0,0 +1,71 @@ +#include "bioloid_robot.h" +#include "bioloid_robot_exceptions.h" +#include "action_id.h" + +#include <iostream> +#include <stdint.h> + +std::string robot_device="/dev/ttyO1"; + +class CCallbacks +{ + public: + CCallbacks(){}; + void user_pb_callback(CBioloidRobot *robot) + { + std::cout << "user push button pressed" << std::endl; + } + void reset_pb_callback(CBioloidRobot *robot) + { + std::cout << "reset push button pressed" << std::endl; + } + void start_pb_callback(CBioloidRobot *robot) + { + std::cout << "start push button pressed" << std::endl; + } + void mode_pb_callback(CBioloidRobot *robot) + { + std::cout << "mode push button pressed" << std::endl; + } + ~CCallbacks(){}; +}; + +int main(int argc, char *argv[]) +{ + unsigned int i=0; + CCallbacks callbacks; + + try{ + CBioloidRobot tina("Tina",robot_device,921600,0x02,GPIO2,25); + tina.assign_button_callback(USER_PB,&callbacks,&CCallbacks::user_pb_callback); + tina.assign_button_callback(RESET_PB,&callbacks,&CCallbacks::reset_pb_callback); + tina.assign_button_callback(START_PB,&callbacks,&CCallbacks::start_pb_callback); + tina.assign_button_callback(MODE_PB,&callbacks,&CCallbacks::mode_pb_callback); + for(i=0;i<4;i++) + { + tina.toggle_led(RXD_LED); + tina.toggle_led(TXD_LED); + tina.toggle_led(USER1_LED); + tina.toggle_led(USER2_LED); + sleep(1); + } + tina.clear_led(RXD_LED); + tina.clear_led(TXD_LED); + tina.clear_led(USER1_LED); + tina.clear_led(USER2_LED); + + tina.blink_led(TXD_LED,1000); + tina.blink_led(RXD_LED,2000); + tina.blink_led(USER1_LED,1000); + tina.blink_led(USER2_LED,2000); + sleep(10); + + tina.clear_led(RXD_LED); + tina.clear_led(TXD_LED); + tina.clear_led(USER1_LED); + tina.clear_led(USER2_LED); + sleep(10); + }catch(CException &e){ + std::cout << e.what() << std::endl; + } +} diff --git a/src/examples/bioloid_robot_mm_action_test.cpp b/src/examples/bioloid_robot_mm_action_test.cpp new file mode 100644 index 0000000000000000000000000000000000000000..cdd34ece2f269ed65dbe48b1d60c5616ae90b9f1 --- /dev/null +++ b/src/examples/bioloid_robot_mm_action_test.cpp @@ -0,0 +1,67 @@ +#include "bioloid_robot.h" +#include "bioloid_robot_exceptions.h" +#include "action_id.h" + +#include <iostream> +#include <stdint.h> + +std::string robot_device="/dev/ttyO1"; + +int main(int argc, char *argv[]) +{ + std::vector<double> angles; + unsigned int i=0,j=0; + + try{ + CBioloidRobot tina("Tina",robot_device,921600,0x02,GPIO2,25); + std::cout << "Number of servos: " << (int)tina.mm_get_num_servos() << std::endl; + for(i=0;i<=MAX_NUM_SERVOS;i++) + { + tina.mm_enable_servo(i); + tina.mm_assign_module(i,BIOLOID_MM_ACTION); + if(tina.mm_is_servo_enabled(i)) + std::cout << "Servo " << i << " is enabled"; + else + std::cout << "Servo " << i << " is disabled"; + std::cout << " and assigned to module " << tina.mm_get_module(i) << std::endl; + } + tina.mm_start(); + tina.mm_enable_power(); + sleep(1); + tina.action_load_page(WALK_READY); + tina.action_start(); + while(tina.action_is_page_running()) + { + // get the servos positions + angles=tina.mm_get_servo_angles(); + for(j=0;j<=MAX_NUM_SERVOS;j++) + std::cout << "Servo " << j << " angle: " << angles[j] << std::endl; + usleep(100000); + } + tina.action_load_page(SIT_DOWN); + tina.action_start(); + while(tina.action_is_page_running()) + { + // get the servos positions + angles=tina.mm_get_servo_angles(); + for(j=0;j<=MAX_NUM_SERVOS;j++) + std::cout << "Servo " << j << " angle: " << angles[j] << std::endl; + usleep(100000); + } + sleep(1); + tina.action_load_page(STAND_UP); + tina.action_start(); + while(tina.action_is_page_running()) + { + // get the servos positions + angles=tina.mm_get_servo_angles(); + for(j=0;j<=MAX_NUM_SERVOS;j++) + std::cout << "Servo " << j << " angle: " << angles[j] << std::endl; + usleep(100000); + } + tina.mm_disable_power(); + tina.mm_stop(); + }catch(CException &e){ + std::cout << e.what() << std::endl; + } +} diff --git a/src/examples/bioloid_robot_mm_status_test.cpp b/src/examples/bioloid_robot_mm_status_test.cpp new file mode 100644 index 0000000000000000000000000000000000000000..5f73d5d031fcfc8675ee0a4d0e5ea82768811cc7 --- /dev/null +++ b/src/examples/bioloid_robot_mm_status_test.cpp @@ -0,0 +1,55 @@ +#include "bioloid_robot.h" +#include "bioloid_robot_exceptions.h" +#include "action_id.h" + +#include <iostream> +#include <stdint.h> + +std::string robot_device="/dev/ttyO1"; + +int main(int argc, char *argv[]) +{ + std::vector<double> angles; + unsigned int i=0,j=0; + + try{ + CBioloidRobot tina("Tina",robot_device,921600,0x02,GPIO2,25); + std::cout << "Number of servos: " << (int)tina.mm_get_num_servos() << std::endl; + tina.mm_set_period(7.8); + std::cout << "Motion manager period: " << tina.mm_get_period() << std::endl; + // check the state of all servos + for(i=0;i<=MAX_NUM_SERVOS;i++) + { + if(tina.mm_is_servo_enabled(i)) + std::cout << "Servo " << i << " is enabled"; + else + std::cout << "Servo " << i << " is disabled"; + std::cout << " and assigned to module " << tina.mm_get_module(i) << std::endl; + } + // enable all servos and assign them to the ACTION MODULE + for(i=0;i<=MAX_NUM_SERVOS;i++) + { + tina.mm_disable_servo(i); + tina.mm_assign_module(i,BIOLOID_MM_NONE); + if(tina.mm_is_servo_enabled(i)) + std::cout << "Servo " << i << " is enabled"; + else + std::cout << "Servo " << i << " is disabled"; + std::cout << " and assigned to module " << tina.mm_get_module(i) << std::endl; + } + tina.mm_start(); + tina.mm_enable_power(); + for(i=0;i<1000;i++) + { + // get the servos positions + angles=tina.mm_get_servo_angles(); + for(j=0;j<=MAX_NUM_SERVOS;j++) + std::cout << "Servo " << j << " angle: " << angles[j] << std::endl; + usleep(100000); + } + tina.mm_disable_power(); + tina.mm_stop(); + }catch(CException &e){ + std::cout << e.what() << std::endl; + } +} diff --git a/src/examples/bioloid_robot_test.cpp b/src/examples/bioloid_robot_test.cpp deleted file mode 100644 index 81a73374b5ac557b31d7a55c0aa3eba2b68fdb80..0000000000000000000000000000000000000000 --- a/src/examples/bioloid_robot_test.cpp +++ /dev/null @@ -1,266 +0,0 @@ -#include "bioloid_robot.h" -#include "bioloid_robot_exceptions.h" -#include "action_id.h" - -#include <iostream> -#include <stdint.h> - -std::string robot_device="/dev/ttyO1"; - -class CCallbacks -{ - public: - CCallbacks(){}; - void user_pb_callback(CBioloidRobot *robot) - { - std::cout << "user push button pressed" << std::endl; - } - void reset_pb_callback(CBioloidRobot *robot) - { - std::cout << "reset push button pressed" << std::endl; - } - void start_pb_callback(CBioloidRobot *robot) - { - std::cout << "start push button pressed" << std::endl; - } - void mode_pb_callback(CBioloidRobot *robot) - { - std::cout << "mode push button pressed" << std::endl; - } - ~CCallbacks(){}; -}; - -int main(int argc, char *argv[]) -{ - std::vector<double> angles; - unsigned int i=0,j=0; - CCallbacks callbacks; - unsigned char key; - double offset=0.0; - - try{ - CBioloidRobot tina("Tina",robot_device,921600,0x02,GPIO2,25); -/* tina.assign_button_callback(USER_PB,&callbacks,&CCallbacks::user_pb_callback); - tina.assign_button_callback(RESET_PB,&callbacks,&CCallbacks::reset_pb_callback); - tina.assign_button_callback(START_PB,&callbacks,&CCallbacks::start_pb_callback); - tina.assign_button_callback(MODE_PB,&callbacks,&CCallbacks::mode_pb_callback); - for(i=0;i<4;i++) - { - tina.toggle_led(RXD_LED); - tina.toggle_led(TXD_LED); - tina.toggle_led(USER1_LED); - tina.toggle_led(USER2_LED); - sleep(1); - } - tina.clear_led(RXD_LED); - tina.clear_led(TXD_LED); - tina.clear_led(USER1_LED); - tina.clear_led(USER2_LED); - - tina.blink_led(TXD_LED,1000); - tina.blink_led(RXD_LED,2000); - tina.blink_led(USER1_LED,1000); - tina.blink_led(USER2_LED,2000); - sleep(10); - - tina.clear_led(RXD_LED); - tina.clear_led(TXD_LED); - tina.clear_led(USER1_LED); - tina.clear_led(USER2_LED); - sleep(10);*/ -/* for(i=0;i<100;i++) - { - std::cout << "temperature: " << tina.adc_get_temperature() << std::endl; - std::cout << "Vref_int: " << tina.adc_get_vrefint() << std::endl; - std::cout << "Channel 1: " << tina.adc_get_value(ADC_CH1) << std::endl; - std::cout << "Channel 2: " << tina.adc_get_value(ADC_CH2) << std::endl; - std::cout << "Channel 3: " << tina.adc_get_value(ADC_CH3) << std::endl; - std::cout << "Channel 4: " << tina.adc_get_value(ADC_CH4) << std::endl; - std::cout << "Channel 5: " << tina.adc_get_value(ADC_CH5) << std::endl; - std::cout << "Channel 6: " << tina.adc_get_value(ADC_CH6) << std::endl; - usleep(100000); - }*/ -/* tina.zigbee_enable_power(); - sleep(3); - tina.zigbee_disable_power();*/ -/* std::cout << "Number of servos: " << (int)tina.mm_get_num_servos() << std::endl; - tina.mm_set_period(7.8); - std::cout << "Motion manager period: " << tina.mm_get_period() << std::endl; - // check the state of all servos - for(i=0;i<=MAX_NUM_SERVOS;i++) - { - if(tina.mm_is_servo_enabled(i)) - std::cout << "Servo " << i << " is enabled"; - else - std::cout << "Servo " << i << " is disabled"; - std::cout << " and assigned to module " << tina.mm_get_module(i) << std::endl; - } - // enable all servos and assign them to the ACTION MODULE - for(i=0;i<=MAX_NUM_SERVOS;i++) - { - tina.mm_disable_servo(i); - tina.mm_assign_module(i,BIOLOID_MM_NONE); - if(tina.mm_is_servo_enabled(i)) - std::cout << "Servo " << i << " is enabled"; - else - std::cout << "Servo " << i << " is disabled"; - std::cout << " and assigned to module " << tina.mm_get_module(i) << std::endl; - } - tina.mm_start(); - tina.mm_enable_power(); - for(i=0;i<1000;i++) - { - // get the servos positions - angles=tina.mm_get_servo_angles(); - for(j=0;j<=MAX_NUM_SERVOS;j++) - std::cout << "Servo " << j << " angle: " << angles[j] << std::endl; - usleep(100000); - } - tina.mm_disable_power(); - tina.mm_stop();*/ -/* std::cout << "Number of servos: " << (int)tina.mm_get_num_servos() << std::endl; - for(i=0;i<=MAX_NUM_SERVOS;i++) - { - tina.mm_enable_servo(i); - tina.mm_assign_module(i,BIOLOID_MM_ACTION); - if(tina.mm_is_servo_enabled(i)) - std::cout << "Servo " << i << " is enabled"; - else - std::cout << "Servo " << i << " is disabled"; - std::cout << " and assigned to module " << tina.mm_get_module(i) << std::endl; - } - tina.mm_start(); -// tina.mm_disable_balance(); - tina.mm_enable_balance(); - tina.mm_enable_power(); - sleep(1); - tina.action_load_page(WALK_READY); - tina.action_start(); - while(tina.action_is_page_running()) - { - // get the servos positions - angles=tina.mm_get_servo_angles(); - for(j=0;j<=MAX_NUM_SERVOS;j++) - std::cout << "Servo " << j << " angle: " << angles[j] << std::endl; - usleep(100000); - } - tina.action_load_page(SIT_DOWN); - tina.action_start(); - while(tina.action_is_page_running()) - { - // get the servos positions - angles=tina.mm_get_servo_angles(); - for(j=0;j<=MAX_NUM_SERVOS;j++) - std::cout << "Servo " << j << " angle: " << angles[j] << std::endl; - usleep(100000); - } - sleep(1); - tina.action_load_page(STAND_UP); - tina.action_start(); - while(tina.action_is_page_running()) - { - // get the servos positions - angles=tina.mm_get_servo_angles(); - for(j=0;j<=MAX_NUM_SERVOS;j++) - std::cout << "Servo " << j << " angle: " << angles[j] << std::endl; - usleep(100000); - } - for(i=0;i<100;i++) - { - // get the servos positions - angles=tina.mm_get_servo_angles(); -// for(j=0;j<=MAX_NUM_SERVOS;j++) -// std::cout << "Servo " << j << " angle: " << angles[j] << std::endl; - std::cout << "Servo " << 13 << " angle: " << angles[13] << std::endl; - std::cout << "Channel 1: " << tina.adc_get_value(ADC_CH1) << std::endl; - std::cout << "Channel 2: " << tina.adc_get_value(ADC_CH2) << std::endl; - usleep(100000); - } - tina.mm_disable_power(); - tina.mm_stop();*/ - //tina.mm_set_period(7.8); -/* std::cout << "Motion manager period: " << tina.mm_get_period() << std::endl; - std::cout << "Number of servos: " << (int)tina.mm_get_num_servos() << std::endl; - for(i=0;i<=MAX_NUM_SERVOS;i++) - { - tina.mm_enable_servo(i); - tina.mm_assign_module(i,BIOLOID_MM_ACTION); - if(tina.mm_is_servo_enabled(i)) - std::cout << "Servo " << i << " is enabled"; - else - std::cout << "Servo " << i << " is disabled"; - std::cout << " and assigned to module " << tina.mm_get_module(i) << std::endl; - } - tina.mm_start(); - //tina.mm_enable_balance(); - tina.mm_enable_power(); - tina.action_load_page(WALK_READY); - tina.action_start(); - while(tina.action_is_page_running()) - usleep(100000); - sleep(2); - tina.action_load_page(RT_S_R); - tina.action_start(); - for(i=0;i<100;i++) - { - std::cout << "Current page: " << (int)tina.action_get_current_page() << std::endl; - usleep(100000); - } - tina.action_stop(); - while(tina.action_is_page_running()) - usleep(100000); - sleep(2); - tina.mm_disable_power(); - tina.mm_stop();*/ - tina.mm_set_offset(1,0.0); - tina.mm_set_offset(2,0.0); - tina.mm_set_offset(8,0.0); - tina.mm_set_offset(9,0.0); - tina.mm_set_offset(10,0.0);//5.25 - tina.mm_set_offset(11,0.0);//1.75 - tina.mm_set_offset(12,0.0);//4.0 - tina.mm_set_offset(13,0.0); - tina.mm_set_offset(14,0.0); - tina.mm_set_offset(15,0.0);//1.75 - tina.mm_set_offset(16,0.0);//-4 - tina.mm_set_offset(17,0.0); - tina.mm_set_offset(18,0.0); - for(i=0;i<=MAX_NUM_SERVOS;i++) - { - std::cout << "Servo " << i << " offset: " << tina.mm_get_offset(i) << std::endl; - } - std::cout << "Motion manager period: " << tina.mm_get_period() << std::endl; - std::cout << "Number of servos: " << (int)tina.mm_get_num_servos() << std::endl; - for(i=0;i<=MAX_NUM_SERVOS;i++) - { - tina.mm_enable_servo(i); - tina.mm_assign_module(i,BIOLOID_MM_ACTION); - if(tina.mm_is_servo_enabled(i)) - std::cout << "Servo " << i << " is enabled"; - else - std::cout << "Servo " << i << " is disabled"; - std::cout << " and assigned to module " << tina.mm_get_module(i) << std::endl; - } - tina.mm_start(); - //tina.mm_enable_balance(); - tina.mm_enable_power(); - tina.action_load_page(WALK_READY); - tina.action_start(); - while(tina.action_is_page_running()) - usleep(100000); - do{ - std::cin >> key; - if(key=='+') - offset+=0.5; - else if(key=='-') - offset-=0.5; - std::cout << offset << std::endl; - tina.mm_set_offset(1,offset); - }while(key!='q'); - sleep(10); - tina.mm_disable_power(); - tina.mm_stop(); - }catch(CException &e){ - std::cout << e.what() << std::endl; - } -} diff --git a/src/examples/bioloid_robot_zigbee_test.cpp b/src/examples/bioloid_robot_zigbee_test.cpp new file mode 100644 index 0000000000000000000000000000000000000000..225a4016019d3d10c9bf644036930825db329e0e --- /dev/null +++ b/src/examples/bioloid_robot_zigbee_test.cpp @@ -0,0 +1,20 @@ +#include "bioloid_robot.h" +#include "bioloid_robot_exceptions.h" +#include "action_id.h" + +#include <iostream> +#include <stdint.h> + +std::string robot_device="/dev/ttyO1"; + +int main(int argc, char *argv[]) +{ + try{ + CBioloidRobot tina("Tina",robot_device,921600,0x02,GPIO2,25); + tina.zigbee_enable_power(); + sleep(3); + tina.zigbee_disable_power(); + }catch(CException &e){ + std::cout << e.what() << std::endl; + } +} diff --git a/src/tools/CMakeLists.txt b/src/tools/CMakeLists.txt new file mode 100644 index 0000000000000000000000000000000000000000..7b118e2bac0548b5000add2114afef5935b41957 --- /dev/null +++ b/src/tools/CMakeLists.txt @@ -0,0 +1,12 @@ +Set(CURSES_NEED_NCURSES TRUE) + +FIND_PACKAGE(Curses REQUIRED) + +INCLUDE_DIRECTORIES(${CURSES_INCLUDE_DIR}) + +# create an example application +ADD_EXECUTABLE(bioloid_offset bioloid_offset.cpp) +# link necessary libraries +TARGET_LINK_LIBRARIES(bioloid_offset bioloid_robot) +TARGET_LINK_LIBRARIES(bioloid_offset ${CURSES_LIBRARIES}) + diff --git a/src/tools/bioloid_offset.cpp b/src/tools/bioloid_offset.cpp new file mode 100644 index 0000000000000000000000000000000000000000..0952484a08c1e7d3686c65cc313b94ce28e512e2 --- /dev/null +++ b/src/tools/bioloid_offset.cpp @@ -0,0 +1,303 @@ +#include "bioloid_robot.h" +#include "bioloid_robot_exceptions.h" +#include "action_id.h" +#include "mutex.h" +#include "threadserver.h" +#include "eventserver.h" + +#include <ncurses.h> +#include <stdint.h> +#include <math.h> +#include <signal.h> +#include <iostream> + +std::string robot_device="/dev/ttyO1"; + +const int num_key_cmd=7; +const int max_key_cmd_length=30; +const unsigned char key_cmds[num_key_cmd][max_key_cmd_length]={"UP - select upper servo", + "DOWN - select lower servo", + "+ - increment servo offset", + "- - decrement servo offset", + "i - increment delta", + "d - decrement delta", + "q - quit"}; + +const unsigned char servo_info[]=" Servo Angle Offset"; +const unsigned char current_inc[]="current angle increment"; + +// bioloid global attributes +int current_servo_sel=1; +double increment=0.0625; +CBioloidRobot *tina=NULL; +// window global attributes +CMutex window_access; +std::string resize_event_id; +std::string finish_thread_event_id; + +int get_cmd_per_line(int col) +{ + return col/max_key_cmd_length; +} + +int get_num_cmd_lines(int col) +{ + int cmd_per_line; + + cmd_per_line=get_cmd_per_line(col); + if(cmd_per_line==0) + return -1; + else + return ceil((double)num_key_cmd/(double)cmd_per_line); +} + +int show_key_cmds(int row,int col) +{ + int i,j,num_lines,cmd_id=0; + + // cout number of lines to fit all the commands + if((num_lines=get_num_cmd_lines(col))<0) + return -1; + for(i=0;i<col;i++) + mvprintw(row-(num_lines+1),i,"-"); + for(i=row-num_lines;i<row;i++) + { + for(j=0;j<get_cmd_per_line(col);j++) + { + if(cmd_id<num_key_cmd) + { + mvprintw(i,j*max_key_cmd_length,"%s",key_cmds[cmd_id]); + cmd_id++; + } + } + } + + return 0; +} + +int show_servo_info(int row,int col) +{ + int i,num_servos,num_cmd_lines,num_servo_lines,servo_id; + std::vector<double> angles; + std::vector<double> offsets; + static int start_show=1; + + num_servos=tina->mm_get_num_servos(); + if((num_cmd_lines=get_num_cmd_lines(col))<0) + return -1; + num_servo_lines=row-num_cmd_lines-3; + // get servo angles and offsets + angles=tina->mm_get_servo_angles(); + offsets=tina->mm_get_servo_offsets(); + attron(A_BOLD); + mvprintw(0,4,"%s",servo_info); + attroff(A_BOLD); + if(num_servos>num_servo_lines) + { + if(start_show>current_servo_sel) + start_show=current_servo_sel; + else + { + if(start_show+num_servo_lines<=current_servo_sel) + start_show=current_servo_sel-num_servo_lines+1; + } + if(start_show>1) + mvprintw(1,1,"*"); + if(num_servos-start_show>=num_servo_lines) + mvprintw(num_servo_lines,1,"*"); + } + else + start_show=1; + for(i=0;i<std::min(num_servo_lines,num_servos);i++) + { + servo_id=start_show+i; + if(servo_id==current_servo_sel) + { + attron(A_BOLD); + attron(COLOR_PAIR(1)); + } + else + { + attroff(COLOR_PAIR(1)); + attroff(A_BOLD); + } + mvprintw(i+1,4,"Servo %d",servo_id); + mvprintw(i+1,16,"%f",angles[servo_id]); + mvprintw(i+1,32,"%f",offsets[servo_id]); + } + + attroff(COLOR_PAIR(1)); + attroff(A_BOLD); + + // show current angle increment + mvprintw(0,col-sizeof(current_inc),"%s",current_inc); + mvprintw(1,col-2*sizeof(current_inc)/3,"%f",increment); + + return 0; +} + +void *resize_thread(void *param) +{ + CEventServer *event_server=CEventServer::instance(); + std::list<std::string> events; + int event_id,row,col; + bool end=false; + + events.push_back(resize_event_id); + events.push_back(finish_thread_event_id); + while(!end) + { + event_id=event_server->wait_first(events); + if(event_id==0)// resize event + { + try{ + window_access.enter(); + endwin(); + refresh(); + clear(); + getmaxyx(stdscr,row,col); + show_key_cmds(row,col); + show_servo_info(row,col); + refresh(); + window_access.exit(); + }catch(CException &e){ + window_access.exit(); + } + } + else if(event_id==1) + end=true; + } + + pthread_exit(NULL); +} + +void do_resize(int dummy) +{ + CEventServer *event_server=CEventServer::instance(); + + if(!event_server->event_is_set(resize_event_id)) + event_server->set_event(resize_event_id); +} + +int main(int argc, char *argv[]) +{ + CEventServer *event_server=CEventServer::instance(); + CThreadServer *thread_server=CThreadServer::instance(); + std::string resize_thread_id; + std::vector<double> offsets; + unsigned int i=0,row,col; + unsigned char key; + + try{ + resize_event_id="window_resize_event"; + event_server->create_event(resize_event_id); + finish_thread_event_id="finish_thread_event"; + event_server->create_event(finish_thread_event_id); + resize_thread_id="resize_thread"; + thread_server->create_thread(resize_thread_id); + thread_server->attach_thread(resize_thread_id,resize_thread,NULL); + thread_server->start_thread(resize_thread_id); + // assign a function to handle window resize events + signal(SIGWINCH, do_resize); + // initialize the robot object + tina=new CBioloidRobot("Tina",robot_device,921600,0x02,GPIO2,25); + for(i=0;i<=MAX_NUM_SERVOS;i++) + { + tina->mm_enable_servo(i); + tina->mm_assign_module(i,BIOLOID_MM_ACTION); + } + tina->mm_start(); + tina->mm_enable_power(); + tina->action_load_page(WALK_READY); + tina->action_start(); + while(tina->action_is_page_running()) + usleep(100000); + offsets=tina->mm_get_servo_offsets(); + // initialize the screen + initscr(); + start_color(); + init_pair(1, COLOR_GREEN, COLOR_BLACK); + keypad(stdscr,true); + do{ + getmaxyx(stdscr,row,col); + show_key_cmds(row,col); + show_servo_info(row,col); + refresh(); + key=getch(); + if(key=='+') + { + offsets[current_servo_sel]+=increment; + if(offsets[current_servo_sel]>7.5) + offsets[current_servo_sel]=7.5; + else if(offsets[current_servo_sel]<-7.5) + offsets[current_servo_sel]=-7.5; + tina->mm_set_servo_offset(current_servo_sel,offsets[current_servo_sel]); + } + else if(key=='-') + { + offsets[current_servo_sel]-=increment; + if(offsets[current_servo_sel]>7.5) + offsets[current_servo_sel]=7.5; + else if(offsets[current_servo_sel]<-7.5) + offsets[current_servo_sel]=-7.5; + tina->mm_set_servo_offset(current_servo_sel,offsets[current_servo_sel]); + } + else if(key=='i') + { + increment+=0.0625; + if(increment>2.0) + increment=2.0; + else if(increment<0.0625) + increment=0.0625; + } + else if(key=='d') + { + increment-=0.0625; + if(increment>2.0) + increment=2.0; + else if(increment<0.0625) + increment=0.0625; + } + else if(key==3) + { + if(current_servo_sel>1) + current_servo_sel--; + } + else if(key==2) + { + if(current_servo_sel<tina->mm_get_num_servos()) + current_servo_sel++; + } + clear(); + }while(key!='q'); + + endwin(); + event_server->set_event(finish_thread_event_id); + thread_server->end_thread(resize_thread_id); + thread_server->detach_thread(resize_thread_id); + thread_server->delete_thread(resize_thread_id); + event_server->delete_event(resize_event_id); + event_server->delete_event(finish_thread_event_id); + tina->action_load_page(SIT_DOWN); + tina->action_start(); + while(tina->action_is_page_running()) + usleep(100000); + tina->mm_disable_power(); + tina->mm_stop(); + delete tina; + + return 0; + }catch(CException &e){ + endwin(); + event_server->set_event(finish_thread_event_id); + thread_server->end_thread(resize_thread_id); + thread_server->detach_thread(resize_thread_id); + thread_server->delete_thread(resize_thread_id); + event_server->delete_event(resize_event_id); + event_server->delete_event(finish_thread_event_id); + tina->mm_disable_power(); + tina->mm_stop(); + if(tina!=NULL) + delete tina; + } +}