void firmware_init(){
// Initialize firmware
dxl_initialize(0,1); //initialize dynamixel communication
serial_initialize(57600); //initialize serial communication
//initialize sensors
sensor_init(SENSOR_FRONT,SENSOR_DISTANCE);
sensor_init(SENSOR_FRONTLEFT,SENSOR_IR);
sensor_init(SENSOR_FRONTRIGHT,SENSOR_IR);
sensor_init(SENSOR_BACKLEFT,SENSOR_IR);
sensor_init(SENSOR_BACKRIGHT,SENSOR_IR);
//initialize I/O
io_init();
io_set_interrupt(BTN_START, &reset_state); //assign callback function when start button is pressed
// Activate general interrupts
sei();
// Set motors to wheel mode
motor_set_mode(254, MOTOR_WHEEL_MODE);
// Set serial communication through ZigBee
serial_set_zigbee();
}
void Dynamixel::begin(int baud) {
//TxDString("[DXL]start begin\r\n");
afio_remap(AFIO_REMAP_USART1);//USART1 -> DXL
afio_cfg_debug_ports(AFIO_DEBUG_FULL_SWJ_NO_NJRST);
#ifdef BOARD_CM900 //Engineering version case
gpio_set_mode(PORT_ENABLE_TXD, PIN_ENABLE_TXD, GPIO_OUTPUT_PP);
gpio_set_mode(PORT_ENABLE_RXD, PIN_ENABLE_RXD, GPIO_OUTPUT_PP);
gpio_write_bit(PORT_ENABLE_TXD, PIN_ENABLE_TXD, 0 );// TX Disable
gpio_write_bit(PORT_ENABLE_RXD, PIN_ENABLE_RXD, 1 );// RX Enable
#else
gpio_set_mode(PORT_TXRX_DIRECTION, PIN_TXRX_DIRECTION, GPIO_OUTPUT_PP);
gpio_write_bit(PORT_TXRX_DIRECTION, PIN_TXRX_DIRECTION, 0 );// RX Enable
#endif
timer_set_mode(TIMER2, TIMER_CH1, TIMER_OUTPUT_COMPARE);
timer_pause(TIMER2);
uint16 ovf = timer_get_reload(TIMER2);
timer_set_count(TIMER2, min(0, ovf));
timer_set_reload(TIMER2, 30000);//set overflow
ovf = timer_get_reload(TIMER2);
timer_set_compare(TIMER2, TIMER_CH1, min(1000, ovf));
timer_attach_interrupt(TIMER2, TIMER_CH1, TIM2_IRQHandler);
timer_generate_update(TIMER2);
timer_resume(TIMER2);
dxl_initialize(0, baud);
}
bool DXLJointInterface::init()
{
if(!dxl_initialize(0, BAUD_NUM))
return false;
dxl_write_byte(200, 24, 1);
// sleep(1);
// dxl_ping(id());
// if(dxl_get_result() != COMM_RXSUCCESS)
// {
// ROS_ERROR("Could not find dynamixel with ID %d", id());
// return false;
// }
dxl_write_word(id(), REG_TORQUE_LIMIT, 1023);
dxl_write_word(id(), REG_CW_ANGLE_LIMIT, 0);
setControlType(CT_POSITION);
dxl_write_word(id(), REG_TORQUE_ENABLE, 1);
dxl_write_byte(id(), REG_P_GAIN, 2);
dxl_write_byte(id(), REG_I_GAIN, 0);
dxl_write_byte(id(), REG_D_GAIN, 0);
return true;
}
/**
* Initialize the servos
*/
void legsInit(void){
dxl_initialize( 0, DEFAULT_BAUDNUM ); // Not using device index
sei(); // Interrupt Enable
// Set speed of dynamixels
dxl_write_word( BROADCAST_ID, 32, 1000);
}
void Dynamixel::begin(int baud) {
//TxDString("[DXL]start begin\r\n");
afio_remap(AFIO_REMAP_USART1);//USART1 -> DXL
afio_cfg_debug_ports(AFIO_DEBUG_FULL_SWJ_NO_NJRST);
#ifdef BOARD_CM900 //Engineering version case
gpio_set_mode(PORT_ENABLE_TXD, PIN_ENABLE_TXD, GPIO_OUTPUT_PP);
gpio_set_mode(PORT_ENABLE_RXD, PIN_ENABLE_RXD, GPIO_OUTPUT_PP);
gpio_write_bit(PORT_ENABLE_TXD, PIN_ENABLE_TXD, 0 );// TX Disable
gpio_write_bit(PORT_ENABLE_RXD, PIN_ENABLE_RXD, 1 );// RX Enable
#else
gpio_set_mode(PORT_TXRX_DIRECTION, PIN_TXRX_DIRECTION, GPIO_OUTPUT_PP);
gpio_write_bit(PORT_TXRX_DIRECTION, PIN_TXRX_DIRECTION, 0 );// RX Enable
#endif
/* timer_set_mode(TIMER2, TIMER_CH1, TIMER_OUTPUT_COMPARE);
timer_pause(TIMER2);
uint16 ovf = timer_get_reload(TIMER2);
timer_set_count(TIMER2, min(0, ovf));
timer_set_reload(TIMER2, 10000);//set overflow
ovf = timer_get_reload(TIMER2);
timer_set_compare(TIMER2, TIMER_CH1, min(1000, ovf));
timer_attach_interrupt(TIMER2, TIMER_CH1, TIM2_IRQHandler);
timer_generate_update(TIMER2);
//timer_resume(TIMER2);*/
/*
* Timer Configuation for Dynamixel bus
* 2013-04-03 ROBOTIS Changed it as the below codes
* Dynamixel bus used timer 2(channel 1) to check timeout for receiving data from the bus.
* So, don't use time 2(channel 1) in other parts.
* */
// Pause the timer while we're configuring it
timer.pause();
// Set up period
timer.setPeriod(1); // in microseconds
// Set up an interrupt on channel 1
timer.setMode(TIMER_CH1,TIMER_OUTPUT_COMPARE);
timer.setCompare(TIMER_CH1, 1); // Interrupt 1 count after each update
timer.attachInterrupt(TIMER_CH1,TIM2_IRQHandler);
// Refresh the timer's count, prescale, and overflow
timer.refresh();
// Start the timer counting
//timer.resume();
dxl_initialize(0, baud);
}
int open_USB2Dyn(const int baudnum,const int deviceIndex){
if( dxl_initialize(deviceIndex, baudnum) == 0 )
{
printf( "Failed to open USB2Dynamixel!\n" );
return 0;
}
else
printf( "Succeed to open USB2Dynamixel!\n" );
return 1;
}
void MotorInit(void){
dxl_initialize( 0, DEFAULT_BAUDNUM ); // Not using device index
//Wheel Mode
// dxl_write_word( 31, P_CW_ANGLE_LIMIT_L, 0 );
// dxl_write_word( 31, P_CCW_ANGLE_LIMIT_L, 0 );
//Set EEP Lock
dxl_write_word( 31, P_EEP_LOCK, 1 );
// Set goal speed
dxl_write_word( BROADCAST_ID, P_GOAL_SPEED_L, 0 );
dxl_write_byte( BROADCAST_ID, P_TORQUE_ENABLE, 0 );
_delay_ms(1000);
}
int main()
{
int baudnum = 1;
int deviceIndex = 0;
int PresentPos;
int CommStatus;
printf( "\n\nRead/Write example for Linux\n\n" );
///////// Open USB2Dynamixel ////////////
if( dxl_initialize(deviceIndex, baudnum) == 0 )
{
printf( "Failed to open USB2Dynamixel!\n" );
printf( "Press Enter key to terminate...\n" );
getchar();
return 0;
}
else
printf( "Succeed to open USB2Dynamixel!\n" );
dxl_write_byte( DEFAULT_ID, P_TORQUE_ENABLE, 0 );
while(1)
{
printf( "Press Enter key to continue!(press ESC and Enter to quit)\n" );
if(getchar() == 0x1b)
break;
do
{
// Read present position
PresentPos = dxl_read_word( DEFAULT_ID, P_PRESENT_POSITION_L );
CommStatus = dxl_get_result();
if( CommStatus == COMM_RXSUCCESS )
{
printf( "Position: %d\n", PresentPos );
PrintErrorCode();
}
else
{
PrintCommStatus(CommStatus);
break;
}
} while(1);
}
// Close device
dxl_terminate();
printf( "Press Enter key to terminate...\n" );
getchar();
return 0;
}
int main(void)
{
/* System Clocks Configuration */
RCC_Configuration();
/* NVIC configuration */
NVIC_Configuration();
/* GPIO configuration */
GPIO_Configuration();
SysTick_Configuration();
Timer_Configuration();
dxl_initialize( 0, 1 );
USART_Configuration(USART_PC, Baudrate_PC);
while(1)
{
bMoving = dxl_read_byte( id, P_MOVING );
CommStatus = dxl_get_result();
if( CommStatus == COMM_RXSUCCESS )
{
if( bMoving == 0 )
{
// Change goal position
if( INDEX == 0 )
INDEX = 1;
else
INDEX = 0;
// Write goal position
dxl_write_word( id, P_GOAL_POSITION_L, GoalPos[INDEX] );
}
PrintErrorCode();
// Read present position
wPresentPos = dxl_read_word( id, P_PRESENT_POSITION_L );
TxDWord16(GoalPos[INDEX]);
TxDString(" ");
TxDWord16(wPresentPos);
TxDByte_PC('\r');
TxDByte_PC('\n');
}
else
PrintCommStatus(CommStatus);
}
return 0;
}
int open_dxl(int deviceIndex, int baudnum) {
//int baudnum = DEFAULT_BAUDNUM;
//printf( "\n\nRead/Write example for Linux\n\n" );
///////// Open USB2Dynamixel ////////////
if( dxl_initialize(deviceIndex, baudnum) == 0 )
{
//printf( "Failed to open USB2Dynamixel via /dev/ttyUSB%d!\n",deviceIndex );
return 0;
}
else
//printf( "Succeeded in opening /dev/ttyUSB2%d\n", deviceIndex);
return 1;
}
Dxl::Dxl(unsigned int device_index, unsigned int baudrate)
: device_index(device_index), baudrate(baudrate)
{
if(!dxl_initialize(device_index, baudrate))
{
// Error
std::stringstream ss;
ss << "dxl_initialize() failed."<< std::endl;
}
else
{
std::cout << "Connection to DXL device is ok." << std::endl;
}
}
int main(void)
{
//int Value = 0;
DDRC = 0x7F;
PORTC = 0x7D;
serial_initialize(57600); // USART Initialize
//serial_initialize(250000); // USART Initialize
dxl_initialize( 0, 7 ); // Not using device index
sei();
SetUpServos();
SetConstants();
SetKConstants();
printf("ARMS CONTROL - Left \r" );
printf("Ver1.0 - Last Moddification 23-03-15\r" );
while (1)
{
if(cmdReady)
{
if(!execCommand()) printf("Unknown command\r");
}
if(doLoop)
{
executeControl();
count = count + 100;
}
if(doGripper)
{
executeGripper();
}
_delay_ms(3);
Leds();
}
return 1;
}
ControlUtils::ControlUtils()
{
vec_set(joints, 0., TOTAL_JOINTS);
vec_set(jointsd, 0., TOTAL_JOINTS);
vec_set(joints_d, 0., TOTAL_JOINTS);
if( dxl_initialize(0, 1) == 0 )
{
printf( "Failed to open USB2Dynamixel!\n" );
printf( "Press Enter key to terminate...\n" );
getchar();
exit(-1);
}
else
printf( "Succeed to open USB2Dynamixel!\n" );
_legIdx = 0;
}
int main(){
//portInit();
serial_initialize(57600);
dxl_initialize( 0, 1 ); // init with baud = 1 Mbps
ADCInit();
sei(); //Enables global interrupts
unsigned int distanceLeft, distanceRight, front, movingLeft, movingRight;
signed int speedLeft, speedRight;
while(1) {
//printf("%d %d\n\n",getSensorValue(3),getSensorValue(4));
//_delay_ms(1000);
// Get sensor reading (in cm)
front = DMSDistance(getSensorValue(1));
distanceLeft = IRValue(getSensorValue(3));
distanceRight = IRValue(getSensorValue(4));
// Calculating the required speed
speedRight = (int)((front-40*distanceLeft));
speedLeft = (int)((front-40*distanceRight));
movingLeft = dxl_read_byte( 1, 38 );
movingRight = dxl_read_byte(2,38);
if(movingLeft < 15 && movingRight < 15){
wheel(1,0);
wheel(2,-20);
_delay_ms(1000);
}else{
wheel(1,-speedRight);
wheel(2,speedLeft);
}
printf("%d %d - %d\n\n",speedLeft,speedRight,getSensorValue(1));
//_delay_ms(1000);
// Making the wheels spin
}
return 0;
}
bool dynamixelApi_setup(int baud, void (*msgHandler)(char*, char, char)) {
EventManager_registerTopic(DYNAMIXEL_ERROR_EVENT);
int res = dxl_initialize();
if (res == 1) {
ase_printf("SUCCESS: USB2Dynamixel opened!\n");
dynamixelApi_setBaud(baud);
dyna.msgHandler = msgHandler;
dyna.logPos = false;
EventManager_registerTopic(DYNAMIXEL_ROTATION_DONE_EVENT);
dyna.updateTimer = TimerManager_createPeriodicTimer(100, 0, updateStates); //10hz updates
return true;
}
else {
char* errorType = "Failed to open USB2Dynamixel!";
ase_printf("ERROR: %s \n",errorType);
Event_t event; event.val_prt = errorType;
EventManager_publish(DYNAMIXEL_ERROR_EVENT, &event);
return false;
}
}
void MotorInit(void) {
dxl_initialize( 0, DEFAULT_BAUDNUM ); // Not using device index
//Wheel Mode
dxl_write_word( RIGHT_MOTOR, P_CW_ANGLE_LIMIT_L, 0 );
dxl_write_word( RIGHT_MOTOR, P_CW_ANGLE_LIMIT_H, 0 );
dxl_write_word( RIGHT_MOTOR, P_CCW_ANGLE_LIMIT_L, 0 );
dxl_write_word( RIGHT_MOTOR, P_CCW_ANGLE_LIMIT_H, 0 );
dxl_write_word( LEFT_MOTOR, P_CW_ANGLE_LIMIT_L, 0 );
dxl_write_word( LEFT_MOTOR, P_CW_ANGLE_LIMIT_H, 0 );
dxl_write_word( LEFT_MOTOR, P_CCW_ANGLE_LIMIT_L, 0 );
dxl_write_word( LEFT_MOTOR, P_CCW_ANGLE_LIMIT_H, 0 );
//Set Torque
dxl_write_word( RIGHT_MOTOR, 24, 1 );
dxl_write_word( LEFT_MOTOR, 24, 1 );
//Set EEP Lock
dxl_write_word( RIGHT_MOTOR, P_EEP_LOCK, 1 );
dxl_write_word( LEFT_MOTOR, P_EEP_LOCK, 1 );
// Set goal speed
dxl_write_word( BROADCAST_ID, P_GOAL_SPEED_L, 0 );
_delay_ms(1000);
}
BBAzimuthalReflex::BBAzimuthalReflex() : _neck_motor(nullptr), _lock(true), _old_ILD(0)
{
try
{
//Initialize DXL framework
if(dxl_initialize(0,1)==1)
{
//Retrieve motor ID and configuration file path if any
int MOT_ID_NECK_AZIMUTH;
std::string cfg_path;
ConfigSingleton::GetInstance()->get_as<int>("MOT_ID_NECK_AZIMUTH",MOT_ID_NECK_AZIMUTH);
ConfigSingleton::GetInstance()->get_as<std::string>("MOT_CFG_PATH",cfg_path);
ConfigSingleton::GetInstance()->get_as<lbfloat>("DSP_SAMPLE_FREQUENCY",_sample_freq);
ConfigSingleton::GetInstance()->get_as<lbfloat>("ILD_STEP_THRESHOLD",_step_threshold);
//Declare motor
_neck_motor = new Mx28(MOT_ID_NECK_AZIMUTH);
//Initialize it
//_neck_motor->configure(cfg_path.c_str());
_neck_motor->get_value(MX28_CW_ANGLE_LIMIT, _cw_angle_limit);
_neck_motor->get_value(MX28_CCW_ANGLE_LIMIT, _ccw_angle_limit);
init();
}
else
{
std::cerr << "Failed to connect to motors." << std::endl;
std::cerr << "Leaving." << std::endl;
exit(100);
}
}
catch(std::out_of_range& e)
{
std::cerr << "Could not initialize BBAzimuthalReflex properly. Object might be ill-formed." << std::endl;
}
}
int main(int argc, char *argv[])
{
signal(SIGINT, signal_callback_handler);
if (dxl_initialize(0,4) == 0)
{
printf( "Failed to open USB2Dynamixel!\n" );
return -1;
}
while (true)
{
for (int i = 1 ; i <= 13 ; i++)
{
if (i >= 1 && i <= 8)
{
int pos_spring = dxl_read_word( 0x64 + i, 0x24 );
int result_spring = dxl_get_result();
if (result_spring != 1) pos_spring = -1;
int pos = dxl_read_word( i, 0x24 );
int result = dxl_get_result();
if (result != 1) pos = -1;
printf("%d:%d,%d:%d,",
i, pos, 0x64+i, pos_spring);
} else {
int pos = dxl_read_word( i, 0x24 );
int result = dxl_get_result();
if (result != 1) pos = -1;
printf("%d:%d,", i, pos);
}
}
printf("\n");
}
return EXIT_SUCCESS;
}
int main(int argc, char *argv[])
{
// Checks command line arguments
if (argc != 2)
{
printf("\nYou must specify an id!\n");
printf("Usage: %s id\n\n", argv[0]);
exit(EXIT_SUCCESS);
}
// Get the id of the servo or spring from
// the command line
int id = atoi(argv[1]);
// This is for handling CTRL+C nicely
signal(SIGINT, signal_callback_handler);
// Initialize the communication with the
// Dynamixel network bus at 250000bps
if (dxl_initialize(4,8) == 0)
{
printf( "Failed to open USB2Dynamixel!\n" );
return -1;
}
while (true)
{
int pos = dxl_read_word( id, 0x24 );
int result = dxl_get_result();
if (result != 1) pos = -1;
printf("%d:%d\n", id, pos);
}
return EXIT_SUCCESS;
}
int main(void)
{
#if 0
int id[NUM_ACTUATOR];
float phase[NUM_ACTUATOR];
float theta = 0;
int AmpPos = 512;
//int AmpPos = 2048; // for EX series
int GoalPos;
int i;
int CommStatus;
int isPress = 0;
int isOn = 0;
unsigned char ReceivedData;
int Value;
mServoList[0] = (stServo *)malloc(sizeof(stServo));
memset((void *)mServoList[0], 0x00, sizeof(stServo) );
mServoList[0]->id = 4;
serial_initialize(57600);
dxl_initialize( 0, DEFAULT_BAUDNUM ); // Not using device index
sei(); // Interrupt Enable
printf( "\n\nSyncWrite example for CM-700\n\n" );
#ifdef MODE_SYNC
for( i=0; i<NUM_ACTUATOR; i++ )
{
id[i] = i+2;
phase[i] = 2*PI * (float)i / (float)NUM_ACTUATOR;
}
#else
int wPresentPos = 512;
#endif
//Set EEP Lock
dxl_write_word( BROADCAST_ID, P_EEP_LOCK, 1 );
// Set goal speed
dxl_write_word( BROADCAST_ID, P_GOAL_SPEED_L, 0 );
// Set goal position
dxl_write_word( BROADCAST_ID, P_GOAL_POSITION_L, AmpPos );
dxl_write_word( 4, P_TORQUE_LIMIT_L, 0);
_delay_ms(1000);
while(1)
{
if(~PIND & SW_START){
isPress = 1;
}else{
if( isPress == 1 ){
if( isOn == 0 ){
isOn = 1;
}else{
isOn = 0;
}
}
isPress = 0;
}
// while( ReceivedData = getchar() != NULL ){
if(ReceivedData == 'u')
Value++;
else if(ReceivedData == 'd')
Value--;
printf("%d, %d\r\n", Value, ReceivedData);
// }
if( isOn ){
#ifdef MODE_SYNC
// Make syncwrite packet
dxl_set_txpacket_id(BROADCAST_ID);
dxl_set_txpacket_instruction(INST_SYNC_WRITE);
dxl_set_txpacket_parameter(0, P_GOAL_POSITION_L);
dxl_set_txpacket_parameter(1, 2);
for( i=0; i<NUM_ACTUATOR; i++ )
{
dxl_set_txpacket_parameter(2+3*i, id[i]);
GoalPos = (int)((sin(theta+phase[i]) + 1.0) * (float)AmpPos);
printf( "%d ", GoalPos );
dxl_set_txpacket_parameter(2+3*i+1, dxl_get_lowbyte(GoalPos));
dxl_set_txpacket_parameter(2+3*i+2, dxl_get_highbyte(GoalPos));
}
dxl_set_txpacket_length((2+1)*NUM_ACTUATOR+4);
printf( "\n" );
dxl_txrx_packet();
CommStatus = dxl_get_result();
if( CommStatus == COMM_RXSUCCESS )
PrintErrorCode();
else
PrintCommStatus(CommStatus);
theta += STEP_THETA;
if( theta > 2*PI )
theta -= 2*PI;
#else
wPresentPos = dxl_read_word( 4, P_PRESENT_POSITION_L );
printf( "%d\n", wPresentPos );
dxl_write_word( 2, P_GOAL_POSITION_L, wPresentPos );
dxl_write_word( 3, P_GOAL_POSITION_L, wPresentPos );
PrintErrorCode();
#endif
}
getServoStatus();
_delay_ms(CONTROL_PERIOD);
}
return 0;
#endif
#if 0
DDRC = 0x7F;
PORTC = 0x7E;
DDRD = 0x70;
PORTD = 0x11;
while (1)
{
if(~PIND & SW_START)
PORTC = ~(LED_BAT|LED_TxD|LED_RxD|LED_AUX|LED_MANAGE|LED_PROGRAM|LED_PLAY);
else PORTC = LED_BAT|LED_TxD|LED_RxD|LED_AUX|LED_MANAGE|LED_PROGRAM|LED_PLAY;
}
return 1;
#endif
while(isFinish == 0){
_delay_ms(500);
getSerialData();
// ReceivedData = getchar();
//if(ReceivedData == 'u'){
//printf("%d\r\n", Value);
//Value++;
//}else if(ReceivedData == 'd'){
//printf("%d\r\n", Value);
//Value--;
//}else if(ReceivedData == 10 || ReceivedData == 13 ){
//printf("%s\r\n", "end");
//break;
//}
printf("%s\r\n", "Loop");
}
printf("%s\r\n", "finish");
return 0;
}
int main()
{
int baudnum = 34;
int GoalPos[2] = {0, 7095};
//int GoalPos[2] = {0, 4095}; // for Ex series
int index = 1;
int deviceIndex = 0;
int Moving, PresentPos;
int CommStatus;
printf( "\n\nRead/Write example for Linux\n\n" );
///////// Open USB2Dynamixel ////////////
if( dxl_initialize(deviceIndex, baudnum) == 0 )
{
printf( "Failed to open USB2Dynamixel!\n" );
printf( "Press Enter key to terminate...\n" );
getchar();
return 0;
}
else
printf( "Succeed to open USB2Dynamixel!\n" );
while(1)
{
printf( "Press Enter key to continue!(press ESC and Enter to quit)\n" );
if(getchar() == 0x1b)
break;
// Write goal position
dxl_write_word( DEFAULT_ID, P_GOAL_POSITION_L, GoalPos[index] );
do
{
// Read present position
PresentPos = dxl_read_word( DEFAULT_ID, P_PRESENT_POSITION_L );
CommStatus = dxl_get_result();
if( CommStatus == COMM_RXSUCCESS )
{
printf( "%d %d\n",GoalPos[index], PresentPos );
PrintErrorCode();
}
else
{
PrintCommStatus(CommStatus);
break;
}
// Check moving done
Moving = dxl_read_byte( DEFAULT_ID, P_MOVING );
CommStatus = dxl_get_result();
if( CommStatus == COMM_RXSUCCESS )
{
if( Moving == 0 )
{
// Change goal position
if( index == 0 )
index = 1;
else
index = 0;
}
PrintErrorCode();
}
else
{
PrintCommStatus(CommStatus);
break;
}
}while(Moving == 1);
}
// Close device
dxl_terminate();
printf( "Press Enter key to terminate...\n" );
getchar();
return 0;
}
CSerialController::CSerialController(std::string port1, int baud1, std::string port2, int baud2, float timeout1, float timeout2, double rate, ros::NodeHandle nh, std::string dmxPort) :
CQboduinoDriver(port1, baud1, port2, baud2, timeout1, timeout2), rate_(rate), nh_(nh), dmxPort_(dmxPort)
{
//Check parameters in ROS_PARAM and start controllers
//Advertise the test service
qboTestService_=nh_.advertiseService("/qbo_arduqbo/test_service", &CSerialController::qboTestService, this);
//Check for controllers that are governed by the head board
if(boards_.count("head")==1)
{
//Controlled servos
if(nh_.hasParam("controlledservos"))
{
std::map< std::string, XmlRpc::XmlRpcValue >::iterator it;
std::map< std::string, XmlRpc::XmlRpcValue > value;
XmlRpc::MyXmlRpcValue servos;
nh_.getParam("controlledservos", servos);
ROS_ASSERT(servos.getType() == XmlRpc::XmlRpcValue::TypeStruct);
value=servos;
for(it=value.begin();it!=value.end();it++)
{
ROS_ASSERT((*it).second.getType() == XmlRpc::XmlRpcValue::TypeStruct);
servosList_[(*it).first]=new ControledServo((*it).first,this);
servosNamesList_.push_back((*it).first);
//Set servos parameters
servosList_[(*it).first]->setParams((*it).second);
servosList_[(*it).first]->setAngle(0);
ROS_INFO_STREAM("Servo pseudocontrolado " << (*it).first << " started");
}
}
//Un-controlled servos
if(nh_.hasParam("uncontrolledservos"))
{
std::map< std::string, XmlRpc::XmlRpcValue >::iterator it;
std::map< std::string, XmlRpc::XmlRpcValue > value;
XmlRpc::MyXmlRpcValue servos;
nh_.getParam("uncontrolledservos", servos);
ROS_ASSERT(servos.getType() == XmlRpc::XmlRpcValue::TypeStruct);
value=servos;
for(it=value.begin();it!=value.end();it++)
{
ROS_ASSERT((*it).second.getType() == XmlRpc::XmlRpcValue::TypeStruct);
servosList_[(*it).first]=new CServo((*it).first,this);
servosNamesList_.push_back((*it).first);
//Set servos parameters
servosList_[(*it).first]->setParams((*it).second);
servosList_[(*it).first]->setAngle(0);
ROS_INFO_STREAM("Servo " << (*it).first << " started");
}
}
if(nh_.hasParam("dynamixelservo"))
{
//Start dynamixel port
if( dxl_initialize(const_cast<char *>(dmxPort_.c_str()), 1) == 0 )
{
ROS_ERROR( "Failed to open dynamixel controller!\n" );
}
else
{
ROS_INFO( "Succeed to open dynamixel controller!\n" );
std::map< std::string, XmlRpc::XmlRpcValue >::iterator it;
std::map< std::string, XmlRpc::XmlRpcValue > value;
XmlRpc::MyXmlRpcValue servos;
nh_.getParam("dynamixelservo", servos);
ROS_ASSERT(servos.getType() == XmlRpc::XmlRpcValue::TypeStruct);
value=servos;
for(it=value.begin();it!=value.end();it++)
{
ROS_ASSERT((*it).second.getType() == XmlRpc::XmlRpcValue::TypeStruct);
servosList_[(*it).first]=new DynamixelServo(nh,(*it).first,this);
servosNamesList_.push_back((*it).first);
//Set servos parameters
((DynamixelServo*)servosList_[(*it).first])->changeTorque(254);
servosList_[(*it).first]->setParams((*it).second);
servosList_[(*it).first]->setAngle(0,0.3);
ROS_INFO_STREAM("Dynamixel " << (*it).first << " started");
}
}
}
//Check for controllers
if(nh_.hasParam("controllers"))
{
std::map< std::string, XmlRpc::XmlRpcValue >::iterator it;
std::map< std::string, XmlRpc::XmlRpcValue > value;
XmlRpc::MyXmlRpcValue controllers;
nh_.getParam("controllers", controllers);
ROS_ASSERT(controllers.getType() == XmlRpc::XmlRpcValue::TypeStruct);
value=controllers;
for(it=value.begin();it!=value.end();it++)
{
ROS_ASSERT((*it).second.getType() == XmlRpc::XmlRpcValue::TypeStruct);
XmlRpc::XmlRpcValue controller_params=(*it).second;
if(controller_params.hasMember("type"))
{
std::string type=controller_params["type"];
//Joint controller
if(type.compare("joint_controller")==0)
{
ROS_INFO("joint_controller started");
controllersList_.push_back(new CJointController((*it).first, this, nh, servosList_));
}
//Mouth controller
else if(type.compare("mouth_controller")==0)
{
ROS_INFO("mouth_controller started");
controllersList_.push_back(new CMouthController((*it).first, this, nh));
}
else if(type.compare("nose_controller")==0)
{
ROS_INFO("nose_controller started");
controllersList_.push_back(new CNoseController((*it).first, this, nh));
}
else if(type.compare("mics_controller")==0)
{
ROS_INFO("mics_controller started");
controllersList_.push_back(new CMicsController((*it).first, this, nh));
}
}
}
}
//The following lines setup the joint state publisher at the given rate
std::string topic;
nh_.param("joint_states_topic", topic, std::string("joint_states"));
joint_pub_ = nh_.advertise<sensor_msgs::JointState>(topic, 1);
timer_=nh_.createTimer(ros::Duration(1/rate_),&CSerialController::timerCallback,this);
}
//Check for controllers that are governed by the base board
if(boards_.count("base")==1)
{
if(nh_.hasParam("controllers"))
{
std::map< std::string, XmlRpc::XmlRpcValue >::iterator it;
std::map< std::string, XmlRpc::XmlRpcValue > value;
XmlRpc::MyXmlRpcValue controllers;
nh_.getParam("controllers", controllers);
ROS_ASSERT(controllers.getType() == XmlRpc::XmlRpcValue::TypeStruct);
value=controllers;
for(it=value.begin();it!=value.end();it++)
{
ROS_ASSERT((*it).second.getType() == XmlRpc::XmlRpcValue::TypeStruct);
XmlRpc::XmlRpcValue controller_params=(*it).second;
if(controller_params.hasMember("type"))
{
std::string type=controller_params["type"];
//Base controller (speed and odometry)
if(type.compare("base_controller")==0)
{
ROS_INFO("base_controller started");
controllersList_.push_back(new CBaseController((*it).first, this, nh));
}
//Battery controller
else if(type.compare("battery_controller")==0)
{
ROS_INFO("battery_controller started");
controllersList_.push_back(new CBatteryController((*it).first, this, nh));
}
//Sensors controller (I2C sonar sensors and ADC readings)
else if(type.compare("sensors_controller")==0)
{
ROS_INFO("sensors_controller started");
sensorsController_ = new CSrf10Controller((*it).first, this, nh);
controllersList_.push_back(sensorsController_);
}
//Back LCD controller
else if(type.compare("lcd_controller")==0)
{
ROS_INFO("lcd_controller started");
controllersList_.push_back(new CLCDController((*it).first, this, nh));
ipTimer_=nh_.createTimer(ros::Duration(5),&CSerialController::ipTimerCallback,this);
}
//IMU board controller
else if(type.compare("imu_controller")==0)
{
ROS_INFO("imu_controller started");
controllersList_.push_back(new CImuController((*it).first, this, nh));
}
else if(type.compare("irs_controller")==0)
{
ROS_INFO("irs_controller started");
controllersList_.push_back(new CInfraRedsController((*it).first, this, nh));
}
}
}
}
}
}
int main()
{
int baudnum = 34;
int deviceIndex = 0;
printf( "\n\nRead/Write example for Linux\n\n" );
///////// Open USB2Dynamixel ////////////
if( dxl_initialize(deviceIndex, baudnum) == 0 )
{
printf( "Failed to open USB2Dynamixel!\n" );
printf( "Press Enter key to terminate...\n" );
getchar();
return 0;
}
else
printf( "Succeed to open USB2Dynamixel!\n" );
while(1)
{
printf( "Press Enter key to continue!(press ESC and Enter to quit)\n" );
if(getchar() == 0x1b)
break;
jointMode(4,180);
jointMode(5,180);
/* int i;
for (i=4;i<=5;i++) {
dxl_ping(i);
printf("%i",i);
int result = dxl_get_result( );
if( result == COMM_TXSUCCESS )
{
printf("TX+");
}
else if( result == COMM_RXSUCCESS )
{
printf("RX+");
int j;
for (j=0;j<=8;j++) {
int value;
result=dxl_get_result( );
if (result == COMM_RXSUCCESS) {
value=dxl_read_byte(i,j);
printf("%i->%i\n",j,value);
} else
printf("Error reading byte\n");
}
}
else if( result == COMM_TXFAIL )
{
printf("TX-");
}
else if( result == COMM_RXFAIL)
{
printf("RX-");
}
else if( result == COMM_TXERROR )
{
printf("TX-");
}
else if( result == COMM_RXWAITING )
{
printf("W?");
}
printf("\n");
} */
}
// Close device
dxl_terminate();
printf( "Press Enter key to terminate...\n" );
getchar();
return 0;
}
int main()
{
int i;
int baudnum = DEFAULT_BAUDNUM;
int GoalPos[2] = {0, 1023};
//int GoalPos[2] = {0, 4095}; // for Ex series
int index = 0;
int deviceIndex = 0;
int Moving, PresentPos;
int CommStatus;
system("/home/humanoid/Documents/visao/bin/Debug/visao");
system("echo 123456 | sudo -S chmod 777 /dev/ttyUSB*");//USB
//system("./home/humanoidfei/Documents/visao/bin/Debug/visao");
//id_t PID = getpid();
char string[50];
sprintf(string,"echo 123456 | sudo -S renice -20 -p %d", getpid());
system(string);
// char command[50];
// sprintf(command, "echo 123456 | sudo -S renice 0 -p %d", getProcIdByName("pulseaudio"));
// system(command);
//int PID = popen("pidof pulseaudio","r");
//system("echo 123456 | sudo -S renice 0 1533");
//setpriority(PRIO_PROCESS, getpid(), -20);
printf( "\n\nRx-28 example for Linux\n\n" );
///////// Open USB2Dynamixel ////////////
if( dxl_initialize(deviceIndex, baudnum) == 0 )
{
printf( "Failed to open USB2Dynamixel!\n" );
printf( "Press Enter key to terminate...\n" );
getchar();
return 0;
}
else
printf( "Succeed to open USB2Dynamixel!\n" );
while(1)
{
printf( "Press Enter key to continue!(press ESC and Enter to quit)\n" );
if(getchar() == 0x1b)
break;
// // Write goal position
for(i = 1;i<21;i++)
dxl_write_word( i, P_GOAL_POSITION_L, GoalPos[index] );
dxl_write_word( 3, 32, 0);//usleep(1000000);
if( index == 0 )
index = 1;
else
index = 0;
/* do
{
// Read present position
//for(i=1;i<11;i++)
PresentPos = dxl_read_word( i, P_PRESENT_POSITION_L ); //usleep(1000000);
CommStatus = dxl_get_result();
if( CommStatus == COMM_RXSUCCESS )
{
printf( "%d %d\n",GoalPos[index], PresentPos );
PrintErrorCode();
}
else
{
PrintCommStatus(CommStatus);
break;
}
// Check moving done
//for(i=1;i<11;i++)
Moving = dxl_read_byte( i, P_MOVING );
CommStatus = dxl_get_result();
if( CommStatus == COMM_RXSUCCESS )
{
if( Moving == 0 )
{
// Change goal position
if( index == 0 )
index = 1;
else
index = 0;
}
PrintErrorCode();
}
else
{
PrintCommStatus(CommStatus);
break;
}
}while(Moving == 1);*/
}
//}
// Close device
dxl_terminate();
printf( "Press Enter key to terminate...\n" );
getchar();
return 0;
}
// High level initialization - specific robot settings for Bioloid
void dxl_init(int baudnum)
{
int commStatus = 0, errorStatus = 0;
// now prepare the Dynamixel servos
// first initialize the bus
dxl_initialize( 0, baudnum );
// wait 0.1s
_delay_ms(100);
// Next check the hardware configuration is valid
for (int i=0; i<NUM_AX12_SERVOS; i++)
{
// ping each servo in turn
errorStatus = dxl_ping(AX12_IDS[i]);
if (errorStatus == -1)
{
printf("\nHardware Configuration Failure at Dynamixel ID %i.\n", AX12_IDS[i]);
dxl_terminate();
return;
}
}
// set alarm LED and shutdown to prevent overheat/overload
commStatus = dxl_write_byte(BROADCAST_ID, DXL_ALARM_LED, 36);
if(commStatus != COMM_RXSUCCESS) {
printf("\nDXL_ALARM_LED Broadcast - ");
dxl_printCommStatus(dxl_get_result());
}
commStatus = dxl_write_byte(BROADCAST_ID, DXL_ALARM_SHUTDOWN, 36);
if(commStatus != COMM_RXSUCCESS) {
printf("\nDXL_ALARM_LED Broadcast - ");
dxl_printCommStatus(dxl_get_result());
}
// now set temperature and voltage limits
commStatus = dxl_write_byte(BROADCAST_ID, DXL_TEMPERATURE_LIMIT, 70);
if(commStatus != COMM_RXSUCCESS) {
printf("\nDXL_TEMPERATURE_LIMIT Broadcast - ");
dxl_printCommStatus(dxl_get_result());
}
commStatus = dxl_write_byte(BROADCAST_ID, DXL_LOW_VOLTAGE_LIMIT, 70);
if(commStatus != COMM_RXSUCCESS) {
printf("\nDXL_LOW_VOLTAGE_LIMIT Broadcast - ");
dxl_printCommStatus(dxl_get_result());
}
// set a 2-point compliance margin (equals 0.58 deg)
commStatus = dxl_write_byte(BROADCAST_ID, DXL_CW_COMPLIANCE_MARGIN, 2);
if(commStatus != COMM_RXSUCCESS) {
printf("\nDXL_CW_COMPLIANCE_MARGIN Broadcast - ");
dxl_printCommStatus(dxl_get_result());
}
commStatus = dxl_write_byte(BROADCAST_ID, DXL_CCW_COMPLIANCE_MARGIN, 2);
if(commStatus != COMM_RXSUCCESS) {
printf("\nDXL_CCW_COMPLIANCE_MARGIN Broadcast - ");
dxl_printCommStatus(dxl_get_result());
}
_delay_ms(100);
// and enable torque to keep positions
commStatus = dxl_write_byte(BROADCAST_ID, DXL_TORQUE_ENABLE, 1);
if(commStatus != COMM_RXSUCCESS) {
printf("\nDXL_TORQUE_ENABLE Broadcast - ");
dxl_printCommStatus(dxl_get_result());
}
_delay_ms(50);
}
int main(){
int devNumber = 0;
int baudNumber = 1; //For 1 Mbps baud
printf("Running StandSit. Press ESC to terminate.\n");
if(!(dxl_initialize(devNumber, baudNumber))){
printf("Failed to open USB Interface. :(\n");
return 0;
}
else{
printf("USB Interfaced Opened! :D\n");
}
// if(SITTING_POS != dxl_read_byte(BROADCAST_ID, P_PRESENT_POSITION_L)){
//
// printf("Going to initial sitting position.\n");
// dxl_write_word(BROADCAST_ID, P_PRESENT_POSITION_L, SITTING_POS);
// while(1 == dxl_read_byte(BROADCAST_ID, P_MOVING));
// }
int loopCount = 0;
printf("Going into main loop...\n");
while(1){
printf("Loop time! Round %d\n", loopCount++);
// if(getchar() == 0x1b)
// break;
if(STANDING_POS == dxl_read_byte(BROADCAST_ID, P_PRESENT_POSITION_L)){
printf("Sitting down...\n");
dxl_write_word(BROADCAST_ID, P_PRESENT_POSITION_L, SITTING_POS);
}
else if(SITTING_POS == dxl_read_byte(BROADCAST_ID, P_PRESENT_POSITION_L)){
printf("Standing up...\n");
dxl_write_word(BROADCAST_ID, P_PRESENT_POSITION_L, STANDING_POS);
}
else{
int CommStatus = dxl_get_result();
switch(CommStatus)
{
case COMM_TXFAIL:
printf("COMM_TXFAIL: Failed transmit instruction packet!\n");
break;
case COMM_TXERROR:
printf("COMM_TXERROR: Incorrect instruction packet!\n");
break;
case COMM_RXFAIL:
printf("COMM_RXFAIL: Failed get status packet from device!\n");
break;
case COMM_RXWAITING:
printf("COMM_RXWAITING: Now recieving status packet!\n");
break;
case COMM_RXTIMEOUT:
printf("COMM_RXTIMEOUT: There is no status packet!\n");
break;
case COMM_RXCORRUPT:
printf("COMM_RXCORRUPT: Incorrect status packet!\n");
break;
default:
printf("This is unknown error code!\n");
break;
}
}
//while(1 == dxl_read_byte(BROADCAST_ID, P_MOVING));
sleep(1);
}
dxl_terminate();
printf("Shutting down...\n");
return 0;
}
int main() {
double CoMavg;
double sumns = 0;
unsigned char dc1[2];
unsigned char dc2[2];
double arrayX[] = { -31.6190, -31.9288, -32.0040, -32.3204, -32.5042, -16.5338, -16.5530, -16.7062, -16.7277, -16.7474, 0.2290,0.1433,-0.1915,-0.4104,-0.6338,16.3727,16.4583,16.5425,16.6221,16.7002,32.8839,32.6383,32.3759,32.1024, 31.8116};
//{-20.0480,-22.4280,-20.0480,-22.4280,-20.0480,-14.0480,-16.4280,-14.0480,-16.4280,-14.0480,-8.0480,-10.4280,-8.0480,-10.4280,-8.0480,-2.0480,-4.4280,-2.0480,-4.4280,-2.0480,3.9520,1.5720,3.9520,1.5720,3.9520,9.9520,7.5720,9.9520,7.5720,9.9520,15.9520,13.5720,15.9520,13.5720,15.9520,21.9520,19.5720,21.9520, 19.5720, 21.9520};
// {4.76, 2.38, 4.76, 2.38, 4.76, 10.76, 8.38, 10.76, 8.38, 10.76, 16.76, 14.38, 16.76, 14.38, 16.76, 22.76,20.38,22.76, 20.38, 22.76,28.76, 26.38,28.76, 26.38, 28.76, 34.76, 32.38, 34.76, 32.38, 34.76, 40.76, 38.38, 40.76, 38.38, 40.76, 46.76, 44.38, 46.76, 44.38, 46.76};
//double arrayY[] = { 12,6,0,-6,-12,12,6,0,-6,-12,12,6,0,-6,-12,12,6,0,-6,-12,12,6,0,-6,-12,12,6,0,-6,-12,12,6,0,-6,-12,12,6,0,-6,-12};
//{27, 21, 15, 9, 3, 27, 21, 15, 9, 3, 27, 21, 15, 9, 3, 27, 21, 15, 9, 3, 27, 21, 15, 9, 3, 27, 21, 15, 9, 3, 27, 21, 15, 9, 3, 27, 21, 15, 9, 3};
double weight[] = {0.5241, 2.2774, 0, 0.3845, 1.4690, 0.0059, 0,3.7938, 0,2.6875,323.1828,238.0930,0,0,0,7.7829,0,1.6664,0,0,0.8394,0.8289,0,1.2467,0.5905};
//dynamixel initialization stuff
int baudnum = 1;
int limits[2] = {0, 1024};
int index = 0;
int deviceIndex = 0;
int Moving, PresentPos;
int CommStatus;
int ns = 20;
double CoMhist[ns];
if( dxl_initialize(deviceIndex, baudnum) == 0 ) {
printf( "Failed to open USB2Dynamixel!\n" );
}
int turnValue = 512;
dxl_write_word( MOTOR, 32, 75); //sets the speed for turning
dxl_write_word( MOTOR, P_GOAL_POSITION_L, turnValue );
TakkTile takktile = TakkTile();
takktile.startSampling(200,dc2);
double baseline[40];
std::cout<< "calibrating..." <<std::endl;
if(takktile.calibrate(baseline,10)==0) {
std::cout<< "error!" <<std::endl;
goto exit;
}
std::cout<<"done calibrating"<<std::endl;
for (int i=0; i<100000; i++) { //args instead of 5
if (i > ns) sumns = sumns - CoMhist[i%ns];
Data data;
if(takktile.getData(data)==0){ std::cout<< "error :(" <<std::endl; }
double sumx=0;
// double sumy=0;
double sum=0;
for (int j = 0; j < 25; j++) {//change to 40 for array
sumx += (double)(data.pressures[j]-baseline[j])*arrayX[j]*weight[j];
// sumy += (double)(data.pressures[i]-baseline[i])*arrayY[i]*weight[i];
sum += (double)(data.pressures[j]-baseline[j])*weight[j];
}
double CoMx = (double)(sumx)/(double)(sum);
// double CoMy = (double)(sumy)/(double)(sum);
CoMhist[i % ns] = CoMx;
sumns = sumns + CoMhist[i%ns];
CoMavg = (sumns / ns) + 1;
if (i > 500){
Moving = dxl_read_byte( DEFAULT_ID, P_MOVING );
std::cout<<CoMx<<","<<CoMavg<<","<<turnValue<<"; ";
//if (Moving){}
//else{
if(turnValue > 700) {
turnValue = 700;}
else if (turnValue < 350) { turnValue = 350;}
else turnValue = turnValue - (int)(CoMavg/2);//plus for arry
//std::cout<<turnValue<<std::endl;
}
dxl_write_word( MOTOR, P_GOAL_POSITION_L, turnValue );
//}
// do {
// Read present position
//PresentPos = dxl_read_word( MOTOR, P_PRESENT_POSITION_L );
//CommStatus = dxl_get_result();
//if( CommStatus == COMM_RXSUCCESS ) { // printf( "%d %d\n",turnValue, PresentPos );
//PrintErrorCode();
// } else {
//PrintCommStatus(CommStatus);
// std::cout<< "comm error" <<std::endl;
// break;
//}
// Check moving done
// Moving = dxl_read_byte( MOTOR, P_MOVING );
//CommStatus = dxl_get_result();
//if( CommStatus == COMM_RXSUCCESS ) {
// //PrintErrorCode();
//} else {
// std::cout<< "comm error 2" <<std::endl;
//PrintCommStatus(CommStatus);
// break;
//}
// } while(Moving == 1);
// PresentPos = dxl_read_word( MOTOR, P_PRESENT_POSITION_L );
// std::cout<< CoMx << " " << CoMy <<std::endl;
}//time how long this takes
// /for (int i = 0; i<100000000;i++);
exit: //note: doesn't quit very well
takktile.stopSampling(dc1);
takktile.TakkClose();
dxl_terminate();
return 0;
};
int main()
{
int id[NUM_ACTUATOR];
int baudnum = 1;
int deviceIndex = 0;
float phase[NUM_ACTUATOR];
float theta = 0;
int AmpPos = 512;
//int AmpPos = 2048; // for EX series
int GoalPos;
int i;
int CommStatus;
printf( "\n\nSyncWrite example for Linux\n\n" );
// Initialize id and phase
for( i=0; i<NUM_ACTUATOR; i++ )
{
id[i] = i+1;
phase[i] = 2*PI * (float)i / (float)NUM_ACTUATOR;
}
///////// Open USB2Dynamixel ////////////
if( dxl_initialize(deviceIndex, baudnum) == 0 )
{
printf( "Failed to open USB2Dynamixel!\n" );
printf( "Press Enter key to terminate...\n" );
getchar();
return 0;
}
else
printf( "Succeed to open USB2Dynamixel!\n" );
// Set goal speed
dxl_write_word( BROADCAST_ID, P_GOAL_SPEED_L, 0 );
// Set goal position
dxl_write_word( BROADCAST_ID, P_GOAL_POSITION_L, AmpPos );
while(1)
{
printf( "Press Enter key to continue!(press ESC and Enter to quit)\n" );
if(getchar() == 0x1b)
break;
theta = 0;
do
{
// Make syncwrite packet
dxl_set_txpacket_id(BROADCAST_ID);
dxl_set_txpacket_instruction(INST_SYNC_WRITE);
dxl_set_txpacket_parameter(0, P_GOAL_POSITION_L);
dxl_set_txpacket_parameter(1, 2);
for( i=0; i<NUM_ACTUATOR; i++ )
{
dxl_set_txpacket_parameter(2+3*i, id[i]);
GoalPos = (int)((sin(theta+phase[i]) + 1.0) * (double)AmpPos);
printf( "%d:%d ", id[i], GoalPos );
dxl_set_txpacket_parameter(2+3*i+1, dxl_get_lowbyte(GoalPos));
dxl_set_txpacket_parameter(2+3*i+2, dxl_get_highbyte(GoalPos));
}
dxl_set_txpacket_length((2+1)*NUM_ACTUATOR+4);
printf( "\n" );
dxl_txrx_packet();
CommStatus = dxl_get_result();
if( CommStatus == COMM_RXSUCCESS )
{
PrintErrorCode();
}
else
{
PrintCommStatus(CommStatus);
break;
}
theta += STEP_THETA;
usleep(CONTROL_PERIOD);
}while(theta < 2*PI);
}
dxl_terminate();
printf( "Press Enter key to terminate...\n" );
getchar();
return 0;
}
int main(void)
{
int id[NUM_ACTUATOR];
float phase[NUM_ACTUATOR];
float theta = 0;
int AmpPos = 512;
//int AmpPos = 2048; // for EX series
int GoalPos;
int i;
int CommStatus;
serial_initialize(57600);
dxl_initialize( 0, DEFAULT_BAUDNUM ); // Not using device index
sei(); // Interrupt Enable
printf( "\n\nSyncWrite example for CM-700\n\n" );
for( i=0; i<NUM_ACTUATOR; i++ )
{
id[i] = i+1;
phase[i] = 2*PI * (float)i / (float)NUM_ACTUATOR;
}
// Set goal speed
dxl_write_word( BROADCAST_ID, P_GOAL_SPEED_L, 0 );
// Set goal position
dxl_write_word( BROADCAST_ID, P_GOAL_POSITION_L, AmpPos );
_delay_ms(1000);
while(1)
{
// Make syncwrite packet
dxl_set_txpacket_id(BROADCAST_ID);
dxl_set_txpacket_instruction(INST_SYNC_WRITE);
dxl_set_txpacket_parameter(0, P_GOAL_POSITION_L);
dxl_set_txpacket_parameter(1, 2);
for( i=0; i<NUM_ACTUATOR; i++ )
{
dxl_set_txpacket_parameter(2+3*i, id[i]);
GoalPos = (int)((sin(theta+phase[i]) + 1.0) * (float)AmpPos);
printf( "%d ", GoalPos );
dxl_set_txpacket_parameter(2+3*i+1, dxl_get_lowbyte(GoalPos));
dxl_set_txpacket_parameter(2+3*i+2, dxl_get_highbyte(GoalPos));
}
dxl_set_txpacket_length((2+1)*NUM_ACTUATOR+4);
printf( "\n" );
dxl_txrx_packet();
CommStatus = dxl_get_result();
if( CommStatus == COMM_RXSUCCESS )
PrintErrorCode();
else
PrintCommStatus(CommStatus);
theta += STEP_THETA;
if( theta > 2*PI )
theta -= 2*PI;
_delay_ms(CONTROL_PERIOD);
}
return 0;
}
DynamixelInterface::DynamixelInterface() {
int status=dxl_initialize(DEVICEINDEX,BAUDNUM) == 0;
// cout << status << endl;
ok=DXL_ComError::isOK(status);
}
