/* variaveis de utilizadas*/
const int nsensores = 5;
const int sensor[] = {2,3,4,5,6};
const int motor_right = 10;
const int motor_left = 11;
/* variaveis auxiliares*/
int senval[] = {0,0,0,0,0};
int vel_right = 0;
int vel_left = 0;
int maxvel = 200;
double errorval = 0.0;
boolean stop = false;
void setup(){
Serial.begin(9600);
pinMode (motor_right, OUTPUT);
pinMode (motor_left, OUTPUT);
pinMode (2, INPUT);
pinMode (3, INPUT);
pinMode (4, INPUT);
pinMode (5, INPUT);
pinMode (6, INPUT);
digitalWrite (2, HIGH);
digitalWrite (3, HIGH);
digitalWrite (4, HIGH);
digitalWrite (5, HIGH);
digitalWrite (6, HIGH);
}
void loop(){
errorval = LineError();
if(errorval < 0.0){
vel_left = (int)(maxvel*(1-fabs(errorval)));
vel_right = (int)(maxvel);
}
else if(errorval > 0.0){
vel_left = (int)(maxvel);
vel_right = (int)(maxvel*(1-errorval));
}
else{
vel_left = (int)(maxvel);
vel_right = (int)(maxvel);
}
//Fazer actuar os motores:
analogWrite(motor_right,vel_right);
analogWrite(motor_left, vel_left);
delay(200);
Serial.println(errorval);
}
// escolher o sensor que esta sobre a linha
double LineError()
{
double error = 0.0;
int valorsensor = 0;
for (int i=0; i < nsensores; i++){
senval[i] = digitalRead (sensor[i]);
}
valorsensor = (senval[0]) | (senval[1]<<1) | (senval[2]<<2) | (senval[3]<<3) | (senval[4]<<4);
Serial.println ("valor do sensor");
Serial.println(valorsensor);
delay(500);
if (valorsensor== 0b10000){
valorsensor = 16;
error = -1.0;
}
else if (valorsensor == 0b11000){
valorsensor = 24;
error = -0.75;
}
else if (valorsensor == 0b01000){
valorsensor = 8;
error = -0.5;
}
else if (valorsensor == 0b01100){
valorsensor = 12;
error = -0.25;
}
else if((valorsensor==0b00100) || (valorsensor=0b01110)){
valorsensor = 4 ||14;
error = 0.0;
}
else if (valorsensor == 0b00110){
valorsensor = 6;
error = 0.25;
}
else if (valorsensor == 0b00010){
valorsensor = 2;
error = 0.5;
}
else if (valorsensor == 0b00011){
valorsensor = 3;
error = 0.75;
}
else if (valorsensor == 0b00001){
valorsensor = 1;
error = 1.0;
}
return error;
}
devia funcionar, e o error não me varia de 0.0..