this is my code
double ___eroare;
double ___senzori;
double ___SA0;
double ___SA1;
double ___SA2;
double ___SA3;
double ___SA4;
int _output_PD5 = 5;
int _output_PD4 = 4;
int _input_SA1 = A1;
int _output_PD7 = 7;
int _input_SA0 = A0;
int _output_PD6 = 6;
int _input_SA3 = A3;
int _input_SA2 = A2;
int _output_PWM10 = 10;
int _output_PWM9 = 9;
int _input_SA4 = A4;
int _led_L = 13;
void citesteSenzoriAnalogi() {
___SA0 = analogRead(_input_SA0);
___SA1 = analogRead(_input_SA1);
___SA2 = analogRead(_input_SA2);
___SA3 = analogRead(_input_SA3);
___SA4 = analogRead(_input_SA4);
}
void calculEroare() {
if ( ___SA2 < 200 ) {
___eroare = 0;
} else {
if ( ( ___SA0 < 200 ) || ( ___SA1 < 200 ) ) {
___eroare = 1;
} else {
if ( ( ___SA3 < 200 ) || ( ___SA4 < 200 ) ) {
___eroare = -1;
}
}
}
}
void controlMotoare() {
digitalWrite(_output_PD4, 1);
digitalWrite(_output_PD5, 0);
digitalWrite(_output_PD6, 0);
digitalWrite(_output_PD7, 1);
if ( ___eroare == 0 ) {
analogWrite(_output_PWM9, 80);
analogWrite(_output_PWM10, 80);
} else {
if ( ___eroare == -1 ) {
analogWrite(_output_PWM9, 80);
analogWrite(_output_PWM10, 0);
} else {
analogWrite(_output_PWM9, 0);
analogWrite(_output_PWM10, 80);
}
}
}
void setup()
{
Serial.begin(9600);
pinMode(_output_PD5, OUTPUT);
pinMode(_output_PD4, OUTPUT);
pinMode(_input_SA1, INPUT);
pinMode(_output_PD7, OUTPUT);
pinMode(_input_SA0, INPUT);
pinMode(_output_PD6, OUTPUT);
pinMode(_input_SA3, INPUT);
pinMode(_input_SA2, INPUT);
pinMode(_output_PWM10, OUTPUT);
pinMode(_output_PWM9, OUTPUT);
pinMode(_input_SA4, INPUT);
pinMode(_led_L, OUTPUT);
___eroare = 0;
___senzori = 5;
___SA0 = 0;
___SA1 = 0;
___SA2 = 0;
___SA3 = 0;
___SA4 = 0;
}
void loop()
{
citesteSenzoriAnalogi();
calculEroare();
controlMotoare();
Serial.print(___SA0);
Serial.print(" ");
Serial.print(___SA1);
Serial.print(" ");
Serial.print(___SA2);
Serial.print(" ");
Serial.print(___SA3);
Serial.print(" ");
Serial.print(___SA4);
Serial.println();
delay(10);
}