#pragma config(Sensor, S1,     cam,                 sensorI2CCustomFastSkipStates)//setup sensor
//SETUP
#include "mindsensors-nxtcam.h" //include commands from mindsensors
#include "nxtcamlib.c" //include commands from nxtcamlib

float x; //setup x as a variable
float y; //setup y as a variable
int _blob1_center;
short xmotor = motorA; //let xmotor be used in place of motorA
short ymotor = motorB; //let ymotor be used in place of motorB
const tSensors cam  = (tSensors) S1; //not quite sure
short t = 100; //time alloted for motor to move (in thousandths of a second)
short d = 15; //"color's" distance from middle of head (in cm)
short s = 50; //motor speed (as a % of fastest possible speed)
float xdegrees; //setup xdegrees as a variable
float ydegrees; //setup ydegrees as a variable
void moveymotor()//move ymotor when conditions exist
         {
         if (nMotorEncoder[ymotor] < nMotorEncoderTarget[ymotor])
            {
            ymotor = s;
            wait1Msec(t);
            }
         if (nMotorEncoder[ymotor] > nMotorEncoderTarget[ymotor])
            {
            ymotor = -s;
            wait1Msec(t);
            }
         }
void movexmotor() //move xmotor to designated encoder target when conditions exist
         {
         if (nMotorEncoder[xmotor] < nMotorEncoderTarget[xmotor])
            {
            xmotor = s;
            wait1Msec(t);
            }
         if (nMotorEncoder[xmotor] > nMotorEncoderTarget[xmotor])
            {
            xmotor = -s;
            wait1Msec(t);
            }
         }
void encodertargets()//set the encoder targets of ymotor and xmotor to the same degree as my head is looking
         {
         nMotorEncoderTarget[xmotor] = atan(_blob1_center/d);
         nMotorEncoderTarget[ymotor] = atan(_blob1_center/d);
         }
/////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
//////////////////////////////////////////////////////////////!!!!Actual Code!!!!////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
/////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////


         
task main()
{

      while (true)
      {
         eraseDisplay(); //clears the display
         blob_array _blobs; //not sure
         bool _condensed = true; //condense all close blobs into one?
         NXTCAMinit(cam); //turns the camera on
         NXTCAMgetBlobs(cam, _blobs, _condensed); //
      
         nMotorEncoder[xmotor] = 0; //sets the current
         nMotorEncoder[ymotor] = 0;
         x = (_blobs[0].x2 + _blobs[0].x1)/2;
         y = (_blobs[0].y2 + _blobs[0].y1)/2;
         xdegrees =   x - 50; //xdegrees = the middle of the block on the x axis - the middle of the camera's vision
         ydegrees = y - 32; //ydegrees = the middle of the block on the y axis = the middle of the camera's vision
         encodertargets(); //set encoder targets
         movexmotor(); //move motors to encoder targets
         //short movexmotor (short nMotorEncoderTarget = xdegrees)
      }
}      

NXTCAMgetCenter(blob_array & blobs, 0, x, y);

*Warning*:Unreferenced function 'moveymotor'

#pragma config(Sensor, S1,     cam,                 sensorI2CCustomFastSkipStates)//setup sensor
//SETUP
#include "drivers/mindsensors-nxtcam.h" //include commands from mindsensors


float x; //setup x as a variable
float y; //setup y as a variable
int _blob1_center;
int xmotor = motorA; //let xmotor be used in place of motorA
int ymotor = motorB; //let ymotor be used in place of motorB


// XS: you don't need this line, it's in the pragma already
// const tSensors cam  = (tSensors) S1; //not quite sure

// XS: Either use const int here, or simply define them
// XS: Also, try not to use single letter variables, why not use
// XS: motorMoveTime, motorDistance and motorSpeed?
// XS: this will make it easier to share your core later and
// XS: and have someone else understand your code quickly.
const int t = 100; //time alloted for motor to move (in thousandths of a second)
const int d = 15; //"color's" distance from middle of head (in cm)
const int s = 50; //motor speed (as a % of fastest possible speed)

float xdegrees; //setup xdegrees as a variable
float ydegrees; //setup ydegrees as a variable

