Nash Lincoln

I’ve made a good bit of progress since last posting. I have some pictures I will post, and some code for the BasicStamp2 and the uClinux that runs on my 3rd Gen. iPod.

BS2 Code

' {$STAMP BS2}
' {$PBASIC 2.5}
' IR navigation with PID control via ipod/linux serial link
' Nash Lincoln, 2007
' --- vars ---
freqSelect VAR Nib
irFrequency VAR Word
irDetectLeft VAR Bit
rDetectRight VAR Bit
distanceLeft VAR Nib
distanceRight VAR Nib
speedLeft VAR Byte
peedRight VAR Byte
dirLeft VAR Bit
dirRight VAR Bit
' --- cons ---
iPodOut CON 11
iPodIn CON 10
rightTP CON 1
leftTP CON 2
baudMode CON 84
motorPin CON 5
' --- init --
HIGH 14
PAUSE 1000
LOW 14
' -----[ Subroutine - NoData ]-----------------------------------------
NoData:
HIGH 14
PAUSE 50
LOW 14
speedLeft = 0
speedRight = 0
GOSUB Drive_Motors
' -----[ Subroutine - Main ]-----------------------------------------
Main:
DO
' collect input
GOSUB Get_Ir_Distances
' output sensor data to ipod for processing/logging
SEROUT iPodOut, baudMode, [distanceLeft,distanceRight]
' input control data from ipod
SERIN iPodIn, baudMode,2000,NoData,[speedLeft]
SERIN iPodIn, baudMode,2000,NoData,[speedRight]
' send control signals
GOSUB Drive_Motors
LOOP
' -----[ Subroutine - Get_Distances ]-----------------------------------------
Get_Ir_Distances:
distanceLeft = 5
distanceRight = 5
FOR freqSelect = 0 TO 4
LOOKUP freqSelect,[37500,38250,39500,40500,41500], irFrequency
FREQOUT leftTP,1,irFrequency
irDetectLeft = IN3
distanceLeft = distanceLeft - irDetectLeft
FREQOUT rightTP,1,irFrequency
irDetectRight = IN0
distanceRight = distanceRight - irDetectRight
NEXT
RETURN
' -----[ Subroutine - Drive_ Motors ]-----------------------------------------
Drive_Motors:
dirRight = speedRight.HIGHBIT
IF dirRight = 1 THEN
speedRight = speedRight & $7f
ENDIF
dirRight = ~dirRight
dirLeft = speedLeft.HIGHBIT
IF dirLeft = 1 THEN
speedLeft = speedLeft & $7f
ENDIF
'left motor
SEROUT motorPin,84,[$80,0,4+dirLeft,speedLeft]
'right motor
SEROUT motorPin,84,[$80,0,6+dirRight,speedRight]
RETURN

C Code for the iPod running iPodLinux

#include 
#include 
#include 
#include 
#include 

#define MODEMDEVICE “/dev/ttyUSB0″
#define _POSIX_SOURCE 1 /* POSIX compliant source */

