Bit Math/Code Structure Porblems

Ok. I am in a bit of a dilemma. I am trying to combine an ATmega168 micro with a BS2e. I need to 'translate' the following code from PBASIC 2.5 to Arduino syntax. Here's the PBASIC code:

' {$STAMP BS2e}
' {$PBASIC 2.5}

' -----[noparse][[/noparse] Variables/Constants ]--------------------------------------------------------------------------------------------

i             VAR   Byte              ' loop counter, whatever
tmp           VAR   Word              ' temporary holder
seed          VAR   Word              ' random number seed

' these are for the servo routines

LEFT    CON    13                     ' left wheel port
RIGHT   CON    12                     ' right wheel port
SACT    CON    5                      ' times through act routine
drive   VAR    Word                     ' wheel command combo
ldrive  VAR    drive.BYTE1                ' left wheel command
rdrive  VAR    drive.BYTE0                ' right wheel command
aDur    VAR    Byte                     ' duration of pulse left

' servo drive commands

fd    CON    $6432                    ' forward (left = 100 ((16*6)+4), right = 50 ((16*3)+2))
rv    CON    $3264                    ' reverse (left = 50 ((16*3)+2), right = 100 ((16*6)+4))
st    CON    $4b4b                    ' STOP (left = 75 ((16*4)+11), right = 75 ((16*4)+11))
tr    CON    $644b                    ' turn right (left = 100 ((16*6)+4), right = 75 ((16*4)+11))
tl    CON    $4b32                    ' turn left (left = 75 ((16*4)+11), right = 50 ((16*3)+2))
rr    CON    $6464                    ' rotate right (left = 100 ((16*6)+4), right = 100 ((16*6)+4))
rl    CON    $3232                    ' rotate left (left = 50 ((16*3)+2), right = 50 ((16*3)+2))

' wander values

wDir    VAR    Word 'wander value
wDur    VAR    Byte 'wander duration


Main:                           ' The program starts here

  GOSUB wander
  GOSUB act

GOTO Main

avdo:

  LOOKUP i,[noparse][[/noparse]rr, rl, drive],tmp
  drive = tmp
  i = 0                             'clear history

avdone:

  RETURN

wander:                            ' randomly wander around
  IF wDur > 0 THEN wDone1
  RANDOM seed                        ' random direction
  i = seed & %111                      ' mask off for 0-7 only
  LOOKUP i,[noparse][[/noparse]fd,tl,fd,fd,fd,fd,tr,fd],wDir      'chose direction
  seed = seed + i
  wDur = (seed & %111111) + 20             ' mask for 64 choices of duration

wDone1:
  wDur = wDur - 1                      'decrement wander counter
  drive = wDir                         'get direction
  RETURN                             'completed

act:                               ' moves servo motors
    IF aDur > 0 THEN aDec                ' already doing one, got here
      aDur = SACT                    ' times through this one
      PULSOUT LEFT,ldrive * 10
      PULSOUT RIGHT,rdrive * 10

aDec:                              ' decrement stuff
      aDur = aDur - 1

aDone:
RETURN

Now the following is the Arduino code. It should be able to operate EXACTLY the same as the PBASIC code.

boolean irDetectLeft;
boolean irDetectCenter;
boolean irDetectRight;
byte pulseCount;
int distance;
int oldDistance;
int counter;
byte task;
int micVal;
int cdsVal;
int irLval;
int irCval;
int irRval;

int i;
int tmp;
int seed;

int wDir;
byte wDur;

int LeftPin = 10;
int RightPin = 9;
int PiezoPin = 11;
int PingPin = 5;
int PingServoPin = 6;
int irLPin = 2;        // Analog 0
int irCPin = 1;        // Analog 1
int irRPin = 0;        // Analog 2

