Robot Arm with Machine vision

Hi
First I would like to thank Conor Peterson for the Code to read ADNS 2610 that I have  adapted by deleteing a lot of it.
conorpeterson.wordpress.com optical-mouse-hacking

And Tim for a circuit diagram etc to  get the Chip working.
Outguessing The Machine

And Also Mr Faciens
HomoFaciens Motion detection with a computer mouse and an Arduino Uno

The A2610 Optial mouse Chip available on ebay.

  

Copy/Paste the code into Arduino Sketch./////////////////////////////////////////////////////////////////////////////////////////#include <Servo.h>#define FRAMELENGTH 324  // number of pixels on A2610 chip 18 x 18#define SCLK 2 // pin 2#define SDIO 3 // pin 3int direct = 0;//servo objectsServo Arm0Servo;Servo Arm1Servo;Servo Arm2Servo;Servo Arm3Servo;int b = 15;   // a & b so I can adjust values without editing whole array.int a = 105;int PosArm[4] ;       int ArmPos[10][4] = {  {a, 43, b, 130},       // positions to sweep area looking for shiney things  {a, 43, b, 100},  {a, 43, b, 60},  {a, 37, b, 60},  {a, 37, b, 100},  {a, 37, b, 130},  {a, 32, b, 100},  {a, 32, b, 100},  {a, 32, b, 100},  {88, 40, 14, 160}   // position of cup to drop marble in};   // array to hold arm positions up to 100int PosCount = 0;     //  to count number of positions increased when button pressedint PosCountMax = 8;  //  number of positions recorded when double button push initiates replay modeint PosReached = 0;   // used to check that arm has moved from one recorded position to next positionint mode, seeNothing, e, h, xD, yD = 0;////////////////////////Camera Variablesbyte frame[FRAMELENGTH];int sync = 0;int x = 0;int y = 0;int xPix = 0;int yPix = 0;int posCount = 0;int pos = 0;const byte regConfig    = 0x00; //a2620 . A2610 =0x00const byte regStatus    = 0x01; //a2620 . A2610 =0x01const byte regPixelData = 0x08; //a2620 . A2610 =0x08const byte maskNoSleep  = 0x01; // unchanged for a2620const byte maskPID      = 0xE0; // idemconst byte regYmov = 0x02; // a2620const byte regXmov = 0x03;  //a2620////////////////////////////void setup() {  // attach servos to relervent pins on arduino nano  //  attach(12) means servo wire has to go to pin 12  Arm0Servo.attach(12);// grip    90 to 180 open    limits of servo movement  // these are the figures for my arm yours will probably be different  PosArm[0] = 120;  Arm0Servo.write(PosArm[0]);  Arm1Servo.attach(11); // 0 in   30 out// elbow    0  to 130 up  PosArm[1] = 12;  Arm1Servo.write(PosArm[1]);  Arm2Servo.attach(10); // shoulder   10 to 50 down  PosArm[2] = 10;  Arm2Servo.write(PosArm[2]);  Arm3Servo.attach(9); //   70 right  120 left // turn    0  to 180 right  PosArm[3] = 100;  Arm3Servo.write(PosArm[3]);
  //// Camera  pinMode(SCLK, OUTPUT);  pinMode(SDIO, OUTPUT);  /// Serial comms  Serial.begin(38400);  Serial.println("Serial established.");  Serial.flush();
  mouseInit();  dumpDiag();}
