Making the photo interrupter mechanism and robot base, location programming continues

 The interrupter mechanism is used to sense how far each motor has moved, and in doing so record the position of the gantry crane at all times.To do this I attached a small cut out shape which will block the signal going through the photo interrupter as it spins. This was made using 3 layers of 1mm styene sheet. The first main layer was in the shape seen below. It is essentially a circle 40mm diameter, split in 4 equal segments, with two of the opposite segments cut out. This was done working around a concentric circle diameter 25mm. The drawing of this is underneath (not to scale). The other two layers were just two small circles of styrene approx. 15mm in diameter that were punched out by a hollow punching tool. These were glued together using styrene glue and a 4mm hole was drilled in the centre using a vertical milling machine to pass through the motor.

Below are some pictures of the finished x and y wheel attached (only with blu tack at the moment):

 The other major change was, a base for the robot has been added. This is just a 320 x 450 x 6 mm wooden board that was cut to size with the ban saw and then sanded down. Using the vertical milling machine again, holes were drilled either side of where each leg would go. A cable tie was then fed through the lego and holes, and secured to mount the robot to the base. The circuit board is not attached at the moment but will be soon using Hex Standoffs.

The most work has gone into the code today. After much fiddling with the code, interrupts just didn't want to work, so this has all been replaced using other alternatives. This has resulted in quite a large messy code with many variables but the main point is it functions as needed. In addition, an index has been created of commands that can be run in serial. These are:
xf=X goes forward
xs=X stops
xr=X reverses
yf=Y goes forward
ys=Y stops
yr=Y reverses
re= reads position
xy " " " "= tells robot to go to that coordinate
This allows for effective testing and manual control. The one issue is with accuracy of the x and y coordinates. The theory is the EMR of each wire may be causing interference with the other causing the photo-interrupter to send incorrect values. This will be rectified by cleaning up the wiring and possibly using shielded cabling. The code at this point is below:

//Automated Gantry Warehousing Robot
const int Xdir=6;
const int Ydir=4;
const int Xenable=5;
const int Yenable=10;
int Xpulse=3;
int Ypulse=2;
boolean Xdirection=false;
boolean Ydirection=false;
int Xposition=0;
int Yposition=0;
boolean Xtravel = false;
boolean Ytravel = false;
int OldXstate, OldYstate;
String inputString = "";
boolean stringComplete=false;
String cmd = "";
int arg [4] = {0,0,0,0};
int ArgIndex;

void setup()
{
  Serial.begin(9600);
  Serial.println(" Automated Gantry Warehouse");
  pinMode(Xdir, OUTPUT);
  pinMode(Ydir, OUTPUT);
  pinMode(Xenable, OUTPUT);
  pinMode(Yenable, OUTPUT);
  pinMode(Xpulse,INPUT);
  pinMode(Ypulse,INPUT);
  OldXstate = digitalRead(Xpulse);
  OldYstate = digitalRead(Ypulse);
}

void loop(){
  CheckEncoders();
  if (Xtravel) if (Xposition == arg[0]) XStop();
  if (Ytravel) if (Yposition == arg[1]) YStop();

  if (stringComplete){
  HandleCommand();
  inputString="";
  stringComplete=false;
  }
}

void ParseInput(){
  int space, slash;
// Serial.println(inputString);          // only for debugging
  for(int x = 0; x < 4; x++){              // flush the last args
    arg[x] = 0;
  }
  ArgIndex = 0;
  space=inputString.indexOf(' ');
  slash = inputString.indexOf('\n');
  inputString = inputString.substring(0,slash);
  cmd = inputString.substring(0,space);
  inputString = inputString.substring(space+1,slash);
  while (inputString.indexOf(' ') > 0){
    space = inputString.indexOf(' ');
    arg[ArgIndex] = inputString.substring(0,space).toInt();
    inputString = inputString.substring(space+1,inputString.length());
   ArgIndex++;  
  }
    arg[ArgIndex] = inputString.toInt();
//    Serial.println(arg[0]);
  if (arg[1] != 0) arg[1] = arg[1] * 1000;
}

