RobotSource

float power = 12;
float dec = 0;
float lastrevcount = 0;
float twopi = PI*2;
float dir = 1;
float x_count = 0;
float y_count = 0;
float lasttime = 0;
float lasttime2 = 0;
float result_t = 1;
bool Mapped = false;
bool Active = true;

int mapY[3700/2];
int mapX[3700/2];


float Revolutions()
{
        float current = (float)SensorValue(I2C_1);
        float raw =  (current/650);
        float percent = (raw-lastrevcount);
        //writeDebugStreamLine("%f",percent);
        if(abs(percent)>=.9)
        {
                // erase map data
                dir=-dir;
                Mapped = true;
                lastrevcount=raw;

                //for(int i = 0;i<3700/2;i++)
                //{
                   //     mapX[i]=NULL;
                 //       mapY[i]=NULL;
               //         x_count=0;
             //           y_count=0;
           //     }
        }
        return percent;
}
void RecordSurrounding(float rev,float dist)
{
        float theta = rev*twopi;
        float distance = dist/4;
        float y = distance*cos(theta);
        float x = dir*(-distance*sin(theta));

        if(x_count!=(3700/2-1))
        {
                y_count++;
                x_count++;
                mapX[x_count]=x;
                mapY[y_count]=y;
               // writeDebugStreamLine("(%f,%f)",x,y);
        }
        else
        {
         WriteDebugStreamLine("%s","Passed threshold!");
         Mapped = true;
        }
}

//float rev = 0;
float mapOutput = 0;
task main()
{

        while(Active)
        {
            if(nPgmTime>5000)
        {
                if(!Mapped)
          {
                float dist = SensorValue(Output);
                //writeDebugStreamLine("%f",dist);

                float rev=Revolutions();
                float time = nPGmTime*1000;
                //100000 = .1 sec
                if(time-lasttime>=10000)
                {
                        lasttime=time;
                       RecordSurrounding(rev,dist);
                }


                motor[Neck]=(dist<5?0:(dir*power));
    }
      else
    {
     motor[Neck]=0;
     float time2 = time100[T2];
     if (time2-lasttime2 >= .1)
       //WriteDebugStreamLine("%d",1);
     {
      lasttime2=time2;
      if(mapOutput < 3700/2-1)
      {
      mapOutput++;
      writeDebugStreamLine("%f,%f",mapX[mapOutput],mapY[mapOutput]);
      }
      else
      {
       Active = false;
      }
     }
    }

                // VectorX = RevolutionCount
                // VectorY = Distance

                //start at 0,
                //-1 for left until full revolution
                // +1 for right until full revolution


        }
      }
        motor[Neck]=0;
}