void loop() {  if ((xPix == -1) && (yPix == -1)) {    if (seeNothing++ > 3)  mode = 0;  } else seeNothing == 0;  switch (mode) {    case 0 :        // search sweep area      PosReached = 0;      PosCount = 0;      while (true) {        PosReached = 0;        for (int i = 0; i < 4 ; i++ ) { //adjust servo positions          // we move the servos in small steps and the delay(20)  makes arm motion smooth and slow          if ( PosArm[i] > ArmPos[PosCount][i]) PosArm[i] = PosArm[i] - 1; // if actual position is greater than requird position reduce position by 1          if ( PosArm[i] < ArmPos[PosCount][i]) PosArm[i] = PosArm[i] + 1; // if actual position is less than required position value increase position valuue by 1          if ( PosArm[i] == ArmPos[PosCount][i]) PosReached++;             // check if servo  has reached required position/angle        }        if (PosReached == 4) PosCount++;       // if all 4 servos have reached position  then increase array index (PosCount)to next position in array
        if (PosCount == PosCountMax) PosCount = 0;  //  if end of array reached reset index to 0 and repeat.        MoveArm();                         // call method        readFrame(frame);        if ((xPix > -1) && (yPix >  -1)) {          mode = 1;          break;        }      }      break;    case (1):     // something seen   zero in      if (PosArm[1] < 20) {        e = 4;        h = 44;      }      if ((PosArm[1] > 19) && (PosArm[1] < 40)) {        e = 5;        h = 46;      }      if ((PosArm[1] > 39) && (PosArm[1] < 45)) {        e = 5;        h = 47;      }      if ((PosArm[1] > 44) && (PosArm[1] < 53)) {        e = 6;        h = 50;      }      if (PosArm[1] > 52) {        e = 7;        h = 52;      }      readFrame(frame);      if  ((yPix > e - 2) && (yPix < e + 2)) {        PosArm[2] += 1;        if (PosArm[2] > h) PosArm[2] = h;        if (PosArm[2] < 10) PosArm[2] = 11;        MoveArm();      // call method        if ((PosArm[2] == h) && (yPix == e)) {          mode = 2;          break;        }      }      if (xPix > 8) xD = -1;      if (xPix < 8) xD = 1;      if (yPix > e) yD = -1;      if (yPix < e) yD = 1;      PosArm[3] -= xD;      if (PosArm[3] < 70) PosArm[3] = 71;      if (PosArm[3] > 120) PosArm[3] = 119;      PosArm[1] -= yD;      if (PosArm[1] < 10) PosArm[1] = 11;      if (PosArm[1] > 101) PosArm[1] = 100;      MoveArm();      // call method      break;    case (2):   //close grip      if ( PosArm[0] > 88) PosArm[0] = PosArm[0] - 1;      if ( PosArm[0] < 88 ) PosArm[0] = PosArm[0] + 1;      if ( PosArm[0] == 88) mode = 3;      MoveArm();      delay(10);      break;    case (3):   //Lift Arm      if ( PosArm[2] > 25) PosArm[2] = PosArm[2] - 1;      if ( PosArm[2] < 25 ) PosArm[2] = PosArm[2] + 1;      if ( PosArm[2] == 25) mode = 4;      MoveArm();      delay(10);      break;    case (4):  // carry marble to drop zone      PosCount = 9;      PosReached = 0;      while (true) {        PosReached = 0;        for (int i = 0; i < 4 ; i++ ) { //adjust servo positions          // we move the servos in small steps and the delay(20)  makes arm motion smooth and slow          if ( PosArm[i] > ArmPos[PosCount][i]) PosArm[i] = PosArm[i] - 1; // if actual position is greater than requird position reduce position by 1          if ( PosArm[i] < ArmPos[PosCount][i]) PosArm[i] = PosArm[i] + 1; // if actual position is less than required position value increase position valuue by 1          if ( PosArm[i] == ArmPos[PosCount][i]) PosReached++;             // check if servo  has reached required position/angle        }        if (PosReached == 4) {   // if all 4 servos have reached position  then increase array index (PosCount)to next position in array          mode = 5;          break;        }        MoveArm();        delay(10);      }      delay(1500);      break;    case (5): //open grip      if ( PosArm[0] > 120) PosArm[0] = PosArm[0] - 1;      if ( PosArm[0] < 120 ) PosArm[0] = PosArm[0] + 1;      if ( PosArm[0] == 120) mode = 0;                  // grip opened   return to mode 0 search mode      MoveArm();      delay(10);      break;  }}///////////////////////////////////////Armvoid MoveArm() {   // write arm position data to servos  for (int i = 0 ; i < 4 ; i++) {    if (PosArm[i] < 10) PosArm[i] = 5;  // limit servo movement to prevent hitting end stops    if (PosArm[i] > 170) PosArm[i] = 175;  // servo.write limited 5 - 175  }  Arm0Servo.write(PosArm[0]);  Arm1Servo.write(PosArm[1]);  Arm2Servo.write(PosArm[2]);  Arm3Servo.write(PosArm[3]);}///////////////////////////////////////Camera// adapted  from https://conorpeterson.wordpress.com/2010/06/04/optical-mouse-hacking-part-1/void mouseInit(void){  digitalWrite(SCLK, HIGH);  delayMicroseconds(5);  digitalWrite(SCLK, LOW);  delayMicroseconds(1);  digitalWrite(SCLK, HIGH);  delay(1025);  writeRegister(regConfig, maskNoSleep); //Force the mouse to be always on.}
void dumpDiag(void){  unsigned int val;
  val = readRegister(regStatus);  /*    Serial.print("Product ID: ");    Serial.println( (unsigned int)((val & maskPID) >> 5));    Serial.println("Ready.");       */  Serial.flush();}void writeRegister(byte addr, byte data){  byte i;  addr |= 0x80; //Setting MSB high indicates a write operation.  //Write the address  pinMode (SDIO, OUTPUT);  for (i = 8; i != 0; i--)  {    digitalWrite (SCLK, LOW);    digitalWrite (SDIO, addr & (1 << (i - 1) ));    digitalWrite (SCLK, HIGH);  }
  //Write the data  for (i = 8; i != 0; i--)  {    digitalWrite (SCLK, LOW);    digitalWrite (SDIO, data & (1 << (i - 1) ));    digitalWrite (SCLK, HIGH);  }}
byte readRegister(byte addr){  byte i;  byte r = 0;
  //Write the address  pinMode (SDIO, OUTPUT);  for (i = 8; i != 0; i--)  {    digitalWrite (SCLK, LOW);    digitalWrite (SDIO, addr & (1 << (i - 1) ));    digitalWrite (SCLK, HIGH);  }
  pinMode (SDIO, INPUT);  //Switch the dataline from output to input  delayMicroseconds(110);  //Wait (per the datasheet, the chip needs a minimum of 100 µsec to prepare the data)
  //Clock the data back in  for (i = 8; i != 0; i--)  {    digitalWrite (SCLK, LOW);    digitalWrite (SCLK, HIGH);    r |= (digitalRead (SDIO) << (i - 1) );  }
  delayMicroseconds(110);  //Tailing delay guarantees >100 µsec before next transaction
  return r;}
//ADNS2610 dumps a 324-byte array, so this function assumes arr points to a buffer of at least 324 bytes. (A2620: unchanged)void readFrame(byte *arr){  byte *pos;  byte *uBound;  unsigned long timeout;  byte val;  xPix = 0;  yPix = 0;  int   pixCount = 0;  //Ask for a frame dump  writeRegister(regPixelData, 0x2A); // wite anything to pixeldatareg to start at first pixel (A2620)
  val = 0;  pos = arr;  uBound = arr + 325;
  timeout = millis() + 1000;
  //There are three terminating conditions from the following loop:  //1. Receive the start-of-field indicator after reading in some data (Success!)  //2. Pos overflows the upper bound of the array (Bad! Might happen if we miss the start-of-field marker for some reason.)  //3. The loop runs for more than one second (Really bad! We’re not talking to the chip properly.)
  //    loop through 324 pixels looking for pixels above 55 light level  while ( millis() < timeout && pos < uBound)  {    val = readRegister(regPixelData);    //Only bother with the next bit if the pixel data is valid.    if ( !(val & 64) ) {      //Serial.println("Invalid data.");      continue;    }    //If we encounter a start-of-field indicator, and the cursor isn’t at the first pixel,    //then stop. (‘Cause the last pixel was the end of the frame.)    if ( ( val & 128 ) &&  ( pos != arr) ) {      sync = 0;      x = 0;      y = 0;     // Serial.println();     // Serial.println("Start of File");      break;    }    *pos = val & 63;    // binary opperation to obtain pixel light level    if (*pos > 55) {     // if above 55 then it is something of interest   and not background      xPix += x;         // sum of x coordinates      yPix += y;          // sum of y coordinates      pixCount++;          // number of bright pixels     // Serial.print("X");     // Serial.print("  ");     // Serial.print(*pos);    }    //Serial.print("  ");   // Serial.print(*pos);    pos++;    x++;    if (x > 17) {        // data comes in as a 1 dimentional stream 324 bytes long         x = 0;             //  this IF(statment) gives each byte a X ,Y position CCD is 18 x 18  0-17      y++;               //     //   Serial.println(".");    }  }  if (pixCount > 0) {   // average x and y coordinates    xPix = xPix / pixCount;       yPix = yPix / pixCount;  } else {              // or return -1 if nothing seen    xPix = -1;    yPix = -1;  }/*  Serial.println();  Serial.print("X   ");  Serial.println(xPix);  Serial.print("Y   ");  Serial.println(yPix);  delay(500);      //    delay to see frames */  xD = 0;  yD = 0;}/////////////////////////////////////////////////////////////////////////////////////