BW-Robot – Тестируем

#include <SoftwareSerial.h>

#define Turn_on_off 2
#define LED 5

SoftwareSerial r_motor(4,3); // Передние моторы - ВСЕ РАБОТАЕТ
SoftwareSerial f_motor(13,12); // Задние моторы - ВСЕ РАБОТАЕТ

boolean prog_stat = false;

int pd2 = 0;

void setup()
{

pinMode(LED,OUTPUT);

f_motor.begin(115200);
r_motor.begin(115200);

stop_all();

digitalWrite(LED,HIGH); // Test
delay(300);
digitalWrite(LED,LOW);

}

/////////////////////////////////// Main Loop ///////////////////////

void loop()
{

pd2 = digitalRead(Turn_on_off);

if (pd2 == 1) {
delay(10);
while(digitalRead(Turn_on_off) == HIGH) delay(10);

prog_stat =! prog_stat;

if (prog_stat == true) {
digitalWrite(LED,HIGH);
forward(9,3000);
revers(9,3000);
stop_all();
left(9,4000); // 360
right(9,4000); // 360
stop_all();
digitalWrite(LED,LOW);
}
else {
digitalWrite(LED,LOW);
stop_all();
}
}

}

/////////////////////// Stop All //////////////////////

void stop_all( void ) {

f_motor.print("1F0\r");
delay(10);
f_motor.print("2F0\r");
delay(10);

r_motor.print("1F0\r");
delay(10);
r_motor.print("2F0\r");
delay(10);

}

//////////////////// Forward /////////////////////////

void forward( int speed, int wait ) {

f_motor.print("2F6\r");
delay(10);
f_motor.print("1F6\r");
delay(10);
r_motor.print("2F6\r");
delay(10);
r_motor.print("1F6\r");
delay(10);

delay(wait);

}

//////////////////// Revers //////////////////////


void revers( int speed, int wait ) {

f_motor.print("2R6\r");
delay(10);
f_motor.print("1R6\r");
delay(10);
r_motor.print("2R6\r");
delay(10);
r_motor.print("1R6\r");
delay(10);

delay(wait);

}

//////////////////// Left //////////////////////


void left( int speed, int wait ) {

f_motor.print("2F6\r");
delay(10);
f_motor.print("1R6\r");
delay(10);
r_motor.print("2R6\r");
delay(10);
r_motor.print("1F6\r");
delay(10);

delay(wait);

}

//////////////////// Right //////////////////////

void right( int speed, int wait ) {

f_motor.print("2R6\r");
delay(10);
f_motor.print("1F6\r");
delay(10);

r_motor.print("2F6\r");
delay(10);
r_motor.print("1R6\r");
delay(10);

delay(wait);

}