#include <Servo.h>
#define Rgrey 512
#define Lgrey 512
Servo servo;
//-------------------------------------
void _exit() {
_stop();
delay(1000);
exit(0);
}
void _move(int l, int r) {
digitalWrite(4, l < 0);
digitalWrite(7, r < 0);
analogWrite(5, abs(constrain(l, -255, 255)));
analogWrite(6, abs(constrain(r, -255, 255)));
}
void go(int l, int r, uint32_t t = 1000) {
_move(l, r);
delay(t);
}
void _stop() {
uint32_t t = millis();
int sp = 100;
while (millis() - t < 100) {
_move(-sp, -sp);
delay(5);
_move(sp, sp);
delay(5);
}
_move(0, 0);
}
void turn_led(bool f) {
digitalWrite(2, f);
}
bool read_Bsen() {
return !digitalRead(3);
}
int read_Lsen() {
return analogRead(A0);
}
int read_Rsen() {
return analogRead(A1);
}
float read_HCsen() {
digitalWrite(8, HIGH); // генерируем 10 мкс импульс запуска
delayMicroseconds(10);
digitalWrite(8, LOW);
// определение на пине echoPin длительности уровня HIGH, мкс:
long duration = pulseIn(9, HIGH);
float distacne_cm = duration * 0.017 ;
return distacne_cm;
}
float read_IRsen() {
int summ = 0;
float value = 0;
// Получение 5 значений
for (int i = 0; i < 5; i++) {
value = analogRead(A2);
summ = summ + value;
delay(55); // Ожидание 55 ms перед каждым чтением
}
value = summ / 5; // усреднить значения
float volts = value * 0.0048828125;
// и в расстояние в см
float distance = 32 * pow(volts, -1.10);
return distance;
}
void turn_servo(bool f) {
if (f) {
servo.write(90);
delay(100);
}
else {
servo.write(180);
delay(100);
}
}
void setup() {
pinMode(4, OUTPUT);
pinMode(7, OUTPUT);
pinMode(5, OUTPUT);
pinMode(6, OUTPUT);
pinMode(A1, INPUT);
pinMode(A0, INPUT);
pinMode(A2, INPUT);
pinMode(8, OUTPUT);
pinMode(9, INPUT);
// pinMode(3, INPUT_PULLUP);
// pinMode(2, OUTPUT);
servo.attach(2);
}
// ------------------------------------------
void test_mot() {
go(255, 255);
go(-255, -255);
go(0, 255);
go(255, 0);
_stop();
}
void test_sen() {
Serial.begin(9600);
while (1) {
Serial.print("lsen "); Serial.print(read_Lsen()); Serial.print("Rsen "); Serial.println(read_Rsen());
Serial.print("IRsen "); Serial.println(read_IRsen());
Serial.print("HCsen "); Serial.println(read_HCsen());
};
}
void test_ser() {
turn_servo(1);
delay(500);
turn_servo(0);
delay(500);
}
void test() {
test_mot();
test_ser();
test_sen();
}
byte move_line(int sp = 100) {
if (read_Lsen() < Lgrey and read_Rsen() < Rgrey) { //black
_move(sp, sp);
return 00;
} else if (read_Lsen() > Lgrey and read_Rsen() > Rgrey) { //white
_move(sp, sp);
return 11;
} else if (read_Lsen()<Lgrey and read_Rsen()>Rgrey) {
_move(0, sp);
return 01;
} else if (read_Lsen() > Lgrey and read_Rsen() < Rgrey) {
_move(sp, 0);
return 10;
}
}
void go_line(uint64_t t) {
int t2 = millis();
while (millis() - t2 < t) {
byte dir = move_line();
}
}
// ------------------------------------------
void loop() {
test();
_exit();
}