void HandleCommand(){
  ParseInput();
  if (cmd=="xf") XForward();
  if (cmd =="xs") XStop();
  if (cmd=="xr") XReverse();
  if (cmd=="yf") YForward();
  if (cmd =="ys") YStop();
  if (cmd=="yr") YReverse();
  if (cmd == "re") ReadEncoders();
  if (cmd =="xy") PickUp();
}
 
void PickUp(){
  if (arg[0] > Xposition) XForward();
  if (arg[0] < Xposition) XReverse();
  Xtravel = true;
  if (arg[1] > Yposition) YForward();
  if (arg[1] < Yposition) YReverse();
  Ytravel = true;
}

void SetXForward(){
 digitalWrite(Xdir,HIGH);
 Xdirection= true;
}

void SetXReverse(){
 digitalWrite(Xdir,LOW);
 Xdirection= false;
}

void ReadEncoders(){
    Serial.print("X= ");
    Serial.print(Xposition);
    Serial.print( "Y= ");
    Serial.println(Yposition);
}
 
void CheckEncoders(){
  int X,Y;
  X = digitalRead(Xpulse);
  Y = digitalRead(Ypulse);
  if (OldXstate == LOW){
      if(X ==HIGH){
          if (Xdirection) Xposition++;
          if (!Xdirection) Xposition--;
          OldXstate = X;
          goto  CheckY;
       //   Serial.println(Xposition);
      }
    }
  if (OldXstate == HIGH){
      if (X == LOW){
          if (Xdirection) Xposition++;
          if (!Xdirection) Xposition--;
          OldXstate = LOW;
  //        Serial.println(Xposition);
      }
  }
  CheckY:
   if (OldYstate == LOW){
      if(Y ==HIGH){
          if (Ydirection) Yposition++;
          if (!Ydirection) Yposition--;
          OldYstate = Y;
 //         Serial.println(Yposition);
          return;
      }
    }
  if (OldYstate == HIGH){
      if (Y == LOW){
          if (Ydirection) Yposition++;
          if (!Ydirection) Yposition--;
          OldYstate = LOW;
  //        Serial.println(Yposition);
      }
  }
}
 
void Test(){
  XForward();
  delay(1250);
  XStop();
  delay(1000);
  XReverse();
  delay(1250);
  XStop();
  delay(1000);

  YForward();
  delay(500);
  YStop();
  delay(1000);
  YReverse();
  delay(500);
  YStop();
  delay(1000);
}
void XForward(){
  Xdirection=true;
  digitalWrite(Xenable,LOW);
  delay(30);
  digitalWrite(Xdir,HIGH);
  digitalWrite(Xenable,HIGH);
}

void YForward(){
  Ydirection=true;
  digitalWrite(Yenable,LOW);
  delay(30);
  digitalWrite(Ydir,HIGH);
  digitalWrite(Yenable,HIGH);
}

void XReverse(){
  Xdirection=false;
  digitalWrite(Xenable,LOW);
  delay(30);
  digitalWrite(Xdir,LOW);
  digitalWrite(Xenable,HIGH);
}

void YReverse(){
  Ydirection=false;
  digitalWrite(Yenable,LOW);
  delay(30);
  digitalWrite(Ydir,LOW);
  digitalWrite(Yenable,HIGH);
}

void XStop(){
  digitalWrite(Xenable,LOW);
  digitalWrite(Xdir,LOW);
  Xtravel = false;
}

void YStop(){
  digitalWrite(Yenable,LOW);
  digitalWrite(Ydir,LOW);
}

void PrintPosition(){
  Serial.print(Xposition);
  Serial.print("  ");
  Serial.println(Yposition);
}

void serialEvent(){
  while(Serial.available()){
   char inChar=(char)Serial.read();
   inputString += inChar;
   if(inChar=='\n'){
       stringComplete=true;
   }
  }
}