Implementation and Serial Commands
From Experimental Robotics
Snake Issues
The snake does not have sufficient torque to lift itself much off the ground, as such, most of the walks, inching in particular need to use smaller angles, and thus heights than we would like.
How we implemented these walks
To implement the above walks, we had to use serial commands (shown below) to tell each individual servo what angle to assume.
In order to implement these walks, we created two arrays of angles, one for the horizontal motors and one for the vertical ones. Every timestep, the snake motors assume the corresponding angles in the arrays and then the arrays are incremented.
Treating the vertical and horizontal motors seperately gives us great flexibility. It so happens that it is necessary for the horizontal array to increment less often than the vertical ones in order for turning to be effective.
This array of angles approach also allows us to easily implement a variety of walks using the smae approach. For instance, inching simply makes the vertical motors alternate between two angle settings. Helix walk was also simple, since the horizontal motors are set to be the same as the vertical ones.
In fact, our two theories of turning were made very simple to implement thanks to this array structure.
Crescent Turn: Crescent turn is simply one where all horizontal motors assume an angle theta. This causes the snake to form a crescent shape, hence the name. The snake then moves along the perimeter of the imagined circle described by this arc.
Angle Turn: Angle turn was implemented by having, in the horizontal array, the last angle set to some angle we wished the motors to assume, this angle was then "rippled" along the array, causing each of the horizontal motors to assume that angle and then, after, return to the neutral stance. This was easily generalised to allow more than one motor to turn sequentially, to do this, the last angle of the array was set to the desired angle repeatedly n times as the array was incremented. Shown below is a snake with 5 horizontal motors turning using 2 motors:
130 130 130 130 130
130 130 130 130 90
130 130 130 90 90
130 130 90 90 130
...
90 90 130 130 130
90 130 130 130 130
130 130 130 130 130
Serial Code Commands to the Snake
Setting Up the Serial Port
To set up a connection to the serial port in C, you do as follows:
unsigned char outdata[6];
unsigned char indata[3];
fd = open("/dev/ttyS0", O_RDWR | O_NOCTTY, S_IRUSR | S_IWUSR);
if(fd == -1) {
perror("failed.\n");
} else {
printf("success.\n");
}
/* Get current attributes, add some more, then set the attrs. */
if (tcgetattr(fd, &term)) {
perror("tcgetattr");
}
cfmakeraw(&term);
//cfsetispeed(&term, B57600);
//cfsetospeed(&term, B57600);
bzero( &term, sizeof(struct termios) );
term.c_cflag = B57600 | CS8 | CLOCAL | CREAD;
term.c_iflag = 0;
term.c_oflag = 0;
term.c_lflag = 0;
term.c_cc[VTIME] = 0;
term.c_cc[VMIN] = 0;
// Initialise and load new attributes
if( tcflush(fd, TCIOFLUSH) < 0 )
{
perror("tcflush");
}
if (tcsetattr(fd, TCSANOW, &term)) {
perror("tcsetattr");
}
/* Make access to the file block (i.e., will wait for data on a read. */
if ( (flags = fcntl(fd, F_GETFL)) == -1) {
perror("fcntl");
}
if (fcntl(fd, F_SETFL, flags & O_NONBLOCK) == -1) {
perror("fcntl");
}
written = write(fd, outdata, 6);
//we sleep to give the serial port time to update
usleep(50000);
int n = read(fd, indata, 3);
printf("Read %d bytes of data: %d %d %d\n", n, indata[0], indata[1], indata[2]);</nowiki>
Setting Motor ID
In order to set a motor's ID, we send a packet down the serial port to the snakebot. Since we may not know what the old id is, we send packets in a loop, essentially, a message for every possible old id (0 - 30). This ensures that whatever the motors id may have been, it is now set to the new id. This of course means that motors must only be set one at a time, or they'll all be assigned the new id.
The 6 bytes sent are as follows where id can be from 0-30.
outdata[0] = 0xFF;
outdata[1] = 224 + old_id;
outdata[2] = 0x0A;
outdata[3] = id;
outdata[4] = id;
outdata[5] = (outdata[1] ^ outdata[2] ^ outdata[3] ^ outdata[4]) & 0x7F;
Setting Motor Position
After setting an id, you may wish to test the motor responds to the proper id. A packet can be sent commanding the motor to assume a position. this position is from 0-360. Obviously, the snake cannot assume all these angles, given its configuration. The angles the snake servos can assume are from 90 to 180, (following this framework, 130 has been found to be the "neutral" position, at least as far as the snake is concerned).
The 4 bytes sent are as follows:
outdata[0] = 0xFF;
outdata[1] = id;
outdata[2] = position;
outdata[3] = (outdata[1] ^ outdata[2]) & 0x7F;
