void servoGoto(int id,int angle) {
// Can handle angles > -180 to +180
int cw=0;
int ccw=0;
while (angle>180) { angle-=360; cw++;}
while (angle<-180) {angle+=360; ccw++;}
printf("Final angle %d\n",angle);
printf("Clockwise revolutions %d\n",cw);
printf("Counterclockwise revolutions %d\n",ccw);
int oldValue=dxl_read_byte(id,P_PRESENT_POSITION);
while (cw>0) {
wheelMode(id,1023);
int newValue=dxl_read_byte(id,P_PRESENT_POSITION);
if ((newValue-oldValue)<-2*JITTER) {
cw--;
}
}
while (ccw>0) {
wheelMode(id,-1023);
int newValue=dxl_read_byte(id,P_PRESENT_POSITION);
if ((oldValue-newValue)<-2*JITTER) {
ccw--;
}
}
jointMode(id,angle);
}
void test() {
bMoving = dxl_read_byte( id, 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, GOAL_POSITION_L, GoalPos[INDEX] );
}
PrintErrorCode();
// Read present position
wPresentPos = dxl_read_word( id, PRESENT_POSITION_L );
TxDWord16(GoalPos[INDEX]);
TxDString(" ");
TxDWord16(wPresentPos);
TxDByte_PC('\r');
TxDByte_PC('\n');
}
else
PrintCommStatus(CommStatus);
}
void isMoving(const int servo_id,int *result,int *CommStatus){
*result = dxl_read_byte( servo_id, P_MOVING);
*CommStatus = dxl_get_result();
if(*CommStatus != COMM_RXSUCCESS)
PrintCommStatus(*CommStatus);
PrintErrorCode();
}
void setMovement(int servo_id,const int angleMin,const int angleMax){
int GoalPos[2] = {angleMin,angleMax};
int index =0;
int Moving,CommStatus;
while(1){
// Write goal position
dxl_write_word( servo_id, P_GOAL_POSITION_L, GoalPos[index] );
do
{
// Check moving done
Moving = dxl_read_byte( servo_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;
}
}
else
{
PrintCommStatus(CommStatus);
break;
}
}while(Moving == 1);
}
}
unsigned char DynamixelInterface::readByte(int id,int address) {
if (!DXL2USB.isOk())throw DXL_ComError(-1,id,__FILE__,__LINE__);
int retry=DynamixelInterface::RETRIES;
int status=0;
unsigned char value=0;
do {
value=dxl_read_byte(id,address);
retry--;
status=dxl_get_result();
if (!DXL_ComError::isOK(status)) usleep(10000);
}while (!DXL_ComError::isOK(status) && retry>0);
if (!DXL_ComError::isOK(status)){
unsigned char b=dxl_read_byte(id,DXL_ALARM_SHUTDOWN_BYTE);
cout << "Alarm Shutdown Byte:"<<((int)b)<<endl;
throw DXL_ComError(status,id,__FILE__,__LINE__);
}
return value;
}
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;
}
int Dxl::isMoving(unsigned int device_id) const
{
int moving = dxl_read_byte(device_id, MOVING);
if(commRXIsOk())
{
return moving;
}
return false;
}
int DynamixelSimpleAPI::getSetting(const int id, const int reg_name, int reg_type, const int device)
{
int value = -1;
if (checkId(id) == true)
{
// Device detection
const int (*cctt)[8] = getRegisterTable(device);
if (cctt == NULL)
{
// Using default control table from this SimpleAPI isntance
cctt = ct;
}
if (cctt)
{
// Find register's informations (addr, size...)
RegisterInfos infos = {-1, -1, -1, -1, -1, -1, -1, -1, -1};
if (getRegisterInfos(cctt, reg_name, infos) == 1)
{
// Read value
if (infos.reg_size == 1)
{
value = dxl_read_byte(id, infos.reg_addr);
}
else if (infos.reg_size == 2)
{
value = dxl_read_word(id, infos.reg_addr);
}
// Check value
if (value < infos.reg_value_min && value > infos.reg_value_max)
{
value = -1;
}
}
else
{
TRACE_ERROR(DAPI, "[#%i] getSetting(reg %i / %s) [REGISTER NAME ERROR]\n", id, reg_name, getRegisterNameTxt(reg_name).c_str());
}
}
else
{
TRACE_ERROR(DAPI, "[#%i] getSetting(reg %i / %s) [CONTROL TABLE ERROR]\n", id, reg_name, getRegisterNameTxt(reg_name).c_str());
}
}
else
{
TRACE_ERROR(DAPI, "[#%i] getSetting(reg %i / %s) [DEVICE ID ERROR]\n", id, reg_name, getRegisterNameTxt(reg_name).c_str());
}
return value;
}
bool ControlUtils::getByte(int8_t *val, int8_t addr, int joint)
{
int8_t tmp = dxl_read_byte(_id[joint], addr);
if(dxl_get_result() == COMM_RXSUCCESS) {
*val = tmp;
return true;
}
else {
printf("faled to get byte from %d at %d\n", addr, joint);
return false;
}
}
int
motorIsMoving(void)
{
int i;
for(i=1;i<NUM_OF_SERVOS_ATTACHED;i++)
{
if(dxl_read_byte(i, P_MOVING))
return 1;
}
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;
}
float DynamixelServo::getAngle()
{
int pos = dxl_read_word( id_, P_PRESENT_POSITION_L );
int CommStatus = dxl_get_result();
if( CommStatus == COMM_RXSUCCESS )
{
PrintErrorCode();
}
else
{
PrintCommStatus(CommStatus);
}
qbo_arduqbo::motor_state motor_state;
motor_state.header.stamp = ros::Time::now();
motor_state.id=id_;
motor_state.goal=dxl_read_word( id_, P_GOAL_POSITION_L );
motor_state.position=pos;
motor_state.error=motor_state.goal-pos;
int speed=dxl_read_word( id_, P_PRESENT_SPEED_L );
if(speed>=1024)
speed=1024-speed;
motor_state.speed=speed;
int load=dxl_read_word( id_, P_PRESENT_LOAD_L );
if(load>=1024)
load=1024-load;
motor_state.load=((float)load)/1024.0;
motor_state.voltage=((float)dxl_read_byte(id_,P_PRESENT_VOLTAGE))/10.0;
motor_state.temperature=dxl_read_byte( id_, P_PRESENT_TEMPERATURE );
motor_state.moving=(dxl_read_byte( id_, P_MOVING)==1);
servo_state_pub_.publish(motor_state);
float angle = (pos - neutral_) * rad_per_tick_;
if (invert_)
angle = angle * -1.0;
angle_ = angle;
ROS_DEBUG_STREAM("Recibed angle " << angle_ << " for servo " << name_ << " from the head board");
return angle_;
}
int DynamixelSimpleAPI::readFirmwareVersion(const int id)
{
int value = -1;
if (checkId(id, false) == true)
{
int addr = getRegisterAddr(ct, REG_FIRMWARE_VERSION);
value = dxl_read_byte(id, addr);
if (dxl_print_error() != 0)
{
value = -1;
}
}
return value;
}
int DynamixelSimpleAPI::getLed(const int id)
{
int value = -1;
if (checkId(id, false) == true)
{
int addr = getRegisterAddr(ct, REG_LED);
value = dxl_read_byte(id, addr);
if (dxl_print_error() != 0)
{
value = -1;
}
}
return value;
}
bool DynamixelSimpleAPI::isMoving(const int id)
{
bool moving = false;
if (checkId(id, false) == true)
{
int addr = getRegisterAddr(ct, REG_MOVING);
if (dxl_read_byte(id, addr) > 0)
{
if (dxl_print_error() == 0)
{
moving = true;
}
}
}
return moving;
}
bool dynamixelApi_connect(int actId) {
dxl_ping(actId);
if (dxl_get_result() == COMM_RXSUCCESS) {
ase_printf("SUCCESS: Found Dynamixel with id = %i renamed to %i \n", actId, dyna.nActuators);
int wValue = dxl_read_word(actId, P_MODEL_NUMBER_L);
if (dxl_get_result() == COMM_RXSUCCESS)
ase_printf("Model number:%d, ", wValue);
int bValue = dxl_read_byte(actId, P_VERSION);
if (dxl_get_result() == COMM_RXSUCCESS)
ase_printf("Version:%d\n", bValue);
dyna.actuators[dyna.nActuators] = actId;
dynamixelApi_setWheelMode(dyna.nActuators, false);
dyna.nActuators++;
return true;
}
else {
ase_printf("ERROR: Unable to connect to Dynamixel with id = %i\n", actId);
return false;
}
}
double DynamixelSimpleAPI::readCurrentTemperature(const int id)
{
double value = 0.0;
if (checkId(id, false) == true)
{
int addr = getRegisterAddr(ct, REG_CURRENT_TEMPERATURE);
value = dxl_read_byte(id, addr);
// Valid temperatures are in range [-5;+70], but there is not much point in validating this value
if (dxl_print_error() == 0)
{
value /= 10.0;
}
else
{
value = 0.0;
}
}
return value;
}
double DynamixelSimpleAPI::readCurrentVoltage(const int id)
{
double value = 0.0;
if (checkId(id, false) == true)
{
int addr = getRegisterAddr(ct, REG_CURRENT_VOLTAGE);
value = dxl_read_byte(id, addr);
// Valid voltages are in range [9;12], but there is not much point in validating this value
if (dxl_print_error() == 0)
{
value /= 10.0;
}
else
{
value = 0.0;
}
}
return value;
}
int DynamixelInterface::readWord(int id,int address) {
if (!DXL2USB.isOk())throw DXL_ComError(-1,id,__FILE__,__LINE__);
int retry=DynamixelInterface::RETRIES;
int status=0;
int value=0;
do {
value=dxl_read_word(id,address);
retry--;
status=dxl_get_result();
// cout << status << endl;
if (!DXL_ComError::isOK(status)) {
// dxl_terminate();
usleep(10000);
// dxl_initialize(DEVICEINDEX,BAUDNUM);
}
}while (!DXL_ComError::isOK(status) && retry>0);
if (!DXL_ComError::isOK(status)) {
unsigned char b=dxl_read_byte(id,DXL_ALARM_SHUTDOWN_BYTE);
cout << "Alarm Shutdown Byte:"<<((int)b)<<endl;
throw DXL_ComError(status,id,__FILE__,__LINE__);
}
return value;
}
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 Dynamixel::readByte(int id, int address) {
return dxl_read_byte( id, address );
}
double DXLJointInterface::currentTemperature()
{
return dxl_read_byte(id(), REG_PRESENT_TEMPERATURE);
}
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;
}
int dynamixelApi_getVoltage(int actuator) {
int temp = dxl_read_byte( dyna.actuators[actuator], P_PRESENT_VOLTAGE );
checkForError(actuator,19);
return temp;
}
int dynamixelApi_getTemperature(int actuator) {
int temp = dxl_read_byte( dyna.actuators[actuator], P_PRESENT_TEMPERATURE );
checkForError(actuator,20);
return temp;
}
bool dynamixelApi_isMoving(int actuator) {
int moving = dxl_read_byte( dyna.actuators[actuator], P_MOVING);
checkForError(actuator,21);
return moving==1;
}
/**
* Checks is a barrier is ready to start moving towards the rotor.
* @param id The id of the dynamixele controlling the barrier
* @return 1 if the barrier is ready and 0 otherwise
*/
int ready(int id){
return !dxl_read_byte(id, MOVING);
}
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;
}
