Source code for omni-directional platform

[ibwood]

Hi everyone,

I'm new to these forums, but I thought it looked cool so I decided to try it out

Anyways, I am researching an omni-directional platform. It has three degrees of freedom and drives with four mecanum wheels. There are also four motors, and two motor controllers to control the motors (two channels in each). Finally, I have a large circuit set up for joystick operation, powered by a 12 V battery. At the end of this circuit I have a microcontroller chip, that takes in commands from the joystick and outputs it. I have my code written, and it compiles, but I still have a feeling that something I did isn't quite right yet.. I would like to get some opinions on what I have so far.

I have used ROS (Robot Operating System) in my c++ code to execute joystick node operation. If you have not encountered this language yet do not worry about it. Either way, I have explained it in the code. The motor controller used in this project is Roboteq's AX2850. The microcontroller used is an MC9S12DT256MFUE model.

Finally, please let me know if you have any other questions regarding this research.

The following program is run under the Linux Ubuntu OS with Karmic Koala (9.10) distro:

Code: [Select]

#include "ros/ros.h"
 #include "std_msgs/String.h"

 #include <sstream>
 #include <cmath>
 #include <iostream>

 #include <sys/ioctl.h>
 #include <sys/types.h>
 #include <sys/stat.h>
 #include <stdio.h>
 #include <limits.h>
 #include <string.h>
 #include <fcntl.h>
 #include <errno.h>
 #include <termios.h>
 #include <unistd.h>

 #define PI 3.14159265358979323846264338327950288419716

 using namespace std;

 const double CIRC=0.7853981633974483, R=0.55, ratio=127; // initialize constants

 int main(int argc, char **argv)
 {
int ms1, ms2, ms3, ms4; //motor speeds of omnimaxbot
int i, j, wr, rd, x=0, y=0, z=0, fd, fd1, fd2, numSent=0;
  char parseChar[1], stringIn[50], mc_char;

  // unsigned long bytes_read = 0; //Number of bytes read from port
  // unsigned long bytes_written = 0; //Number of bytes written to the port




  // attempt to open serial port connected to the microprocessor

  fd = open("/dev/HCS12",O_RDWR | O_NOCTTY | O_NDELAY);
 
// set settings for HCS12 microcontroller

  system("stty -F /dev/HCS12 115200 cs8 -cstopb -parity -icanon hupcl -crtscts min 1 time 1");


  // check for errors opening port

  if (fd == -1 )

  {

printf("open_port: Unable to open /dev/HCS12\n");

  }

 
  else // if no error

  {

  fcntl(fd,F_SETFL,0);

printf("Test Port HCS12 has been successfully opened and %d is the file description\n",fd);

  }

//open serial port connected to motor driver for motors 1 and 2
fd1 = open("/dev/Driver12",O_RDWR | O_NOCTTY | O_NDELAY);

//settings for motor drivers
system("stty -F /dev/Driver12 9600 cs8 -cstopb -parity -icanon hupcl -crtscts min 1 time 1");

if (fd1 == -1 )

{

perror("open_port: Unable to open /dev/Driver12\n");

}

else // if no error

  {

  fcntl(fd1,F_SETFL,0);

printf("Test Port Driver12 has been successfully opened and %d is the file description\n",fd1);

  }

//open serial port connected to motor driver for motors 3 and 4
fd2 = open("/dev/Driver34",O_RDWR | O_NOCTTY | O_NDELAY);

//settings for motor drivers
system("stty -F /dev/Driver34 9600 cs8 -cstopb -parity -icanon hupcl -crtscts min 1 time 1");

if (fd2 == -1 )

{

perror("open_port: Unable to open /dev/Driver34\n");

}

else // if no error

  {

  fcntl(fd2,F_SETFL,0);

printf("Test Port Driver34 has been successfully opened and %d is the file description\n",fd2);

  }


// initalize node
ros::init(argc, argv, "joyTalker");
ros::NodeHandle joyTalker;

// Publish to topic joyChatter
ros::Publisher pub = joyTalker.advertise<std_msgs::String>("joyChatter", 1000);

ros::Rate r(10);

while (joyTalker.ok())
{
parseChar[0]=0;  

for(j=0; j<50; j++)
{
stringIn[j]=0; //initialize the string
}

i=0;

while(parseChar[0] != 10)
{
if(i==50) // exceeds the allowable size
{
break;
}

rd = read(fd,parseChar,1); //read values in from the joystick

if ((parseChar[0] > 43) && ((parseChar[0] < 58) || (parseChar[0]==32))) //ASCII characters
                        {
stringIn[i]=parseChar[0];
i++;
}

// kinematics of the omnimaxbot according to mecanum wheels
ms1 = ((x/sqrt(2))+y+(R*tan((z/ratio)*(2*PI))))/CIRC;
ms2 = (-(x/sqrt(2))+y+(R*tan((z/ratio)*(2*PI))))/CIRC;
ms3 = ((x/sqrt(2))+y-(R*tan((z/ratio)*(2*PI))))/CIRC;
ms4 = (-(x/sqrt(2))+y-(R*tan((z/ratio)*(2*PI))))/CIRC;

//motor controller language, specified by an exclamation mark, a character (a or A is channel 1, b or B is channel 2), and then the speed

if(ms1<0) //Motor Controller 1, Channel 1, Motor 1
{
ms1=-ms1;
mc_char = 'a'; //move backwards
}
else
{
mc_char = 'A'; //move forwards
}
//write/send data to the specified port
numSent = sprintf(stringIn, "!%c%d", mc_char, ms1);
wr = write(fd1,stringIn,numSent);

if(ms2<0) //Motor Controller 1, Channel 2, Motor 2
{
ms2=-ms2;
mc_char = 'b'; //move backwards
}
else
{
mc_char = 'B'; //move forwards
}

numSent = sprintf(stringIn, "!%c%d", mc_char, ms2);
wr = write(fd1,stringIn,numSent);

if(ms3<0) //Motor Controller 2, Channel 1, Motor 3
{
ms3=-ms3;
mc_char = 'a';
}
else
{
mc_char = 'A';
}
numSent = sprintf(stringIn, "!%c%d", mc_char, ms3);
wr = write(fd2,stringIn,numSent);

if(ms4<0) //Motor Controller 2, Channel 2, Motor 4
{
ms4=-ms4;
mc_char = 'b';
}
else
{
mc_char = 'B';
}
numSent = sprintf(stringIn, "!%c%d", mc_char, ms4);
wr = write(fd2,stringIn,numSent);      
}
printf("got this string %s\n",stringIn);

// convert message to string for ROS node communication
std_msgs::String msg;
std::stringstream ss;
ss << stringIn;
ROS_INFO("%s", ss.str().c_str());
msg.data = ss.str();
// publish data under topic joyChatter
pub.publish(msg);

ros::spinOnce();
}
 
 return 0;
 }