diff --git a/src/CMakeLists.txt b/src/CMakeLists.txt
index ca1934ca8c3bbaa0a1bc7bcbd07a278be3baf626..173b64e0e0dcdf7c7b2b9747ad6f9a4f586f85bb 100644
--- a/src/CMakeLists.txt
+++ b/src/CMakeLists.txt
@@ -13,7 +13,7 @@ INCLUDE (${PROJECT_SOURCE_DIR}/FindKDL.cmake)
# driver source files
SET(robot_sources darwin_robot.cpp darwin_robot_exceptions.cpp)
# application header files
-SET(robot_headers darwin_robot.h darwin_robot_exceptions.h darwin_registers.h)
+SET(robot_headers darwin_robot.h darwin_robot_exceptions.h)
# driver source files
SET(kin_sources darwin_arm_kinematics.cpp darwin_robot_exceptions.cpp)
@@ -27,6 +27,7 @@ FIND_PACKAGE(dynamixel REQUIRED)
INCLUDE_DIRECTORIES(.)
INCLUDE_DIRECTORIES(${iriutils_INCLUDE_DIR})
INCLUDE_DIRECTORIES(${dynamixel_INCLUDE_DIR})
+INCLUDE_DIRECTORIES(../../../stm32_firmware/include/)
# create the shared library
ADD_LIBRARY(darwin_robot SHARED ${robot_sources} ${XSD_SOURCES})
ADD_DEPENDENCIES(darwin_robot xsd_files_gen)
diff --git a/src/darwin_registers.h b/src/darwin_registers.h
deleted file mode 100644
index 7167737b04dd9b7f5d6f4e90ccd671e0f4ac1382..0000000000000000000000000000000000000000
--- a/src/darwin_registers.h
+++ /dev/null
@@ -1,250 +0,0 @@
-#ifndef _DARWIN_REGISTERS_
-#define _DARWIN_REGISTERS_
-
-#define MAX_NUM_SERVOS 27
-
-typedef enum {
- DARWIN_MODEL_NUMBER_L = 0x00,
- DARWIN_MODEL_NUMBER_H = 0x01,
- DARWIN_VERSION = 0x02,
- DARWIN_ID = 0x03,
- DARWIN_BAUD_RATE = 0x04,
- DARWIN_RETURN_DELAY_TIME = 0x05,
- DARWIN_MM_PERIOD_LOW = 0x06,
- DARWIN_MM_PERIOD_HIGH = 0x07,
- DARWIN_RETURN_LEVEL = 0x10,
- DARWIN_DXL_POWER = 0x18,
- DARWIN_LED_PANNEL = 0x19,
- DARWIN_LED_5_L = 0x1A,
- DARWIN_LED_5_H = 0x1B,
- DARWIN_LED_6_L = 0x1C,
- DARWIN_LED_6_H = 0x1D,
- DARWIN_PUSHBUTTON = 0x1E,
- DARWIN_MM_NUM_SERVOS = 0x1F,
- DARWIN_MM_CONTROL = 0x20,
- DARWIN_MM_STATUS = 0x21,
- DARWIN_WALK_CONTROL = 0x22,
- DARWIN_WALK_STATUS = 0x23,
- DARWIN_IMU_CONTROL = 0x24,
- DARWIN_IMU_STATUS = 0x25,
- DARWIN_IMU_ACCEL_X_L = 0x26,
- DARWIN_IMU_ACCEL_X_H = 0x27,
- DARWIN_IMU_ACCEL_Y_L = 0x28,
- DARWIN_IMU_ACCEL_Y_H = 0x29,
- DARWIN_IMU_ACCEL_Z_L = 0x2A,
- DARWIN_IMU_ACCEL_Z_H = 0x2B,
- DARWIN_IMU_GYRO_X_L = 0x2C,
- DARWIN_IMU_GYRO_X_H = 0x2D,
- DARWIN_IMU_GYRO_Y_L = 0x2E,
- DARWIN_IMU_GYRO_Y_H = 0x2F,
- DARWIN_IMU_GYRO_Z_L = 0x30,
- DARWIN_IMU_GYRO_Z_H = 0x31,
- DARWIN_VOLTAGE = 0x32,
- DARWIN_MIC1_L = 0x33,
- DARWIN_MIC1_H = 0x34,
- DARWIN_ADC2_L = 0x35,
- DARWIN_ADC2_H = 0x36,
- DARWIN_ADC3_L = 0x37,
- DARWIN_ADC3_H = 0x38,
- DARWIN_ADC4_L = 0x39,
- DARWIN_ADC4_H = 0x3A,
- DARWIN_ADC5_L = 0x3B,
- DARWIN_ADC5_H = 0x3C,
- DARWIN_ADC6_L = 0x3D,
- DARWIN_ADC6_H = 0x3E,
- DARWIN_ADC7_L = 0x3F,
- DARWIN_ADC7_H = 0x40,
- DARWIN_ADC8_L = 0x41,
- DARWIN_ADC8_H = 0x42,
- DARWIN_MIC2_L = 0x43,
- DARWIN_MIC2_H = 0x44,
- DARWIN_ADC10_L = 0x45,
- DARWIN_ADC10_H = 0x46,
- DARWIN_ADC11_L = 0x47,
- DARWIN_ADC11_H = 0x48,
- DARWIN_ADC12_L = 0x49,
- DARWIN_ADC12_H = 0x4A,
- DARWIN_ADC13_L = 0x4B,
- DARWIN_ADC13_H = 0x4C,
- DARWIN_ADC14_L = 0x4D,
- DARWIN_ADC14_H = 0x4E,
- DARWIN_ADC15_L = 0x4F,
- DARWIN_ADC15_H = 0x50,
- DARWIN_MM_MODULE_EN0 = 0x51,
- DARWIN_MM_MODULE_EN1 = 0x52,
- DARWIN_MM_MODULE_EN2 = 0x53,
- DARWIN_MM_MODULE_EN3 = 0x54,
- DARWIN_MM_MODULE_EN4 = 0x55,
- DARWIN_MM_MODULE_EN5 = 0x56,
- DARWIN_MM_MODULE_EN6 = 0x57,
- DARWIN_MM_MODULE_EN7 = 0x58,
- DARWIN_MM_MODULE_EN8 = 0x59,
- DARWIN_MM_MODULE_EN9 = 0x5A,
- DARWIN_MM_MODULE_EN10 = 0x5B,
- DARWIN_MM_MODULE_EN11 = 0x5C,
- DARWIN_MM_MODULE_EN12 = 0x5D,
- DARWIN_MM_MODULE_EN13 = 0x5E,
- DARWIN_MM_MODULE_EN14 = 0x5F,
- DARWIN_MM_MODULE_EN15 = 0x60,
- DARWIN_MM_SERVO0_CUR_POS_L = 0x61,
- DARWIN_MM_SERVO0_CUR_POS_H = 0x62,
- DARWIN_MM_SERVO1_CUR_POS_L = 0x63,
- DARWIN_MM_SERVO1_CUR_POS_H = 0x64,
- DARWIN_MM_SERVO2_CUR_POS_L = 0x65,
- DARWIN_MM_SERVO2_CUR_POS_H = 0x66,
- DARWIN_MM_SERVO3_CUR_POS_L = 0x67,
- DARWIN_MM_SERVO3_CUR_POS_H = 0x68,
- DARWIN_MM_SERVO4_CUR_POS_L = 0x69,
- DARWIN_MM_SERVO4_CUR_POS_H = 0x6A,
- DARWIN_MM_SERVO5_CUR_POS_L = 0x6B,
- DARWIN_MM_SERVO5_CUR_POS_H = 0x6C,
- DARWIN_MM_SERVO6_CUR_POS_L = 0x6D,
- DARWIN_MM_SERVO6_CUR_POS_H = 0x6E,
- DARWIN_MM_SERVO7_CUR_POS_L = 0x6F,
- DARWIN_MM_SERVO7_CUR_POS_H = 0x70,
- DARWIN_MM_SERVO8_CUR_POS_L = 0x71,
- DARWIN_MM_SERVO8_CUR_POS_H = 0x72,
- DARWIN_MM_SERVO9_CUR_POS_L = 0x73,
- DARWIN_MM_SERVO9_CUR_POS_H = 0x74,
- DARWIN_MM_SERVO10_CUR_POS_L = 0x75,
- DARWIN_MM_SERVO10_CUR_POS_H = 0x76,
- DARWIN_MM_SERVO11_CUR_POS_L = 0x77,
- DARWIN_MM_SERVO11_CUR_POS_H = 0x78,
- DARWIN_MM_SERVO12_CUR_POS_L = 0x79,
- DARWIN_MM_SERVO12_CUR_POS_H = 0x7A,
- DARWIN_MM_SERVO13_CUR_POS_L = 0x7B,
- DARWIN_MM_SERVO13_CUR_POS_H = 0x7C,
- DARWIN_MM_SERVO14_CUR_POS_L = 0x7D,
- DARWIN_MM_SERVO14_CUR_POS_H = 0x7E,
- DARWIN_MM_SERVO15_CUR_POS_L = 0x7F,
- DARWIN_MM_SERVO15_CUR_POS_H = 0x80,
- DARWIN_MM_SERVO16_CUR_POS_L = 0x81,
- DARWIN_MM_SERVO16_CUR_POS_H = 0x82,
- DARWIN_MM_SERVO17_CUR_POS_L = 0x83,
- DARWIN_MM_SERVO17_CUR_POS_H = 0x84,
- DARWIN_MM_SERVO18_CUR_POS_L = 0x85,
- DARWIN_MM_SERVO18_CUR_POS_H = 0x86,
- DARWIN_MM_SERVO19_CUR_POS_L = 0x87,
- DARWIN_MM_SERVO19_CUR_POS_H = 0x88,
- DARWIN_MM_SERVO20_CUR_POS_L = 0x89,
- DARWIN_MM_SERVO20_CUR_POS_H = 0x8A,
- DARWIN_MM_SERVO21_CUR_POS_L = 0x8B,
- DARWIN_MM_SERVO21_CUR_POS_H = 0x8C,
- DARWIN_MM_SERVO22_CUR_POS_L = 0x8D,
- DARWIN_MM_SERVO22_CUR_POS_H = 0x8E,
- DARWIN_MM_SERVO23_CUR_POS_L = 0x8F,
- DARWIN_MM_SERVO23_CUR_POS_H = 0x90,
- DARWIN_MM_SERVO24_CUR_POS_L = 0x91,
- DARWIN_MM_SERVO24_CUR_POS_H = 0x92,
- DARWIN_MM_SERVO25_CUR_POS_L = 0x93,
- DARWIN_MM_SERVO25_CUR_POS_H = 0x94,
- DARWIN_MM_SERVO26_CUR_POS_L = 0x95,
- DARWIN_MM_SERVO26_CUR_POS_H = 0x96,
- DARWIN_ACTION_PAGE = 0x97,
- DARWIN_ACTION_CONTROL = 0x98,
- DARWIN_ACTION_STATUS = 0x99,
- DARWIN_X_OFFSET = 0x9A,
- DARWIN_Y_OFFSET = 0x9B,
- DARWIN_Z_OFFSET = 0x9C,
- DARWIN_ROLL = 0x9D,
- DARWIN_PITCH = 0x9E,
- DARWIN_YAW = 0x9F,
- DARWIN_HIP_PITCH_OFF_L = 0xA0,
- DARWIN_HIP_PITCH_OFF_H = 0xA1,
- DARWIN_PERIOD_TIME_L = 0xA2,
- DARWIN_PERIOD_TIME_H = 0xA3,
- DARWIN_DSP_RATIO = 0xA4,
- DARWIN_STEP_FW_BW_RATIO = 0xA5,
- DARWIN_STEP_FW_BW = 0xA6,
- DARWIN_STEP_LEFT_RIGHT = 0xA7,
- DARWIN_STEP_DIRECTION = 0xA8,
- DARWIN_FOOT_HEIGHT = 0xA9,
- DARWIN_SWING_RIGHT_LEFT = 0xAA,
- DARWIN_SWING_TOP_DOWN = 0xAB,
- DARWIN_PELVIS_OFFSET = 0xAC,
- DARWIN_ARM_SWING_GAIN = 0xAD,
- DARWIN_P_GAIN = 0xAE,
- DARWIN_D_GAIN = 0xAF,
- DARWIN_I_GAIN = 0xB0,
- DARWIN_MAX_ACC = 0xB1,
- DARWIN_MAX_ROT_ACC = 0xB2,
- DARWIN_MM_KNEE_GAIN = 0xB3,
- DARWIN_MM_ANKLE_PITCH_GAIN = 0xB4,
- DARWIN_MM_HIP_ROLL_GAIN = 0xB5,
- DARWIN_MM_ANKLE_ROLL_GAIN = 0xB6,
- DARWIN_SERVO_0_SPEED = 0xB7,
- DARWIN_SERVO_1_SPEED = 0xB8,
- DARWIN_SERVO_2_SPEED = 0xB9,
- DARWIN_SERVO_3_SPEED = 0xBA,
- DARWIN_SERVO_4_SPEED = 0xBB,
- DARWIN_SERVO_5_SPEED = 0xBC,
- DARWIN_SERVO_6_SPEED = 0xBD,
- DARWIN_SERVO_7_SPEED = 0xBE,
- DARWIN_SERVO_8_SPEED = 0xBF,
- DARWIN_SERVO_9_SPEED = 0xC0,
- DARWIN_SERVO_10_SPEED = 0xC1,
- DARWIN_SERVO_11_SPEED = 0xC2,
- DARWIN_SERVO_12_SPEED = 0xC3,
- DARWIN_SERVO_13_SPEED = 0xC4,
- DARWIN_SERVO_14_SPEED = 0xC5,
- DARWIN_SERVO_15_SPEED = 0xC6,
- DARWIN_SERVO_16_SPEED = 0xC7,
- DARWIN_SERVO_17_SPEED = 0xC8,
- DARWIN_SERVO_18_SPEED = 0xC9,
- DARWIN_SERVO_19_SPEED = 0xCA,
- DARWIN_SERVO_20_SPEED = 0xCB,
- DARWIN_SERVO_21_SPEED = 0xCC,
- DARWIN_SERVO_22_SPEED = 0xCD,
- DARWIN_SERVO_23_SPEED = 0xCE,
- DARWIN_SERVO_24_SPEED = 0xCF,
- DARWIN_SERVO_25_SPEED = 0xD0,
- DARWIN_SERVO_26_SPEED = 0xD1,
- DARWIN_SERVO_0_ACCEL = 0xD2,
- DARWIN_SERVO_1_ACCEL = 0xD3,
- DARWIN_SERVO_2_ACCEL = 0xD4,
- DARWIN_SERVO_3_ACCEL = 0xD5,
- DARWIN_SERVO_4_ACCEL = 0xD6,
- DARWIN_SERVO_5_ACCEL = 0xD7,
- DARWIN_SERVO_6_ACCEL = 0xD8,
- DARWIN_SERVO_7_ACCEL = 0xD9,
- DARWIN_SERVO_8_ACCEL = 0xDA,
- DARWIN_SERVO_9_ACCEL = 0xDB,
- DARWIN_SERVO_10_ACCEL = 0xDC,
- DARWIN_SERVO_11_ACCEL = 0xDD,
- DARWIN_SERVO_12_ACCEL = 0xDE,
- DARWIN_SERVO_13_ACCEL = 0xDF,
- DARWIN_SERVO_14_ACCEL = 0xE0,
- DARWIN_SERVO_15_ACCEL = 0xE1,
- DARWIN_SERVO_16_ACCEL = 0xE2,
- DARWIN_SERVO_17_ACCEL = 0xE3,
- DARWIN_SERVO_18_ACCEL = 0xE4,
- DARWIN_SERVO_19_ACCEL = 0xE5,
- DARWIN_SERVO_20_ACCEL = 0xE6,
- DARWIN_SERVO_21_ACCEL = 0xE7,
- DARWIN_SERVO_22_ACCEL = 0xE8,
- DARWIN_SERVO_23_ACCEL = 0xE9,
- DARWIN_SERVO_24_ACCEL = 0xEA,
- DARWIN_SERVO_25_ACCEL = 0xEB,
- DARWIN_SERVO_26_ACCEL = 0xEC,
- DARWIN_JOINTS_CONTROL = 0xED,
- DARWIN_JOINTS_STATUS = 0xEE,
- DARWIN_HEAD_PAN_L = 0xEF,
- DARWIN_HEAD_PAN_H = 0xF0,
- DARWIN_HEAD_TILT_L = 0xF1,
- DARWIN_HEAD_TILT_H = 0xF2,
- DARWIN_HEAD_PAN_P = 0xF3,
- DARWIN_HEAD_PAN_I = 0xF4,
- DARWIN_HEAD_PAN_D = 0xF5,
- DARWIN_HEAD_TILT_P = 0xF6,
- DARWIN_HEAD_TILT_I = 0xF7,
- DARWIN_HEAD_TILT_D = 0xF8,
- DARWIN_HEAD_CONTROL = 0xF9,
- DARWIN_HEAD_STATUS = 0xFA,
- DARWIN_MM_CUR_PERIOD = 0xFB,
- DARWIN_GRIPPER_CONTROL = 0xFC,
- DARWIN_GRIPPER_STATUS = 0xFD,
- DARWIN_GRIPPER_FORCE_LEFT = 0xFE,
- DARWIN_GRIPPER_FORCE_RIGHT = 0xFF
-} darwin_registers;
-
-#endif
diff --git a/src/darwin_robot.cpp b/src/darwin_robot.cpp
index 25fc55adf8643add4d8cf0c188e6e12489772ba9..a3ad4e545c90890861b4a9a5f9778fd33a61c022 100644
--- a/src/darwin_robot.cpp
+++ b/src/darwin_robot.cpp
@@ -36,22 +36,33 @@ const std::string servo_names[MAX_NUM_SERVOS]={std::string("Servo0"),
std::string("Servo25"),
std::string("Servo26")};
-CDarwin_Robot::CDarwin_Robot(const std::string &name,std::string &bus_id,int bus_speed, unsigned char id)
+const led_info_t leds[GPIO_NUM_LEDS]={{DARWIN_TX_LED_CNTRL,DARWIN_TX_LED_PERIOD_L,GPIO_VALUE,GPIO_TOGGLE,GPIO_BLINK},
+ {DARWIN_RX_LED_CNTRL,DARWIN_RX_LED_PERIOD_L,GPIO_VALUE,GPIO_TOGGLE,GPIO_BLINK},
+ {DARWIN_PLAY_LED_CNTRL,DARWIN_PLAY_LED_PERIOD_L,GPIO_VALUE,GPIO_TOGGLE,GPIO_BLINK},
+ {DARWIN_EDIT_LED_CNTRL,DARWIN_EDIT_LED_PERIOD_L,GPIO_VALUE,GPIO_TOGGLE,GPIO_BLINK},
+ {DARWIN_MNG_LED_CNTRL,0x0000,GPIO_VALUE,GPIO_TOGGLE,0x00},
+ {DARWIN_AUX1_LED_CNTRL,DARWIN_AUX1_LED_COLOR_L,GPIO_VALUE_R,GPIO_TOGGLE_R,0x00},
+ {DARWIN_AUX1_LED_CNTRL,DARWIN_AUX1_LED_COLOR_L,GPIO_VALUE_G,GPIO_TOGGLE_G,0x00},
+ {DARWIN_AUX1_LED_CNTRL,DARWIN_AUX1_LED_COLOR_L,GPIO_VALUE_B,GPIO_TOGGLE_B,0x00},
+ {DARWIN_AUX2_LED_CNTRL,DARWIN_AUX2_LED_COLOR_L,GPIO_VALUE_R,GPIO_TOGGLE_R,0x00},
+ {DARWIN_AUX2_LED_CNTRL,DARWIN_AUX2_LED_COLOR_L,GPIO_VALUE_G,GPIO_TOGGLE_G,0x00},
+ {DARWIN_AUX2_LED_CNTRL,DARWIN_AUX2_LED_COLOR_L,GPIO_VALUE_B,GPIO_TOGGLE_B,0x00}};
+
+
+CDarwinRobot::CDarwinRobot(const std::string &name,std::string &bus_id,int bus_speed, unsigned char id)
{
int num_buses;
this->robot_device=NULL;
- this->current_led_value=0x00;
- this->current_pushbutton_value=0x00;
if(name!="")
{
this->robot_name=name;
- this->dyn_server=CDynamixelServer::instance();
+ this->dyn_server=CDynamixelServerFTDI::instance();
num_buses=this->dyn_server->get_num_buses();
if(num_buses>0)
{
this->dyn_server->config_bus(bus_id,bus_speed);
- this->robot_device=dyn_server->get_device(id);
+ this->robot_device=dyn_server->get_device(id,dyn_version2);
this->robot_id=id;
}
else
@@ -68,100 +79,122 @@ CDarwin_Robot::CDarwin_Robot(const std::string &name,std::string &bus_id,int bus
}
// GPIO interface
-void CDarwin_Robot::set_TxD_LED(void)
-{
- this->current_led_value|=0x08;
- this->robot_device->write_byte_register(DARWIN_LED_PANNEL,this->current_led_value);
-}
-
-void CDarwin_Robot::clear_TxD_LED(void)
-{
- this->current_led_value&=0xF7;
- this->robot_device->write_byte_register(DARWIN_LED_PANNEL,this->current_led_value);
-}
-
-void CDarwin_Robot::set_RxD_LED(void)
-{
- this->current_led_value|=0x10;
- this->robot_device->write_byte_register(DARWIN_LED_PANNEL,this->current_led_value);
-}
-
-void CDarwin_Robot::clear_RxD_LED(void)
+void CDarwinRobot::gpio_set_led(led_t led)
{
- this->current_led_value&=0xEF;
- this->robot_device->write_byte_register(DARWIN_LED_PANNEL,this->current_led_value);
+ if(this->robot_device!=NULL)
+ this->robot_device->write_byte_register(leds[led].control_reg,leds[led].value_mask);
+ else
+ throw CDarwinRobotException(_HERE_,"Invalid robot device");
}
-void CDarwin_Robot::set_LED2(void)
+void CDarwinRobot::gpio_clear_led(led_t led)
{
- this->current_led_value|=0x01;
- this->robot_device->write_byte_register(DARWIN_LED_PANNEL,this->current_led_value);
+ if(this->robot_device!=NULL)
+ this->robot_device->write_byte_register(leds[led].control_reg,0x00);
+ else
+ throw CDarwinRobotException(_HERE_,"Invalid robot device");
}
-void CDarwin_Robot::clear_LED2(void)
+void CDarwinRobot::gpio_toggle_led(led_t led)
{
- this->current_led_value&=0xFE;
- this->robot_device->write_byte_register(DARWIN_LED_PANNEL,this->current_led_value);
+ if(this->robot_device!=NULL)
+ this->robot_device->write_byte_register(leds[led].control_reg,leds[led].toggle_mask);
+ else
+ throw CDarwinRobotException(_HERE_,"Invalid robot device");
}
-void CDarwin_Robot::set_LED3(void)
+void CDarwinRobot::gpio_blink_led(led_t led,int period_ms)
{
- this->current_led_value|=0x02;
- this->robot_device->write_byte_register(DARWIN_LED_PANNEL,this->current_led_value);
-}
+ unsigned char data[3];
-void CDarwin_Robot::clear_LED3(void)
-{
- this->current_led_value&=0xFD;
- this->robot_device->write_byte_register(DARWIN_LED_PANNEL,this->current_led_value);
+ if(this->robot_device!=NULL)
+ {
+ if(led>=LED_TX && led<=LED_3)
+ {
+ data[0]=leds[led].blink_mask;// blink led
+ data[1]=period_ms%256;
+ data[2]=period_ms/256;
+ this->robot_device->write_registers(leds[led].control_reg,data,3);
+ }
+ else
+ throw CDarwinRobotException(_HERE_,"The desired LED can not be configured to blink");
+ }
+ else
+ throw CDarwinRobotException(_HERE_,"Invalid robot device");
}
-void CDarwin_Robot::set_LED4(void)
+void CDarwinRobot::gpio_aux1_color(double red, double green, double blue)
{
- this->current_led_value|=0x04;
- this->robot_device->write_byte_register(DARWIN_LED_PANNEL,this->current_led_value);
-}
+ unsigned char data[3];
+ unsigned short int color;
-void CDarwin_Robot::clear_LED4(void)
-{
- this->current_led_value&=0xFB;
- this->robot_device->write_byte_register(DARWIN_LED_PANNEL,this->current_led_value);
+ if(this->robot_device!=NULL)
+ {
+ data[0]=GPIO_COLOR;
+ /* saturate the color */
+ if(red>1.0) red=1.0;
+ else if(red<0.0) red=0.0;
+ if(green>1.0) green=1.0;
+ else if(green<0.0) green=0.0;
+ if(blue>1.0) blue=1.0;
+ else if(blue<0.0) blue=0.0;
+ color=((unsigned short int)(red*31.0))+(((unsigned short int)(green*31.0))<<5)+(((unsigned short int)(blue*31.0))<<10);
+ data[1]=color%256;
+ data[2]=color/256;
+ this->robot_device->write_registers(DARWIN_AUX1_LED_CNTRL,data,3);
+ }
+ else
+ throw CDarwinRobotException(_HERE_,"Invalid robot device");
}
-bool CDarwin_Robot::is_mode_button_pressed(void)
+void CDarwinRobot::gpio_aux2_color(double red, double green, double blue)
{
- unsigned char value;
+ unsigned char data[3];
+ unsigned short int color;
- this->robot_device->read_byte_register(DARWIN_PUSHBUTTON,&value);
-
- this->current_pushbutton_value|=value;
- if(this->current_pushbutton_value|0x01)
+ if(this->robot_device!=NULL)
{
- this->current_pushbutton_value&=0xFE;
- return true;
+ data[0]=GPIO_COLOR;
+ /* saturate the color */
+ if(red>1.0) red=1.0;
+ else if(red<0.0) red=0.0;
+ if(green>1.0) green=1.0;
+ else if(green<0.0) green=0.0;
+ if(blue>1.0) blue=1.0;
+ else if(blue<0.0) blue=0.0;
+ color=((unsigned short int)(red*31.0))+(((unsigned short int)(green*31.0))<<5)+(((unsigned short int)(blue*31.0))<<10);
+ data[1]=color%256;
+ data[2]=color/256;
+ this->robot_device->write_registers(DARWIN_AUX2_LED_CNTRL,data,3);
}
else
- return false;
+ throw CDarwinRobotException(_HERE_,"Invalid robot device");
}
-bool CDarwin_Robot::is_start_button_pressed(void)
+bool CDarwinRobot::is_button_pressed(pushbutton_t pushbutton)
{
unsigned char value;
- this->robot_device->read_byte_register(DARWIN_PUSHBUTTON,&value);
-
- this->current_pushbutton_value|=value;
- if(this->current_pushbutton_value|0x02)
+ if(this->robot_device!=NULL)
{
- this->current_pushbutton_value&=0xFD;
- return true;
+ switch(pushbutton)
+ {
+ case START_PB: this->robot_device->read_byte_register(DARWIN_START_PB_CNTRL,&value);
+ break;
+ case MODE_PB: this->robot_device->read_byte_register(DARWIN_MODE_PB_CNTRL,&value);
+ break;
+ }
+ if(value&GPIO_VALUE)
+ return true;
+ else
+ return false;
}
else
- return false;
+ throw CDarwinRobotException(_HERE_,"Invalid robot device");
}
// motion manager interface
-void CDarwin_Robot::mm_set_period(double period_ms)
+/*void CDarwinRobot::mm_set_period(double period_ms)
{
unsigned short period;
// internal time in 0|16 fixed point float format in seconds
@@ -169,7 +202,7 @@ void CDarwin_Robot::mm_set_period(double period_ms)
this->robot_device->write_word_register(DARWIN_MM_PERIOD_LOW,period);
}
-double CDarwin_Robot::mm_get_period(void)
+double CDarwinRobot::mm_get_period(void)
{
unsigned short period;
@@ -177,7 +210,7 @@ double CDarwin_Robot::mm_get_period(void)
return (((double)period)*1000.0)/65536;
}
-double CDarwin_Robot::mm_get_current_period(void)
+double CDarwinRobot::mm_get_current_period(void)
{
unsigned short period;
@@ -185,7 +218,7 @@ double CDarwin_Robot::mm_get_current_period(void)
return ((double)period)/1000.0;
}
-void CDarwin_Robot::mm_get_num_servos(int *v1_servos,int *v2_servos)
+void CDarwinRobot::mm_get_num_servos(int *v1_servos,int *v2_servos)
{
unsigned char data;
@@ -194,7 +227,7 @@ void CDarwin_Robot::mm_get_num_servos(int *v1_servos,int *v2_servos)
*v2_servos=(data>>5)&0x07;
}
-void CDarwin_Robot::mm_start(void)
+void CDarwinRobot::mm_start(void)
{
unsigned char status;
@@ -203,7 +236,7 @@ void CDarwin_Robot::mm_start(void)
this->robot_device->write_byte_register(DARWIN_MM_CONTROL,status);
}
-void CDarwin_Robot::mm_stop(void)
+void CDarwinRobot::mm_stop(void)
{
unsigned char status;
@@ -212,18 +245,18 @@ void CDarwin_Robot::mm_stop(void)
this->robot_device->write_byte_register(DARWIN_MM_CONTROL,status);
}
-void CDarwin_Robot::mm_enable_power(void)
+void CDarwinRobot::mm_enable_power(void)
{
this->robot_device->write_byte_register(DARWIN_DXL_POWER,0x01);
}
-void CDarwin_Robot::mm_disable_power(void)
+void CDarwinRobot::mm_disable_power(void)
{
this->robot_device->write_byte_register(DARWIN_DXL_POWER,0x00);
sleep(1);
}
-bool CDarwin_Robot::mm_is_power_enabled(void)
+bool CDarwinRobot::mm_is_power_enabled(void)
{
unsigned char value;
@@ -234,13 +267,13 @@ bool CDarwin_Robot::mm_is_power_enabled(void)
return false;
}
-void CDarwin_Robot::mm_enable_servo(unsigned char servo_id)
+void CDarwinRobot::mm_enable_servo(unsigned char servo_id)
{
unsigned char value;
if(servo_id>MAX_NUM_SERVOS)
{
- /* handle exceptions */
+ // handle exceptions
throw CDarwinRobotException(_HERE_,"Invalid servo identifier");
}
// get the current value
@@ -252,13 +285,13 @@ void CDarwin_Robot::mm_enable_servo(unsigned char servo_id)
this->robot_device->write_byte_register(DARWIN_MM_MODULE_EN0+servo_id/2,value);
}
-void CDarwin_Robot::mm_disable_servo(unsigned char servo_id)
+void CDarwinRobot::mm_disable_servo(unsigned char servo_id)
{
unsigned char value;
if(servo_id>MAX_NUM_SERVOS)
{
- /* handle exceptions */
+ // handle exceptions
throw CDarwinRobotException(_HERE_,"Invalid servo identifier");
}
// get the current value
@@ -270,13 +303,13 @@ void CDarwin_Robot::mm_disable_servo(unsigned char servo_id)
this->robot_device->write_byte_register(DARWIN_MM_MODULE_EN0+servo_id/2,value);
}
-bool CDarwin_Robot::mm_is_servo_enabled(unsigned char servo_id)
+bool CDarwinRobot::mm_is_servo_enabled(unsigned char servo_id)
{
unsigned char value;
if(servo_id>MAX_NUM_SERVOS)
{
- /* handle exceptions */
+ // handle exceptions
throw CDarwinRobotException(_HERE_,"Invalid servo identifier");
}
// get the current value
@@ -297,7 +330,7 @@ bool CDarwin_Robot::mm_is_servo_enabled(unsigned char servo_id)
}
}
-void CDarwin_Robot::mm_enable_balance(void)
+void CDarwinRobot::mm_enable_balance(void)
{
unsigned char status;
@@ -306,7 +339,7 @@ void CDarwin_Robot::mm_enable_balance(void)
this->robot_device->write_byte_register(DARWIN_MM_CONTROL,status);
}
-void CDarwin_Robot::mm_disable_balance(void)
+void CDarwinRobot::mm_disable_balance(void)
{
unsigned char status;
@@ -315,7 +348,7 @@ void CDarwin_Robot::mm_disable_balance(void)
this->robot_device->write_byte_register(DARWIN_MM_CONTROL,status);
}
-bool CDarwin_Robot::mm_is_balance_enabled(void)
+bool CDarwinRobot::mm_is_balance_enabled(void)
{
unsigned char value;
@@ -327,13 +360,13 @@ bool CDarwin_Robot::mm_is_balance_enabled(void)
}
-void CDarwin_Robot::mm_assign_module(unsigned char servo_id, mm_mode_t mode)
+void CDarwinRobot::mm_assign_module(unsigned char servo_id, mm_mode_t mode)
{
unsigned char value;
if(servo_id>MAX_NUM_SERVOS)
{
- /* handle exceptions */
+ // handle exceptions
throw CDarwinRobotException(_HERE_,"Invalid servo identifier");
}
// get the current value
@@ -351,7 +384,7 @@ void CDarwin_Robot::mm_assign_module(unsigned char servo_id, mm_mode_t mode)
this->robot_device->write_byte_register(DARWIN_MM_MODULE_EN0+servo_id/2,value);
}
-void CDarwin_Robot::mm_assign_module(std::string &servo,std::string &module)
+void CDarwinRobot::mm_assign_module(std::string &servo,std::string &module)
{
unsigned int i=0;
@@ -376,13 +409,13 @@ void CDarwin_Robot::mm_assign_module(std::string &servo,std::string &module)
}
}
-mm_mode_t CDarwin_Robot::mm_get_module(unsigned char servo_id)
+mm_mode_t CDarwinRobot::mm_get_module(unsigned char servo_id)
{
unsigned char value;
if(servo_id>MAX_NUM_SERVOS)
{
- /* handle exceptions */
+ // handle exceptions
throw CDarwinRobotException(_HERE_,"Invalid servo identifier");
}
// get the current value
@@ -393,7 +426,7 @@ mm_mode_t CDarwin_Robot::mm_get_module(unsigned char servo_id)
return (mm_mode_t)((value&0x70)>>4);
}
-void CDarwin_Robot::mm_load_config(std::string &filename)
+void CDarwinRobot::mm_load_config(std::string &filename)
{
darwin_config_t::servo_iterator iterator;
unsigned int i=0;
@@ -415,7 +448,7 @@ void CDarwin_Robot::mm_load_config(std::string &filename)
throw CDarwinRobotException(_HERE_,"Configuration file does not exist");
}
-std::vector<double> CDarwin_Robot::mm_get_servo_angles(void)
+std::vector<double> CDarwinRobot::mm_get_servo_angles(void)
{
int i=0;
short int values[MAX_NUM_SERVOS];
@@ -429,14 +462,14 @@ std::vector<double> CDarwin_Robot::mm_get_servo_angles(void)
return angles;
}
-double CDarwin_Robot::mm_get_servo_angle(unsigned char servo_id)
+double CDarwinRobot::mm_get_servo_angle(unsigned char servo_id)
{
unsigned short int value;
double angle;
if(servo_id>MAX_NUM_SERVOS)
{
- /* handle exceptions */
+ / handle exceptions
throw CDarwinRobotException(_HERE_,"Invalid servo identifier");
}
this->robot_device->read_word_register(DARWIN_MM_SERVO0_CUR_POS_L+servo_id/2,&value);
@@ -445,7 +478,7 @@ double CDarwin_Robot::mm_get_servo_angle(unsigned char servo_id)
return angle;
}
-void CDarwin_Robot::mm_set_balance_gains(double knee,double ankle_pitch,double hip_roll,double ankle_roll)
+void CDarwinRobot::mm_set_balance_gains(double knee,double ankle_pitch,double hip_roll,double ankle_roll)
{
this->robot_device->write_byte_register(DARWIN_MM_KNEE_GAIN,(unsigned char)(knee*64.0));
this->robot_device->write_byte_register(DARWIN_MM_ANKLE_PITCH_GAIN,(unsigned char)(ankle_pitch*64.0));
@@ -453,7 +486,7 @@ void CDarwin_Robot::mm_set_balance_gains(double knee,double ankle_pitch,double h
this->robot_device->write_byte_register(DARWIN_MM_ANKLE_ROLL_GAIN,(unsigned char)(ankle_roll*64.0));
}
-void CDarwin_Robot::mm_get_balance_gains(double *knee,double *ankle_pitch,double *hip_roll,double *ankle_roll)
+void CDarwinRobot::mm_get_balance_gains(double *knee,double *ankle_pitch,double *hip_roll,double *ankle_roll)
{
unsigned char value;
@@ -468,12 +501,12 @@ void CDarwin_Robot::mm_get_balance_gains(double *knee,double *ankle_pitch,double
}
// motion action interface
-void CDarwin_Robot::action_load_page(unsigned char page_id)
+void CDarwinRobot::action_load_page(unsigned char page_id)
{
this->robot_device->write_byte_register(DARWIN_ACTION_PAGE,page_id);
}
-unsigned char CDarwin_Robot::action_get_current_page(void)
+unsigned char CDarwinRobot::action_get_current_page(void)
{
unsigned char page_id;
@@ -482,17 +515,17 @@ unsigned char CDarwin_Robot::action_get_current_page(void)
return page_id;
}
-void CDarwin_Robot::action_start(void)
+void CDarwinRobot::action_start(void)
{
this->robot_device->write_byte_register(DARWIN_ACTION_CONTROL,0x01);
}
-void CDarwin_Robot::action_stop(void)
+void CDarwinRobot::action_stop(void)
{
this->robot_device->write_byte_register(DARWIN_ACTION_CONTROL,0x02);
}
-bool CDarwin_Robot::action_is_page_running(void)
+bool CDarwinRobot::action_is_page_running(void)
{
unsigned char status;
@@ -504,7 +537,7 @@ bool CDarwin_Robot::action_is_page_running(void)
}
// imu interface
-void CDarwin_Robot::imu_start(void)
+void CDarwinRobot::imu_start(void)
{
unsigned char status;
@@ -513,7 +546,7 @@ void CDarwin_Robot::imu_start(void)
this->robot_device->write_byte_register(DARWIN_IMU_CONTROL,status);
}
-void CDarwin_Robot::imu_stop(void)
+void CDarwinRobot::imu_stop(void)
{
unsigned char status;
@@ -522,7 +555,7 @@ void CDarwin_Robot::imu_stop(void)
this->robot_device->write_byte_register(DARWIN_IMU_CONTROL,status);
}
-void CDarwin_Robot::imu_start_gyro_cal(void)
+void CDarwinRobot::imu_start_gyro_cal(void)
{
unsigned char status;
@@ -531,7 +564,7 @@ void CDarwin_Robot::imu_start_gyro_cal(void)
this->robot_device->write_byte_register(DARWIN_IMU_CONTROL,status);
}
-bool CDarwin_Robot::imu_is_gyro_cal_done(void)
+bool CDarwinRobot::imu_is_gyro_cal_done(void)
{
unsigned char status;
@@ -542,7 +575,7 @@ bool CDarwin_Robot::imu_is_gyro_cal_done(void)
return false;
}
-bool CDarwin_Robot::imu_is_accel_detected(void)
+bool CDarwinRobot::imu_is_accel_detected(void)
{
unsigned char status;
@@ -553,7 +586,7 @@ bool CDarwin_Robot::imu_is_accel_detected(void)
return false;
}
-bool CDarwin_Robot::imu_is_gyro_detected(void)
+bool CDarwinRobot::imu_is_gyro_detected(void)
{
unsigned char status;
@@ -564,7 +597,7 @@ bool CDarwin_Robot::imu_is_gyro_detected(void)
return false;
}
-void CDarwin_Robot::imu_get_accel_data(short int *x,short int *y,short int *z)
+void CDarwinRobot::imu_get_accel_data(short int *x,short int *y,short int *z)
{
unsigned char data[6];
@@ -575,7 +608,7 @@ void CDarwin_Robot::imu_get_accel_data(short int *x,short int *y,short int *z)
*z=data[4]+(data[5]<<8);
}
-void CDarwin_Robot::imu_get_gyro_data(short int *x,short int *y,short int *z)
+void CDarwinRobot::imu_get_gyro_data(short int *x,short int *y,short int *z)
{
unsigned char data[6];
@@ -587,7 +620,7 @@ void CDarwin_Robot::imu_get_gyro_data(short int *x,short int *y,short int *z)
}
// adc interface
-void CDarwin_Robot::adc_get_data(void)
+void CDarwinRobot::adc_get_data(void)
{
unsigned char data[27];
int i=0;
@@ -599,24 +632,24 @@ void CDarwin_Robot::adc_get_data(void)
}
// walking interface
-void CDarwin_Robot::walking_set_speeds(double fw_step,double side_step,double turn)
+void CDarwinRobot::walking_set_speeds(double fw_step,double side_step,double turn)
{
this->robot_device->write_byte_register(DARWIN_STEP_FW_BW,(unsigned char)fw_step);
this->robot_device->write_byte_register(DARWIN_STEP_LEFT_RIGHT,(unsigned char)side_step);
this->robot_device->write_byte_register(DARWIN_STEP_DIRECTION,(unsigned char)(turn*8.0));
}
-void CDarwin_Robot::walking_start(void)
+void CDarwinRobot::walking_start(void)
{
this->robot_device->write_byte_register(DARWIN_WALK_CONTROL,0x01);
}
-void CDarwin_Robot::walking_stop(void)
+void CDarwinRobot::walking_stop(void)
{
this->robot_device->write_byte_register(DARWIN_WALK_CONTROL,0x02);
}
-bool CDarwin_Robot::is_walking(void)
+bool CDarwinRobot::is_walking(void)
{
unsigned char status;
@@ -627,7 +660,7 @@ bool CDarwin_Robot::is_walking(void)
return false;
}
-void CDarwin_Robot::walking_set_param(int param_id,double value)
+void CDarwinRobot::walking_set_param(int param_id,double value)
{
switch(param_id)
{
@@ -664,7 +697,7 @@ void CDarwin_Robot::walking_set_param(int param_id,double value)
}
}
-double CDarwin_Robot::walking_get_param(int param_id)
+double CDarwinRobot::walking_get_param(int param_id)
{
unsigned char byte_value;
unsigned short int word_value;
@@ -715,7 +748,7 @@ double CDarwin_Robot::walking_get_param(int param_id)
}
// joints interface
-void CDarwin_Robot::joints_load(std::vector<int> &servos,std::vector<double> &angles,std::vector<double> &speeds,std::vector<double> &accels)
+void CDarwinRobot::joints_load(std::vector<int> &servos,std::vector<double> &angles,std::vector<double> &speeds,std::vector<double> &accels)
{
unsigned int i=0;
@@ -727,17 +760,17 @@ void CDarwin_Robot::joints_load(std::vector<int> &servos,std::vector<double> &an
}
}
-void CDarwin_Robot::joints_start(void)
+void CDarwinRobot::joints_start(void)
{
this->robot_device->write_byte_register(DARWIN_JOINTS_CONTROL,0x01);
}
-void CDarwin_Robot::joints_stop(void)
+void CDarwinRobot::joints_stop(void)
{
this->robot_device->write_byte_register(DARWIN_JOINTS_CONTROL,0x02);
}
-bool CDarwin_Robot::are_joints_moving(void)
+bool CDarwinRobot::are_joints_moving(void)
{
unsigned char status;
@@ -749,37 +782,37 @@ bool CDarwin_Robot::are_joints_moving(void)
}
// head tracking interface
-void CDarwin_Robot::head_tracking_set_target(double pan,double tilt)
+void CDarwinRobot::head_tracking_set_target(double pan,double tilt)
{
this->robot_device->write_word_register(DARWIN_HEAD_PAN_L,(unsigned short int)(pan*128.0));
this->robot_device->write_word_register(DARWIN_HEAD_TILT_L,(unsigned short int)(tilt*128.0));
}
-void CDarwin_Robot::head_tracking_set_pan_pid(double p,double i,double d)
+void CDarwinRobot::head_tracking_set_pan_pid(double p,double i,double d)
{
this->robot_device->write_byte_register(DARWIN_HEAD_PAN_P,(unsigned char)(p*1024.0));
this->robot_device->write_byte_register(DARWIN_HEAD_PAN_I,(unsigned char)(i*1024.0));
this->robot_device->write_byte_register(DARWIN_HEAD_PAN_D,(unsigned char)(d*1024.0));
}
-void CDarwin_Robot::head_tracking_set_tilt_pid(double p,double i,double d)
+void CDarwinRobot::head_tracking_set_tilt_pid(double p,double i,double d)
{
this->robot_device->write_byte_register(DARWIN_HEAD_TILT_P,(unsigned char)(p*1024.0));
this->robot_device->write_byte_register(DARWIN_HEAD_TILT_I,(unsigned char)(i*1024.0));
this->robot_device->write_byte_register(DARWIN_HEAD_TILT_D,(unsigned char)(d*1024.0));
}
-void CDarwin_Robot::head_tracking_start(void)
+void CDarwinRobot::head_tracking_start(void)
{
this->robot_device->write_byte_register(DARWIN_HEAD_CONTROL,0x01);
}
-void CDarwin_Robot::head_tracking_stop(void)
+void CDarwinRobot::head_tracking_stop(void)
{
this->robot_device->write_byte_register(DARWIN_HEAD_CONTROL,0x00);
}
-bool CDarwin_Robot::is_head_tracking(void)
+bool CDarwinRobot::is_head_tracking(void)
{
unsigned char data;
@@ -790,8 +823,8 @@ bool CDarwin_Robot::is_head_tracking(void)
return true;
}
-/* gripper interface */
-void CDarwin_Robot::gripper_open(grippers_t gripper)
+/ gripper interface
+void CDarwinRobot::gripper_open(grippers_t gripper)
{
if(gripper==LEFT_GRIPPER)
this->robot_device->write_byte_register(DARWIN_GRIPPER_CONTROL,0x01);
@@ -799,15 +832,15 @@ void CDarwin_Robot::gripper_open(grippers_t gripper)
this->robot_device->write_byte_register(DARWIN_GRIPPER_CONTROL,0x04);
}
-void CDarwin_Robot::gripper_close(grippers_t gripper)
+void CDarwinRobot::gripper_close(grippers_t gripper)
{
if(gripper==LEFT_GRIPPER)
this->robot_device->write_byte_register(DARWIN_GRIPPER_CONTROL,0x02);
else
this->robot_device->write_byte_register(DARWIN_GRIPPER_CONTROL,0x08);
-}
+}*/
-CDarwin_Robot::~CDarwin_Robot()
+CDarwinRobot::~CDarwinRobot()
{
if(this->robot_device!=NULL)
{
diff --git a/src/darwin_robot.h b/src/darwin_robot.h
index af2204bd295d6628d06b35448d4bd2f79f9a39d6..3bb810ae47edc11a3caf678f9064daf72a501ed2 100644
--- a/src/darwin_robot.h
+++ b/src/darwin_robot.h
@@ -2,41 +2,54 @@
#define _DARWIN_ROBOT_H
#include "dynamixel.h"
+#include "dynamixelserver_ftdi.h"
#include "darwin_registers.h"
-extern const std::string servo_names[MAX_NUM_SERVOS];
+#define MAX_NUM_SERVOS 31
+extern const std::string servo_names[MAX_NUM_SERVOS];
+/* available motion modules */
typedef enum {DARWIN_MM_NONE=0x00,DARWIN_MM_ACTION=0x01,DARWIN_MM_WALKING=0x02,DARWIN_MM_JOINTS=0x03,DARWIN_MM_HEAD=0x04,DARWIN_MM_GRIPPER=0x05} mm_mode_t;
-
+/* available grippers */
typedef enum {LEFT_GRIPPER=0,RIGHT_GRIPPER=1} grippers_t;
+/* available leds */
+#define GPIO_NUM_LEDS 11
+
+typedef enum {LED_TX=0,LED_RX=1,LED_2=2,LED_3=3,LED_4=4,LED_5_R=5,LED_5_G=6,LED_5_B=7,LED_6_R=8,LED_6_G=9,LED_6_B=10} led_t;
+
+typedef struct
+{
+ unsigned short int control_reg;
+ unsigned short int data_reg;
+ unsigned char value_mask;
+ unsigned char toggle_mask;
+ unsigned char blink_mask;
+}led_info_t;
+
+/* available pushbuttons */
+#define GPIO_NUM_PB 2
+
+typedef enum {START_PB=0,MODE_PB=1} pushbutton_t;
-class CDarwin_Robot
+class CDarwinRobot
{
private:
- CDynamixelServer *dyn_server;
+ CDynamixelServerFTDI *dyn_server;
CDynamixel *robot_device;
std::string robot_name;
unsigned char robot_id;
- // GPIO variables
- unsigned char current_led_value;
- unsigned char current_pushbutton_value;
// motion manager variables
unsigned char num_servos;
public:
- CDarwin_Robot(const std::string &name,std::string &bus_id,int bus_speed, unsigned char id);
+ CDarwinRobot(const std::string &name,std::string &bus_id,int bus_speed, unsigned char id);
// GPIO interface
- void set_TxD_LED(void);
- void clear_TxD_LED(void);
- void set_RxD_LED(void);
- void clear_RxD_LED(void);
- void set_LED2(void);
- void clear_LED2(void);
- void set_LED3(void);
- void clear_LED3(void);
- void set_LED4(void);
- void clear_LED4(void);
- bool is_mode_button_pressed(void);
- bool is_start_button_pressed(void);
+ void gpio_set_led(led_t led);
+ void gpio_clear_led(led_t led);
+ void gpio_toggle_led(led_t led);
+ void gpio_blink_led(led_t led, int period_ms);
+ void gpio_aux1_color(double red, double green, double blue);
+ void gpio_aux2_color(double red, double green, double blue);
+ bool is_button_pressed(pushbutton_t pushbutton);
// motion manager interface
void mm_set_period(double period_ms);
double mm_get_period(void);
@@ -100,7 +113,7 @@ class CDarwin_Robot
// gripper interface
void gripper_open(grippers_t gripper);
void gripper_close(grippers_t gripper);
- ~CDarwin_Robot();
+ ~CDarwinRobot();
};
#endif
diff --git a/src/examples/CMakeLists.txt b/src/examples/CMakeLists.txt
index a1ae3a682b81c8bc90a18b78032415cbb9c345aa..72a43c17331bff04616c7f278776dba807e4297f 100644
--- a/src/examples/CMakeLists.txt
+++ b/src/examples/CMakeLists.txt
@@ -1,39 +1,44 @@
# create an example application
-ADD_EXECUTABLE(darwin_robot_test darwin_robot_test.cpp)
+#ADD_EXECUTABLE(darwin_robot_test darwin_robot_test.cpp)
# link necessary libraries
-TARGET_LINK_LIBRARIES(darwin_robot_test darwin_robot)
+#TARGET_LINK_LIBRARIES(darwin_robot_test darwin_robot)
# create an example application
-ADD_EXECUTABLE(darwin_joint_motion_test darwin_joint_motion_test.cpp)
+ADD_EXECUTABLE(darwin_gpio_test darwin_gpio_test.cpp)
# link necessary libraries
-TARGET_LINK_LIBRARIES(darwin_joint_motion_test darwin_robot)
+TARGET_LINK_LIBRARIES(darwin_gpio_test darwin_robot)
# create an example application
-ADD_EXECUTABLE(darwin_action_test darwin_action_test.cpp)
+#ADD_EXECUTABLE(darwin_joint_motion_test darwin_joint_motion_test.cpp)
# link necessary libraries
-TARGET_LINK_LIBRARIES(darwin_action_test darwin_robot)
+#TARGET_LINK_LIBRARIES(darwin_joint_motion_test darwin_robot)
# create an example application
-ADD_EXECUTABLE(darwin_walking_test darwin_walking_test.cpp)
+#ADD_EXECUTABLE(darwin_action_test darwin_action_test.cpp)
# link necessary libraries
-TARGET_LINK_LIBRARIES(darwin_walking_test darwin_robot)
+#TARGET_LINK_LIBRARIES(darwin_action_test darwin_robot)
# create an example application
-ADD_EXECUTABLE(darwin_imu_test darwin_imu_test.cpp)
+#ADD_EXECUTABLE(darwin_walking_test darwin_walking_test.cpp)
# link necessary libraries
-TARGET_LINK_LIBRARIES(darwin_imu_test darwin_robot)
+#TARGET_LINK_LIBRARIES(darwin_walking_test darwin_robot)
# create an example application
-ADD_EXECUTABLE(darwin_head_tracking_test darwin_head_tracking_test.cpp)
+#ADD_EXECUTABLE(darwin_imu_test darwin_imu_test.cpp)
# link necessary libraries
-TARGET_LINK_LIBRARIES(darwin_head_tracking_test darwin_robot)
+#TARGET_LINK_LIBRARIES(darwin_imu_test darwin_robot)
# create an example application
-ADD_EXECUTABLE(darwin_kin darwin_kin.cpp)
+#ADD_EXECUTABLE(darwin_head_tracking_test darwin_head_tracking_test.cpp)
# link necessary libraries
-TARGET_LINK_LIBRARIES(darwin_kin darwin_arm_kinematics)
+#TARGET_LINK_LIBRARIES(darwin_head_tracking_test darwin_robot)
# create an example application
-ADD_EXECUTABLE(darwin_gripper_test darwin_gripper_test.cpp)
+#ADD_EXECUTABLE(darwin_kin darwin_kin.cpp)
# link necessary libraries
-TARGET_LINK_LIBRARIES(darwin_gripper_test darwin_robot)
+#TARGET_LINK_LIBRARIES(darwin_kin darwin_arm_kinematics)
+
+# create an example application
+#ADD_EXECUTABLE(darwin_gripper_test darwin_gripper_test.cpp)
+# link necessary libraries
+#TARGET_LINK_LIBRARIES(darwin_gripper_test darwin_robot)
diff --git a/src/examples/darwin_action_test.cpp b/src/examples/darwin_action_test.cpp
index d9179f9a9481b13d01c2d49fa6e383b3275f04c1..465e55da0bf9dc6d204c4b6f62de5b9f49e3feca 100644
--- a/src/examples/darwin_action_test.cpp
+++ b/src/examples/darwin_action_test.cpp
@@ -10,7 +10,7 @@ int main(int argc, char *argv[])
int i=0,num_v1_servos,num_v2_servos;
try{
- CDarwin_Robot darwin("Darwin",robot_device,1000000,0xC0);
+ CDarwinRobot darwin("Darwin",robot_device,1000000,0xC0);
darwin.mm_get_num_servos(&num_v1_servos,&num_v2_servos);
std::cout << "Found " << num_v1_servos << " v1 servos and " << num_v2_servos << " v2 servos" << std::endl;
diff --git a/src/examples/darwin_gpio_test.cpp b/src/examples/darwin_gpio_test.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..d03307378be43a245ecbf69ae9ea6a299c1c0068
--- /dev/null
+++ b/src/examples/darwin_gpio_test.cpp
@@ -0,0 +1,25 @@
+#include "darwin_robot.h"
+#include "darwin_robot_exceptions.h"
+
+#include <iostream>
+
+//std::string robot_device="A603LOBS";
+std::string robot_device="A4008atn";
+
+int main(int argc, char *argv[])
+{
+ try{
+ CDarwinRobot darwin("Darwin",robot_device,926100,0x02);
+ std::cout << "found darwin controller" << std::endl;
+ darwin.gpio_blink_led(LED_TX,1000);
+ darwin.gpio_blink_led(LED_RX,2000);
+ darwin.gpio_blink_led(LED_2,4000);
+ darwin.gpio_blink_led(LED_3,8000);
+ darwin.gpio_set_led(LED_4);
+ darwin.gpio_aux1_color(1.0,1.0,1.0);
+ darwin.gpio_aux2_color(0.5,0.5,0.5);
+ sleep(10);
+ }catch(CException &e){
+ std::cout << e.what() << std::endl;
+ }
+}
diff --git a/src/examples/darwin_gripper_test.cpp b/src/examples/darwin_gripper_test.cpp
index bc44b01e72c8db1ab73197309954af5af2835ef4..90b49a70ee9ef4bb5fc0f000d48475028540b985 100644
--- a/src/examples/darwin_gripper_test.cpp
+++ b/src/examples/darwin_gripper_test.cpp
@@ -11,7 +11,7 @@ int main(int argc, char *argv[])
std::vector<double> angles;
try{
- CDarwin_Robot darwin("Darwin",robot_device,1000000,0xC0);
+ CDarwinRobot darwin("Darwin",robot_device,1000000,0xC0);
darwin.mm_get_num_servos(&num_v1_servos,&num_v2_servos);
std::cout << "Found " << num_v1_servos << " v1 servos and " << num_v2_servos << " v2 servos" << std::endl;
darwin.mm_enable_power();
diff --git a/src/examples/darwin_head_tracking_test.cpp b/src/examples/darwin_head_tracking_test.cpp
index 27850e8300b9e24418be47073f718a1924635a17..8fc919f4ba6535c29242cc61f11a530357dcae2a 100644
--- a/src/examples/darwin_head_tracking_test.cpp
+++ b/src/examples/darwin_head_tracking_test.cpp
@@ -12,7 +12,7 @@ int main(int argc, char *argv[])
std::vector<double> angles,speeds,accels;
try{
- CDarwin_Robot darwin("Darwin",robot_device,1000000,0xC0);
+ CDarwinRobot darwin("Darwin",robot_device,1000000,0xC0);
darwin.mm_get_num_servos(&num_v1_servos,&num_v2_servos);
std::cout << "Found " << num_v1_servos << " v1 servos and " << num_v2_servos << " v2 servos" << std::endl;
diff --git a/src/examples/darwin_imu_test.cpp b/src/examples/darwin_imu_test.cpp
index 0019c46d79e7eec0e6378ab24ed3289489252189..7ac428d3b5616aa262046ca976870cfd88761846 100644
--- a/src/examples/darwin_imu_test.cpp
+++ b/src/examples/darwin_imu_test.cpp
@@ -13,7 +13,7 @@ int main(int argc, char *argv[])
short int gyro_x,gyro_y,gyro_z;
try{
- CDarwin_Robot darwin("Darwin",robot_device,1000000,0xC0);
+ CDarwinRobot darwin("Darwin",robot_device,1000000,0xC0);
darwin.mm_get_num_servos(&num_v1_servos,&num_v2_servos);
std::cout << "Found " << num_v1_servos << " v1 servos and " << num_v2_servos << " v2 servos" << std::endl;
darwin.mm_enable_power();
diff --git a/src/examples/darwin_joint_motion_test.cpp b/src/examples/darwin_joint_motion_test.cpp
index fce075f23d131134644b55c81e17949ac2d19b3f..ab4de08b33f4902c85c5030bdc2bf0fad7ae5955 100644
--- a/src/examples/darwin_joint_motion_test.cpp
+++ b/src/examples/darwin_joint_motion_test.cpp
@@ -12,7 +12,7 @@ int main(int argc, char *argv[])
std::vector<double> angles,speeds,accels;
try{
- CDarwin_Robot darwin("Darwin",robot_device,1000000,0xC0);
+ CDarwinRobot darwin("Darwin",robot_device,1000000,0xC0);
darwin.mm_get_num_servos(&num_v1_servos,&num_v2_servos);
std::cout << "Found " << num_v1_servos << " v1 servos and " << num_v2_servos << " v2 servos" << std::endl;
darwin.mm_enable_power();
diff --git a/src/examples/darwin_robot_test.cpp b/src/examples/darwin_robot_test.cpp
index 0c0e7383010d68325fe12996c02e79b706a31fcb..907003eabd12f495a631f8f566d7f858faf72e84 100644
--- a/src/examples/darwin_robot_test.cpp
+++ b/src/examples/darwin_robot_test.cpp
@@ -13,7 +13,7 @@ int main(int argc, char *argv[])
std::vector<double> angles,speeds,accels;
try{
- CDarwin_Robot darwin("Darwin",robot_device,1000000,0xC0);
+ CDarwinRobot darwin("Darwin",robot_device,1000000,0xC0);
darwin.mm_load_config(config_file);
darwin.mm_get_num_servos(&num_v1_servos,&num_v2_servos);
std::cout << "Found " << num_v1_servos << " v1 servos and " << num_v2_servos << " v2 servos" << std::endl;