PDA

View Full Version : ROS Port of Commander.ino for PhantomX Reactor Arm



ramabrahma
06-24-2013, 01:48 PM
I'm porting the Commander.ino code into ROS for use with an xbox 360 controller. I want to use the 360 controller to move my arm.

The firmware I have loaded into my Arbotix microcontroller on my PhantomX is PhantomX_Reactor_IK.ino.

At the moment I'm having issues with typecasting and writing bytes to the microcontroller through ROS.

A bit of clarification on the code below.


btn is a struct consisting of double precision joystick values
cmd[] is an unsigned char array of size 8
I'm using the cereal_port package to handle the serial communication.

My issue is that even with the typecasting to (unsigned char) for each command, I don't think the commands are being output correctly to the serial stream. What am I doing wrong?

Please check out the code below:


int PhantomXROS::arbotixCmdPackage(cont_value btn)
{

int d_cmd=0;
int right_V=this->map(btn.rightV, -1, 1, -100, 100)+128;
int right_H=this->map(btn.rightH, -1, 1, -100, 100)+128;
int left_V=this->map(btn.leftV, -1, 1, -100, 100)+128;
int left_H=this->map(btn.leftH, -1, 1, -100, 100)+128;

//Hex conversion
unsigned char st_code;
std::stringstream ss;
ss << std::hex << "0xFF";
ss >> st_code;

this->cmd[0] = (unsigned char)st_code;
this->cmd[1] = (unsigned char)right_V;
this->cmd[2] = (unsigned char)right_H;
this->cmd[3] = (unsigned char)left_V;
this->cmd[4] = (unsigned char)left_H;

d_cmd += (btn.R1 > 0) ? 1 : 0;
d_cmd += (btn.R2 > 0) ? 2 : 0;
d_cmd += (btn.R3 > 0) ? 4 : 0;
d_cmd += (btn.L4 > 0) ? 8 : 0;
d_cmd += (btn.L5 > 0) ? 16 : 0;
d_cmd += (btn.L6 > 0) ? 32 : 0;
d_cmd += (btn.RT > 0) ? 64 : 0;
d_cmd += (btn.LT > 0) ? 128 : 0;

this->cmd[5] = (unsigned char)d_cmd;
this->cmd[6] = (unsigned char)0;
this->cmd[7] = (unsigned char)((255 - (right_V+right_H+left_V+left_H+d_cmd)%6));


std::string buf(reinterpret_cast<const char*>(cmd), 8);

try{


sp_->write(buf.data(),buf.size());

}
catch(cereal::Exception& e){
ROS_INFO("Could not write to Arbotix:");
//ROS_BREAK();
return(-1);
}




return 0;


}

ramabrahma
07-04-2013, 07:24 PM
I've been able to get the joystick working with the arm. For the C++ code, all I had done was change the block:



std::string buf(reinterpret_cast<const char*>(cmd), 8); try{ sp_->write(buf.data(),buf.size()); } catch(cereal::Exception& e){ ROS_INFO("Could not write to Arbotix:"); //ROS_BREAK(); return(-1); }


To:


try{



sp_->write(reinterpret_cast<const char*>(cmd), 8);




}
catch(cereal::Exception& e){
ROS_INFO("Could not write to Arbotix:");
//ROS_BREAK();
return(-1);
}


In the PhantomX_Reactor_IK.ino, I changed the command.begin(38400) line to Serial.begin(38400) . command.begin is a wrapper for the serial.begin and was causing read/write issues to the arm.

My PhantomX Robot arm now officially works with the xbox 360 joystick with ROS running it. So happy! :)