void moveymotor()//move ymotor when conditions exist
{
  if (nMotorEncoder[ymotor] < nMotorEncoderTarget[ymotor])
  {
    ymotor = s;
    wait1Msec(t);
  }
  if (nMotorEncoder[ymotor] > nMotorEncoderTarget[ymotor])
  {
    ymotor = -s;
    wait1Msec(t);
  }
}
void movexmotor() //move xmotor to designated encoder target when conditions exist
{
  if (nMotorEncoder[xmotor] < nMotorEncoderTarget[xmotor])
  {
    xmotor = s;
    wait1Msec(t);
  }
  if (nMotorEncoder[xmotor] > nMotorEncoderTarget[xmotor])
  {
    xmotor = -s;
    wait1Msec(t);
  }
}
void encodertargets()//set the encoder targets of ymotor and xmotor to the same degree as my head is looking
{
  nMotorEncoderTarget[xmotor] = atan(_blob1_center/d);
  nMotorEncoderTarget[ymotor] = atan(_blob1_center/d);
}
/////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
//////////////////////////////////////////////////////////////!!!!Actual Code!!!!////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
/////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////



task main()
{

  while (true)
  {
    eraseDisplay(); //clears the display
    blob_array _blobs; //not sure
    bool _condensed = true; //condense all close blobs into one?
    NXTCAMinit(cam); //turns the camera on
    NXTCAMgetBlobs(cam, _blobs, _condensed); //

    nMotorEncoder[xmotor] = 0; //sets the current
    nMotorEncoder[ymotor] = 0;
    x = (_blobs[0].x2 + _blobs[0].x1)/2;
    y = (_blobs[0].y2 + _blobs[0].y1)/2;
    xdegrees =   x - 50; //xdegrees = the middle of the block on the x axis - the middle of the camera's vision
    ydegrees = y - 32; //ydegrees = the middle of the block on the y axis = the middle of the camera's vision
    encodertargets(); //set encoder targets
    movexmotor(); //move motors to encoder targets
    //int movexmotor (int nMotorEncoderTarget = xdegrees)
  }
}

#pragma config(Sensor, S1,     cam,                 sensorI2CCustomFastSkipStates)         //setup sensor

#include "mindsensors-nxtcam.h"         //include commands from mindsensors

float x;         //setup x as a variable
float y;         //setup y as a variable
int xmotor = motorB;         //let xmotor be used in place of motorA
int ymotor = motorA;         //let ymotor be used in place of motorB
int xtarget;         //define xtarget as variable
int ytarget;         //define ytarget as variable
int xdegrees;         //define xdegrees as variable
int ydegrees;         //define ydegrees as variable
const int motortime = 100;         //time alloted for motor to move (in thousandths of a second)
const int blobdistance = 15;      //"blob's" distance from middle of head (in cm)
const int motorspeed = 10;         //motor speed (as a % of fastest possible speed)
float xradians;         //setup xradians as a variable
float yradians;         //setup yradians as a variable

void moveymotor()         //move ymotor when conditions exist
      {
      if (ytarget > 0)
           {
           nMotorEncoderTarget[ymotor] = ytarget;
          motor[ymotor] = motorspeed;
          wait1Msec(motortime);
          //while (nMotorEncoder[ymotor] < ytarget) {}
           }
     if (ytarget < 0)
           {
           nMotorEncoderTarget[ymotor] = ytarget;
          motor[ymotor] = -motorspeed;
          wait1Msec(motortime);
          //while (nMotorEncoder[ymotor] > ytarget) {}
           }
      }
void movexmotor()         //move xmotor to designated encoder target when conditions exist
      {
     if (xtarget > 0)
           {
           nMotorEncoderTarget[xmotor] = xtarget;
          motor[xmotor] = -motorspeed;
          wait1Msec(motortime);
          //while (nMotorEncoder[xmotor] < xtarget) {}
           }
     if (xtarget < 0)
           {
           nMotorEncoderTarget[xmotor] = xtarget;
          xmotor = motorspeed;
          wait1Msec(motortime);
          //while (nMotorEncoder[xmotor] > xtarget) {}
           }
      }
int encodertargetx()         //set the encoder targets of ymotor and xmotor to the same degree as my head is looking
      {
      xradians = atan(xdegrees/blobdistance);
      xtarget = radiansToDegrees(xradians);
      return (xtarget);
      }
   
int encodertargety()         //set the encoder targets of ymotor and xmotor to the same degree as my head is looking
      {
      yradians = atan(xdegrees/blobdistance);
      ytarget = radiansToDegrees(yradians);
      return (ytarget);
      }
      
/////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
//////////////////////////////////////////////////////////////!!!!Actual Code!!!!////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
/////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////

