Typecasting bytes properly for writing to a serial handle

[ramabrahma]

Background:

I want to control the "Trossen Robotics PhantomX Reactor Robot Arm" using an XBOX 360 Controller and ROS. The robot arm is using an Arbotix microcontroller based off an arduino and has firmware loaded on it that works with an arduino-based controller that has similar button/joystick mappings to the xbox 360 controller. I've tried to port the Arduino code of the CONTROLLER into a ROS Node or ROS code (based on C++).

The robot arm firmware should receive an 8 Byte packet consisting of

Byte 1 - Header - 0xFF
Byte 2 - Right Joy Vertical - value between -100 to 100 shifted up by 128
Byte 3 - Right Joy Horizontal - value between -100 to 100 shifted up by 128
Byte 4 - Left Joy Vertical - value between -100 to 100 shifted up by 128
Byte 5 - Right Joy Horizontal - value between -100 to 100 shifted up by 128
Byte 6 - Button Values - 8 bit byte where each bit is mapped to a button
Byte 7 - Extended Inst. - "0" not needed
Byte 8 - Checksum

Problem:

I'm trying to create an 8 byte packet that has a 0xFF as a header byte, integers and bitwise mappings where each needs to be converted to unsigned char or bytes. The packet is then sent through a serial comm. handle.

My issue is with typecasting and making sure the data is being casted from integer and bitwise values to bytes properly, and being sent in a correct sequence to the serial handle. How do I typecast and do this properly based on the code given below?

Code clarification

btn is a struct consisting of double precision joystick values
cmd[] is an unsigned char array of size 8
sp->write(const char * data, int length) is a wrapper for the fstream write function. I've included the definition just in case.
ROS_INFO is a wrapper for printf for outputting to a ROS Console

// *****************************************************************************
// Create the Commander unsigned char packet as per the Arbotix Commander specifications 
int PhantomXROS::arbotixCmdPackage(cont_value btn)
{
	
	int d_cmd=0;
	unsigned char st_code;

	std::stringstream ss;
	ss << std::hex << "0xFF";
	ss >> st_code;

	//Mapping joystick values to the servo values of the robot arm
	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;

	
	
	//Bytes 1-5 Convert integers to bytes
	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;
	
	//Byte 6 - Assign a button ON/OFF to its corresponding bit
	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;
	
	//Byte 7 - Extended instruction - none, therefore 0
	this->cmd[6] = (unsigned char)0;

	//Byte 8 - Checksum
	this->cmd[7] = (unsigned char)((255 - (right_V+right_H+left_V+left_H+d_cmd)%256));

	//Reinterpret the cmd byte array to an 8 char string
	std::string buf(reinterpret_cast<const char*>(cmd), 8);
	
	//write to the arbotix serial handle
	try{
		sp_->write(buf.c_str(),buf.size());	

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

		
	//Output data to the ROS console
	if(this->timer > 500)
	{
		
			
		ROS_INFO("Arbotix Cmd right_v has written: %d", right_V);
		ROS_INFO("Arbotix Cmd right_h has written: %d", right_H);
		ROS_INFO("Arbotix Cmd left_v has written: %d", left_V);
		ROS_INFO("Arbotix Cmd left_h has written: %d", left_H);
		ROS_INFO("Arbotix Cmd d_cmd has written: %d", d_cmd);
		ROS_INFO("String command: %s",buf.c_str());

		this->timer = 0;
		
	}
	
	
	return 0;
	
	
}

int cereal::CerealPort::write(const char * data, int length)
00146 {
00147         int len = length==-1 ? strlen(data) : length;
00148 
00149         // IO is currently non-blocking. This is what we want for the more cerealon read case.
00150         int origflags = fcntl(fd_, F_GETFL, 0);
00151         fcntl(fd_, F_SETFL, origflags & ~O_NONBLOCK); // TODO: @todo can we make this all work in non-blocking?
00152         int retval = ::write(fd_, data, len);
00153         fcntl(fd_, F_SETFL, origflags | O_NONBLOCK);
00154 
00155         if(retval == len) return retval;
00156         else CEREAL_EXCEPT(cereal::Exception, "write failed");
00157 }