#define BAUD 9600
#define SACT 5
#define fd   0x6432                    // forward (left = 100 ((16*6)+4), right = 50 ((16*3)+2))
#define rv   0x3264                     // reverse (left = 50 ((16*3)+2), right = 100 ((16*6)+4))
#define st   0x4b4b                    // STOP (left = 75 ((16*4)+11), right = 75 ((16*4)+11))
#define tr   0x644b                    // turn right (left = 100 ((16*6)+4), right = 75 ((16*4)+11))
#define tl   0x4b32                    // turn left (left = 75 ((16*4)+11), right = 50 ((16*3)+2))
#define rr   0x6464                    // rotate right (left = 100 ((16*6)+4), right = 100 ((16*6)+4))
#define rl   0x3232                    // rotate left (left = 50 ((16*3)+2), right = 50 ((16*3)+2))
int drive;
byte ldrive, rdrive;
byte aDur;

void setup() {
  Serial.begin(9600);
  ldrive = (byte)(drive >>8);
  rdrive = (byte)(drive & 0xff);
  pinMode(PiezoPin, OUTPUT);
  pinMode(LeftPin, OUTPUT);
  pinMode(RightPin, OUTPUT);
  pinMode(irLPin, INPUT);
  pinMode(irCPin, INPUT);
  pinMode(irRPin, INPUT);
  for(i = 0; i < 500; i++) {
    digitalWrite(PiezoPin, HIGH);
    delayMicroseconds(1000);
    digitalWrite(PiezoPin, LOW);
    delayMicroseconds(1000);
  }
  i = 0;    
}

void loop()
{
  wander();
  act();
  //irCval = analogRead(irCPin);
  //Serial.print("Val: ");
  //Serial.println(irCval);
  //delay(200);
}
void avdo() {
  switch(i) {
    case 0:
    tmp = rr;
    case 1:
    tmp = rl;
    case 2:
    tmp = drive;
  }
  drive = tmp;
  i = 0;
}
void avdone() {
  return;
}
void wander() {
  if(wDir > 0) {
    wDone1();
  }
  seed = random(seed);
  i = seed & B11;
  switch(i) { 
    case 0: 
    wDir = fd;
    case 1:
    wDir = tl;
    case 2:
    wDir = fd;
    case 3:
    wDir = fd;
    case 4:
    wDir = fd;
    case 5:
    wDir = fd;
    case 6:
    wDir = tr;
    case 7:
    wDir = fd;
  }
  seed = seed + i;
  wDur = (seed & B111111) + 20;
}
void wDone1() {
  wDur = wDur - 1;
  drive = wDir;
  return;
}
void act() {
  if(aDur > 0) {
    aDec();
  }
  aDur = SACT;
  digitalWrite(LeftPin, HIGH);
  delayMicroseconds(ldrive * 10);
  digitalWrite(LeftPin, LOW);
  delay(20);
  digitalWrite(RightPin, HIGH);
  delayMicroseconds(rdrive * 10);
  digitalWrite(RightPin, LOW);
  delay(20);
}
void aDec() {
  aDur = aDur - 1;
}
void aDone() {
  return;
}

'Commented out' sections of code in the Arduino code are preceded by '//'. Ignore any commented-out sections. There are some variables in there that I'm not using at the moment; ignore them.

The problem I have been facing with this code is that when I hook everything up, the Arduino ONLY sends pulses to the servo connectedto pin 9; if it used both servos I'm not sure this code is even working the way it should yet. I switched the servos around just in case there was something wrong with one; there wasn't. Something is wrong with either the code structure, or the bitwise math. I have a really hard time grasping the programming concept of bitwise math, so I had some people on another forum help me out with that part of the 'translation'. The code section in PBASIC

ldrive  VAR    drive.BYTE1                ' left wheel command
rdrive  VAR    drive.BYTE0                ' right wheel command

is translated to

  ldrive = (byte)(drive >>8);
  rdrive = (byte)(drive & 0xff);

...in Arduino code. I have NO idea how that code section works, or even if it's right. Is that part of code wrong?

All help would be greatly appreciated. Thanks.

▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔
Giggly Googley!