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.