Programa para seguir linha

Bem,

Ainda não aplicando os conceitos de Sistemas de Tempo Real fizemos uma rotina de interpretação e correção com intuito de seguir a linha. A idéia era testar a mecânica do robô interagindo com o ambiente a qual este será utilizado.

O programa é em linguagem estruturada utilizando pooling, ou seja, linha por linha, testando os sensores luz e corrigindo a rota.

Segue programa:

#include "NXCDefs.h"
#define THRESHOLD 35 // ver range
byte velA, velB;
void corrigeRota(byte  dir){
 if (dir == 1){
   OnRev(OUT_A, 45);
   OnFwd(OUT_B, 75);
   Wait(100);
   TextOut(5,LCD_LINE3,"motor A rev ");
 }else{
   OnRev(OUT_B, 45);
   OnFwd(OUT_A, 75);
   Wait(100);
   TextOut(5,LCD_LINE3,"motor B rev");
  }
}
void segueRota(){
 OnFwd(OUT_A, velA);
 OnFwd(OUT_B, velB);
}
task main(){
TextOut(5,LCD_LINE1,"START");
//Definindo sensores
SetSensor(IN_1,SENSOR_TOUCH);
SetSensorLight(IN_3);
SetSensorLight(IN_2);
while (true)
 {
 if ((Sensor(IN_3) < THRESHOLD) && (Sensor(IN_2) > THRESHOLD)){
  corrigeRota(1);
  TextOut(5,LCD_LINE2,"rota 1");
  }else{
 if ((Sensor(IN_2) < THRESHOLD) && (Sensor(IN_3) > THRESHOLD)){
  corrigeRota(0);
  TextOut(5,LCD_LINE2,"rota 0");
  }
  }
 if ((Sensor(IN_3) >= THRESHOLD) && (Sensor(IN_2) >= THRESHOLD)){
   TextOut(5,LCD_LINE2,"Sensores fora linha preta");
  velA = 90;
  velB = 90;
  TextOut(5,LCD_LINE3,"vel 100");
  }
 if ((Sensor(IN_3) < THRESHOLD) && (Sensor(IN_2) < THRESHOLD)){
   TextOut(5,LCD_LINE2,"Sensores na linha preta");
  velA = 90;
  velB = 90;
  TextOut(5,LCD_LINE3,"vel 100");
  }
 segueRota();
 }
}