Line follower update 03

I've gotten the program pretty much worked out.  I have only a few details to be worked out, but for now at least, all building has been put on the back burner as I am now focusing entirely on my schooling about seven hours a day seven days a week. speaking of which, I am supposed to be reading right now... well any way, I am going to post the whole program for ya'all now, so be ware!!!




#define ant(a) until(Button == a) Button = ButtonPressed(3, true)
#define sound(b); PlayTone(b, 50); Wait(100);
#define Move(c,d); { int T; OnFwdEx(OUT_A, d, false); if(c > 0) until( T > c ){ T = MotorTachoCount(OUT_A);} else until( T < c ){ T = MotorTachoCount(OUT_A);} }

bool alth, Go = true, Button = false, line = true;

bool where[] = {false, false, false, false, false};
//int turndeg[] = {-300, -150, 0, 150, 300};
int width = 0, line_position = 0, LLP = 0, thresh = 25;
int m = 0;

safecall int MAP(int input, int input_ref, int output_ref)
     { return (input * output_ref) / input_ref; }

bool AlterThresh(int T, int S, int L, bool AT){
   if(T > S && T < L && AT == true){ thresh = thresh + 5; AT = true; }
   else{ AT = false; } return AT; }

safecall void MOTOR_CONTROL(bool FB, int speed){
     if(FB == true){ int turnpct = MAP(line_position, 100, 200) - 100;
     RotateMotorEx(OUT_BC, speed, 90, turnpct, true, false);}
     else RotateMotorEx(OUT_BC, -speed, 90, 0, false, false);}

task DISPLAY(){ while(Go == true){
     SetSensorLight(IN_3);
     TextOut(0, 57, "W", true); NumOut(10, 57, width, false);
     TextOut(50, 57, "T", false); NumOut(60, 57, MAP(MotorTachoCount(OUT_A), 180, 100), false);
     TextOut(0, 43, "P", false); NumOut(10, 43, line_position, false);
     //TextOut(50, 43, "%", false); NumOut(60, 43, turnpot, false);
     TextOut(0, 0, "L", false); NumOut(10, 0, m, false);
     TextOut(25, 0, "LS", false); NumOut(38, 0, Sensor(IN_3), false);
     TextOut(55, 0, "TH", false); NumOut(68, 0, thresh, false);
     for(int c = 0; c < 5; c++){
          if (where[c] == true) TextOut(c*10, 28, "O", false);
          else TextOut(c*10, 28, "X", false);
     }
     Wait(100);
}}

task SCAN(){
     SetSensorLight(IN_3);
     int T = 0, pace = 50;
     alth = true;
     for(m = 0; Go == true;)
     {
         OnFwdEx(OUT_A, pace, false);
         until( T > 180 || Sensor(IN_3) < thresh ){ T = MotorTachoCount(OUT_A);}
         until( T > 180 || Sensor(IN_3) > thresh ){ T = MotorTachoCount(OUT_A);}
         alth = AlterThresh(T, 160, 200, alth);
         Move(pace, MotorTachoCount(OUT_A) + 10);
         OffEx(OUT_A, false);
         Wait(100);
    
         OnFwdEx(OUT_A, -pace, false);
         until( T < 1 || Sensor(IN_3) < thresh ){T = MotorTachoCount(OUT_A);}
         until( T < 1 || Sensor(IN_3) < thresh ){T = MotorTachoCount(OUT_A);}
         alth = AlterThresh(T, -200, 20, alth);
         OffEx(OUT_A, false);
         Wait(100);
         if(alth == false && m == 0){thresh = thresh + 5; m = 1;}
     }
}

task DATA_LOG(){
     SetSensorLight(IN_3);
     int end, sta;
     while(Go == true){
          bool FR = true;
          int c = 0, now = CurrentTick();
          until(Sensor(IN_3) < thresh){
               if(CurrentTick() - now > 2000 && alth == false){ FR = false; MOTOR_CONTROL(false, 25); }}
          if(FR == true) MOTOR_CONTROL(true, 50);
          sta = MAP(MotorTachoCount(OUT_A), 180, 100);
          sound(800);
          until(Sensor(IN_3) > thresh);
          end = MAP(MotorTachoCount(OUT_A), 180, 100);
          sound(1000);
          line_position = (end + sta) / 2;
          if(sta > end) width = sta - end;
          else width = end - sta;
          for(;c<5;c++) where[c] = false;
          // 0-25, 25-35, 35-65, 65-75, 75-100
          if (line_position > 0 && line_position < 26) where[0] = true;
          if (line_position > 25 && line_position < 50 - (width/2)) where[1] = true;
          if (line_position > 50 - (width/2) && line_position < 50 + (width/2)) where[2] = true;
          if (line_position > 50 + (width/2) && line_position < 74) where[3] = true;
          if (line_position > 75 && line_position < 100) where[4] = true;
     }
}

task main(){
     RotateMotor(OUT_A, 35, -90);
     ResetTachoCount(OUT_A);
     Button = ButtonPressed(3, true);
     while(true)
     {
          start DISPLAY;
          start SCAN; sound(1500);
          for(int c=0;c<5;c++) where[c] = false;
          start DATA_LOG; sound(1500);
          Button = ButtonPressed(3, true);
          ant(true); sound(500);
          ant(false); sound(1000); Go = true;
     }
}

Wow!  If anyone has suggestions or comments, feel free to type, I am always looking for advice!  Also, be sure to check out my other blog the Legoton Press.  You can get there by clicking on "view complete profile" over on the right.

P.S. I recognize that by posting my code to the public, I have given you the permission to use and modify to any extent the said code.  But, this code may not be sold without permission from me, and I would appreciate it if you would give me credit if you do use this code.  Thank you!.