task main()
{

  while (true)
  {
    blob_array _blobs;            //sets up an array?
    bool _condensed = true;      //condense all close blobs into one
    NXTCAMinit(cam);               //turns the camera on
    NXTCAMgetBlobs(cam, _blobs, _condensed);         //gets the blobs

    nMotorEncoder[xmotor] = 0;         //sets the current value of xmotor to 0 and sets that value =x
    nMotorEncoder[ymotor] = 0;         //sets the current value of ymotor to 0 and sets that value =y
   
    x = (_blobs[0].x2 + _blobs[0].x1)/2;         //finds the middle point of the blob on the x axis
    y = (_blobs[0].y2 + _blobs[0].y1)/2;         //finds the middle point of the blob on the y axis
   
    xdegrees =   x - 77;         //xdegrees = the middle of the block on the x axis - the middle of the camera's vision
    ydegrees = y - 44;         //ydegrees = the middle of the block on the y axis - the middle of the camera's vision
   
    encodertargetx();         //set x encoder target
    encodertargety();         //set y encoder target
    movexmotor();            //move xmotor to the encoder target
    moveymotor();                 //move ymotor to the encoder target
  }
}

#pragma config(Sensor, S1,     NXTCAM,         sensorI2CCustomFastSkipStates)
#pragma config(Motor,  motorA,          YMOTOR,        tmotorNXT, PIDControl, encoder)
#pragma config(Motor,  motorB,          XMOTOR,        tmotorNXT, PIDControl, encoder)
//*!!Code automatically generated by 'ROBOTC' configuration wizard               !!*//

// XS: this automagically opens the debugStream window.
#pragma debuggerWindows(debugStream)

#include "drivers/mindsensors-nxtcam.h"         //include commands from mindsensors

const float blobdistance = 15.0;   //"blob's" distance from middle of head (in cm)
const int motorspeed = 10;         //motor speed (as a % of fastest possible speed)

//move the motor when conditions exist
// XS: if you find the motor moves in the wrong direction, use the motor setup
// XS: to configure it as "reversed"
void moveMotor(tMotor mot, int target)
{
  writeDebugStreamLine("moveMotor called: motor: %d, target: %d", mot, target);
  if (target > 0)
  {
    nMotorEncoderTarget[mot] = target;
    motor[mot] = motorspeed;
  }
  else if (target < 0)
  {
    nMotorEncoderTarget[mot] = target;
    motor[mot] = -motorspeed;
  }
  else
  {
    motor[mot] = 0;
  }

  // Wait for the motor to be done running
  while (nMotorRunState[mot] != runStateIdle || nMotorRunState[mot] != runStateHoldPosition)
  {
    wait1Msec(5);
  }
}

//set the encoder targets of YMOTOR and XMOTOR to the same degree as my head is looking
int encodertarget(int degrees)
{
  int retval = 0;
  writeDebugStreamLine("encodertarget called: %d", degrees);
  float radians = atan((float)degrees / blobdistance);
  retval = radiansToDegrees(radians);
  writeDebugStreamLine("encodertarget returns: %d", retval);
  return retval;
}

/////////////////////////////////////////////////////////////////////////////////////
///////////////////////////////!!!!Actual Code!!!!///////////////////////////////////
/////////////////////////////////////////////////////////////////////////////////////

task main()
{
  int xtarget;
  int ytarget;
  int xdegrees;
  int ydegrees;
  int x;
  int y;

  // XS: You only need to do this once.
  // XS: encoder targets are always relative to whatever they were before.
  nMotorEncoder[XMOTOR] = 0;         //sets the current value of XMOTOR to 0 and sets that value =x
  nMotorEncoder[YMOTOR] = 0;         //sets the current value of YMOTOR to 0 and sets that value =y

  NXTCAMinit(NXTCAM);            //turns the camera on
  blob_array _blobs;             //sets up an array?
  bool _condensed = true;        //condense all close blobs into one

  while (true)
  {
    NXTCAMgetBlobs(NXTCAM, _blobs, _condensed);         //gets the blobs

    x = (_blobs[0].x2 + _blobs[0].x1)/2;         //finds the middle point of the blob on the x axis
    y = (_blobs[0].y2 + _blobs[0].y1)/2;         //finds the middle point of the blob on the y axis

    writeDebugStreamLine("NXTCAM returned: x: %d, y: %d", x, y);

    xdegrees =   x - 77;         //xdegrees = the middle of the block on the x axis - the middle of the camera's vision
    ydegrees = y - 44;         //ydegrees = the middle of the block on the y axis - the middle of the camera's vision

    xtarget = encodertarget(xdegrees);         //set x encoder target
    ytarget = encodertarget(ydegrees);         //set y encoder target
    moveMotor(XMOTOR, xtarget);                //move XMOTOR to the encoder target
    moveMotor(YMOTOR, ytarget);                //move YMOTOR to the encoder target
  }
}