main(int argc, char *argv[])
{

char out1,out2;
int fd, c, res;
struct termios oldtio,newtio;
char buf[255];

fd = open(MODEMDEVICE, O_RDWR | O_NOCTTY );
if (fd < 0) {perror(MODEMDEVICE); exit(-1); }

tcgetattr(fd,&oldtio); /* save current port settings */

bzero(&newtio, sizeof(newtio));
newtio = oldtio;
newtio.c_iflag &= ~(ICRNL | INPCK | ISTRIP | IXON); // I'm not sure how many of these
newtio.c_iflag |= (BRKINT);
newtio.c_lflag &= ~(ICANON | ECHO | ISIG | IEXTEN);	// settings actually need setting.
newtio.c_oflag &= ~(OPOST);
newtio.c_oflag |= (ONOCR | ONLRET);
newtio.c_cc[VTIME]= 10;  /* inter-character timer */
newtio.c_cc[VMIN] = 2;   /* blocking read until n chars received */

tcflush(fd, TCIFLUSH);
tcsetattr(fd,TCSANOW,&newtio);

/////  PID controller variables
int fp = 10, fi = 1, df = 4; // PID weights
int anti_windup = 15; // max val of errorI to prevent integral windup
int errorPl, errorIl, errorDl, errorPpl = 0, totall, speedl = 0;
int errorPr, errorIr, errorDr, errorPpr = 0, totalr, speedr = 0;
int errorPoint = 5;
int max_speed = 50; // can be up to 127
/////

// if you want, pass in some args
// carefull, not much error checking here
if(argc == 6){
sscanf(argv[1],"%d", &fp);
sscanf(argv[3],"%d", &df);
sscanf(argv[2],"%d", &fi);
sscanf(argv[4],"%d", &anti_windup);
sscanf(argv[5],"%d", &max_speed);
}
c = 0;
while (c < 200) {   /* loop */
res = read(fd,&buf,255);   /* returns after n chars have been input */
buf[res] = 0; /* terminate string... */

/////////// PID for right wheel
errorPl = errorPoint - (2 * buf[0]);
errorDl = (errorPpl - errorPl);
errorIl += errorPl;
if(errorIl < -anti_windup) errorIl = -anti_windup;
else if(errorIl > anti_windup) errorIl = anti_windup;
errorPpl = errorPl;
totall = errorPl * fp + errorIl * fi + errorDl * df;
// make output a signed byte
speedl += totall;
if(speedl > max_speed) speedl = max_speed;
else if(speedl < -max_speed) speedl = -max_speed;
if(speedl < 0){
out2 = -speedl;
out2 |= 0x80;
}
else
out2 = speedl;
///////////
/////////// PID for left wheel
errorPr = errorPoint - (2 * buf[1]);
errorDr = (errorPpr - errorPr);
errorIr += errorPr;
if(errorIr < -anti_windup) errorIr = -anti_windup;
else if(errorIr > anti_windup) errorIr = anti_windup;
errorPpr = errorPr;
totalr = errorPr * fp + errorIr * fi + errorDr * df;
// scale output and make output a signed byte
speedr += totalr;
if(speedr > max_speed) speedr = max_speed;
else if(speedr < -max_speed) speedr = -max_speed;
if(speedr < 0){
out1 = -speedr;
out1 |= 0x80;
}
else
out1 = speedr;
///////////

if(c % 20 == 0){
// debug/display every nth iteration
printf("Input:  l:%d r:%dnOutput: l:%d r:%dn", buf[0],buf[1],speedl,speedr);
}
write(fd, &out1, 1);
usleep(1000);  // haven't tested smaller delay value yet.  should
write(fd, &out2, 1);

c++;
}
tcsetattr(fd,TCSANOW,&oldtio);
}

The code isn’t commented very well, and is a bit rough, so I’ll explain a bit of what’s going on.

The BS2 is connected to the iPod via the serial IO port on the iPod and pins 10 and 11 on the BS2.

I use a Pololu dual serial motor controller, so if you are using the continuous rotational servos you will have to modify the BS2 code appropriately. The main loop of the BS2 program writes 2 bytes of data — one byte for each distance reading IR emitter/detector pair, connected to BS2 pins 0 and 3 — and reads 2 bytes of data — speed settings, 1 byte for each wheel as a signed byte, from -127 to 127.
The c program is pretty straightforward, it reads 2 bytes of data and then runs through an iteration of a PID algorithm then outputs two values that correspond to left and right motor speed (-127 to 127 max, signed byte). You can specify weights via the command prompt or just use the defaults, which, again, are a little rough. The algorithm also features anti-windup. This program is hard-coded to point to /dev/ttyUSB0, you will probably have to change that or soft-link to /dev/ttyS0.

If you have any questions at all, please feel free to comment here or send email to nashira_lincoln @AT yahoo .dot com.

A few months back I decided to get a Boe-Bot robot from parallax with the Basic Stamp2 micro controller. It’s the first that I’ve ever played with electronics like this before and I was amazed by how easy it is to make good use of the various sensors and actuators that are so ubiquitous here in the 21st century.

After playing around I realized I need something with better ability to sense it’s environment. Photoresistors, ir emitters and detectors, even ultrasonic range finders meet their useful limit rather quickly without some extremely clever ideas. I don’t know if I’m being excessively anthropocentric, but it seems to me that some type of ability to detect light in the visible spectrum with decent resolution, say about that of a web cam, would be the minimum an autonomous robot would need to perform in a unknown environment. I don’t think any of the small commercial microprocessors on the market would work, and one of my eventual goals is to build a autonomous vehicle capable of successfully running a 1/16 — 1/20th scale version of one of the DARPA Grand Challenge courses, which is going to need a fairly powerful processor, at least megabytes of RAM rather than the couple hundred kilobytes that is typical of other micro controllers and all the storage it can get.

Then, while waiting for a load of laundry to finish and listening to my ipod the batteries went dead. I thought to myself, well it’s been a good run, 4 years and the battery only recently started going dead, I wondered what else I could do with this thing. And the idea was born, my 3rd gen ipod has and ARM7 32bit processor, 32 M ram, and a 20 G hard drive!! This is the king of micro controllers, prebuilt with readily available serial ports via the microphone jack that can be very easily hooked to a basic stamp for additional servo control, DC motor control, sensor input, etc. I’ve used the uClinux embedded operating system off and on for the past few years, but never got into hardware hacking and robotics until leaving grad school and had some more free time so I’m not even sure what can be done with the ipod in robotics yet, and my first, shallow google search hasn’t turned up any similar projects, so this will prove to be an exciting and educational experience.