// these include is difference in all Operating System.
#include <SCC_Driver.h>
#include <ballCoordinate.h>
#include<CameraArmDriver.h>

//2;r1;n,g1,n;
CameraArmDriver cameraArm;


void setup() {
  Serial1.begin(115200);
  Serial.begin(115200);
  toFetal();

  toReady(void);
}

point[0] = COOR_X_1;

point[1] = COOR_Y_1;

point[2] = COOR_Z_1;

void loop() {
          facePoint(point[0],point[1],point[2]);
toPoint(point[0],point[1],point[2]);
          openClaw();
          closeClaw();
          facePoint(COOR_X_1,COOR_Y_1,COOR_Z_1);
          toCane(COOR_X_DROP,COOR_Y_DROP,COOR_Z_DROP);
          openClaw();
          closeClaw();

  while(1);

//  getDataStream();
//  if(parseData()){
//    printSimplePoint();
//    for(int i =0; i<4; i++){
//      if(color[i]>0){
//        switch(i){
//          case 0:
//          //
//          toCane(COOR_X_1,COOR_Y_1,COOR_Z_1);
//          openClaw();
//          closeClaw();
//          toCane(COOR_X_DROP,COOR_Y_DROP,COOR_Z_DROP);
//          openClaw();
//          closeClaw();
//          break;
//
//          case 1:
//          //facePoint(COOR_X_2,COOR_Y_2,COOR_Z_2);
//          toCane(COOR_X_2,COOR_Y_2,COOR_Z_2);
//          openClaw();
//          closeClaw();
//          toCane(COOR_X_DROP,COOR_Y_DROP,COOR_Z_DROP);
//          break;
//
//          case 2:
//          //facePoint(COOR_X_3,COOR_Y_3,COOR_Z_3);
//          toCane(COOR_X_3,COOR_Y_3,COOR_Z_3);
//          openClaw();
//          closeClaw();
//          toCane(COOR_X_DROP,COOR_Y_DROP,COOR_Z_DROP);
//          break;
//
//          case 3:
//          //facePoint(COOR_X_4,COOR_Y_4,COOR_Z_4);
//          toCane(COOR_X_4,COOR_Y_4,COOR_Z_4);
//          openClaw();
//          closeClaw();
//          toCane(COOR_X_DROP,COOR_Y_DROP,COOR_Z_DROP);
//          break;
//        }
//      }
//    }// end for
//
//    while(1);
//  }
}