#include "NXCDefs.h"

void SAMPLE_USRF(int near)
{
 //this function samples the ultrasonic sensor 3 times in an attempt to detect false positives:
 //seeing an obstacle when there really isn't one
 //the motor is on during this function and doesn't shut off until this function ends
 
until (SensorUS(IN_3) < near);                 //until detects obstacle
int POSITIVE;                                  //establish counter
do{
   POSITIVE = 0;                                       //reset counter
   if (SensorUS(IN_3) < near) POSITIVE = POSITIVE +1;   // sample USRF 1st time
   Wait (2);                                           // wait 2 milliseconds
   if (SensorUS(IN_3) < near) POSITIVE = POSITIVE +1;   // sample USRF 2nd time
   Wait(2);                                            // wait 2 milliseconds
   if (SensorUS(IN_3) < near) POSITIVE = POSITIVE +1;   // sample USRF 3rd time
 }
 while (POSITIVE != 3);  // stay in do loop until USRF returns three consecutive positive readings
}      // end function SAMPLE_USRF


task main()
{
//written by Jason Atwood
//using Bricx Command Center v3.3
//   3/27/2007 -> 7/22/2008

//setup write capabilities
byte fileHandle;
short bytesWritten;

//initialize variables
int encoder;           //motor encoder
int US_value1;         //ultrasonic range finder
int US_value2;         //ultrasonic range finder
int random_turn;      //random turn rotation
int i;                //counter
int near;             //obstacle proximity

i=1;            //begin counter
near = 30;     //centimeters

//initialize sensors
SetSensorType (IN_3, SENSOR_TYPE_LOWSPEED_9V);       //US sensor in port 3

//setup output file
DeleteFile("move.txt");
CreateFile("move.txt", 512, fileHandle);

//action
while (true)
      {
      OnRevReg(OUT_AC,50,OUT_REGMODE_SYNC);          //set left and right motors in sync
      SAMPLE_USRF(near);                            // call SAMPLE_USRF function
      Off(OUT_AC);                                   //and stop the motor

      encoder = GetOutput(OUT_C, TachoCount);   //get current encoder value
      encoder = encoder * (-1);                   //change encoder sign
      US_value1 = SensorUS(IN_3);                //get current US_value  (first reading)
      
      RotateMotor(OUT_A,50,100);                //turn right 3 degrees
      US_value2 = SensorUS(IN_3);               //get current US_value (second reading)
      
      
      //move random_turn
      random_turn=(Random(1500) + Random(-1500));//generate random number (roughly a full 'about face' cw or ccw)
      RotateMotor(OUT_A,50,random_turn);//rotate right wheel random number of degrees at 50% power
      Off(OUT_A); //turn off motor, may not be necessary.

      //convert integers to strings, concatenate, and write to file
      string i_str = NumToStr(i);   //convert from int to string
      string enc_str = NumToStr(encoder);
      string US_str1 = NumToStr(US_value1);
      string US_str2 = NumToStr(US_value2);
      string random_turn_str = NumToStr(random_turn+100);  //add 100 to include turn made to capture 2nd US reading
      string cat = StrCat(i_str, ",", enc_str, ",", US_str1, ",", US_str2, ",", random_turn_str);
      WriteLnString(fileHandle, cat, bytesWritten);

      i = i+1;                  //increase counter
      ResetTachoCount(OUT_C);   //reset motor encoder
      }
//continue loop until battery dies or human intervention
}    // end task main