#include <Bounce.h>
byte LeftMotorPin[] = {3, 6, 9}; // номер выхода разъема для левого мотора. ШИМ/ реле 1/ реле 2
byte RightMotorPin[] = {5, 7,8};// номер выхода разъема для правого мотора. ШИМ/ реле 1/ реле 2
byte OptoBarrierPin = 12; // номер входа разъёма для опто датчика
byte OptoBarrierPinLed = 13; //номер выхода разъёма индикации срабатывания опто барьера
Bounce bouncer = Bounce(OptoBarrierPin, 40);
boolean motorsEnabled = true; //разрешения действий над моторами
unsigned int ControlTime = 0;
unsigned long CurrentTime;
unsigned long LastTime = 0;
//byte CommandType = 0;
//byte CommandState = 0;
//int ParamValue1, ParamValue2;
char versionMessage[] ="Rockman Robo V1.0 2010\r\n";
String message;
char inData;
int pos;
void setup()
{
// start serial port at 9600 bps:
Serial.begin(9600);
for(int i =0; i < 3; i++)
{
pinMode(LeftMotorPin[i], OUTPUT);
pinMode(RightMotorPin[i], OUTPUT);
}
pinMode(OptoBarrierPin, INPUT);
pinMode(OptoBarrierPinLed, OUTPUT);
Serial.print(versionMessage);
}
void loop()
{
if (bouncer.update()) { //если произошло событие
if (bouncer.read()== HIGH) { //если сработал опто датчик
Stop();
motorsEnabled = false; //вводим моторы в неактивное состояние
Serial.print("Opto Barrier HIGH\r\n"); //вывод сообщения о препятствии
digitalWrite(OptoBarrierPinLed, HIGH);
bouncer.rebounce(500); //повторить событие через 500мс
}
else
{
motorsEnabled = true;//активируем моторы
digitalWrite(OptoBarrierPinLed, LOW);
Serial.print("Opto Barrier LOW\r\n"); //вывод сообщения об отсутсвии препятствия
}
}
CurrentTime = millis();
ControlTime += CurrentTime - LastTime;
LastTime = CurrentTime;
if(ControlTime >= 100)
{
ControlTime = 0;
Stop();
}
if (Serial.available() > 0) {
// get incoming byte:
inData = Serial.read();
if(inData != '\n')
{
message += inData;
if(message.length() > 255)
{
Serial.print("Out of size Command (>255): " + message + "\r\n");
message = "";
}
}
else
{
if(message.indexOf("Forward",0) == 0) //команда на движение вперед
Forward();
else if(message.indexOf("Backward",0) == 0)//команда на движение назад
Backward();
else if(message.indexOf("Stop",0) == 0) //команда останова
Stop();
else if(message.indexOf("Right",0) == 0)//команда на движение вправо
Right();
else if( message.indexOf("Left",0) == 0)//команда на движение влево
Left();
else if(message.indexOf("Rotate+",0) == 0)//команда на разворот по часовой стрелке
RotatePos();
else if(message.indexOf("Rotate-",0) == 0)//команда на разворот против часовой стрелки
RotateNeg();
else if( message.indexOf ("Get Version", 0) == 0)//команда на запрос версии
{
Serial.print(versionMessage);
}
else Serial.print("Unknown Command: " + message + "\r\n"); //неизвестная или ошибочный синтаксис команды
message = "";
}
}
}
//-------------------------------------------
int GetParamValue1(String mes)
{
pos = mes.indexOf(',');
if(pos > 0)
{
/*ParamValue1 = int(mes[pos - 1]);
String s ;
for(int i = 0; i < mes.length(); i ++)
{
s += String(int(mes[i])) + " ";
}
Serial.print("Message: " + s);
Serial.print("ParamValue1: " + (String)ParamValue1+"\r\n");
return ParamValue1;*/
return int(mes[pos - 1]);
}
else
{
Serial.print("Eror: ParamValue1 not found\r\n");
return -1;
}
}
int GetParamValue2(String mes)
{
pos = mes.indexOf(',');
if(pos > 0)
{
if(mes.length() > pos + 1) return int(mes[pos + 1]);
}
Serial.print("Eror: ParamValue2 not found\r\n");
return -1;
//Serial.print("Entered Command: " + message + "\r\n");
}
void SetMotorValue(byte *MotorPinArr, int inverted, int power)
{
ControlTime = 0;
if(inverted >= 0 && power >= 0)
{
//Выключаем оба реле мотора
digitalWrite(MotorPinArr[1], LOW);
digitalWrite(MotorPinArr[2], LOW);
if (motorsEnabled == true)
{
if(power > 0)
{
if(inverted == 1)digitalWrite(MotorPinArr[1], HIGH);
else digitalWrite(MotorPinArr[2], HIGH);
}
}
else power = 0; //убираем мощность двигателя на 0 если он в неактивном состоянии
analogWrite(MotorPinArr[0], power);
}
else Serial.print("Error SetMotorValue: arguments < 0!\r\n");
}
//-----------------------------------------------------------------
void Stop()
{
SetMotorValue(RightMotorPin, 0, 0);
SetMotorValue(LeftMotorPin, 0, 0);
}
void Right()
{
SetMotorValue(LeftMotorPin, 0, 0);
SetMotorValue(RightMotorPin, GetParamValue1(message), GetParamValue2(message));
}
void Left()
{
SetMotorValue(RightMotorPin, 0, 0);
SetMotorValue(LeftMotorPin, GetParamValue1(message), GetParamValue2(message));
}
void Forward()
{
SetMotorValue(LeftMotorPin, 0, GetParamValue1(message));
SetMotorValue(RightMotorPin, 0, GetParamValue2(message));
}
void RotatePos()
{
SetMotorValue(RightMotorPin, 0, GetParamValue1(message));
SetMotorValue(LeftMotorPin, 1, GetParamValue2(message));
}
void RotateNeg()
{
SetMotorValue(RightMotorPin, 1, GetParamValue1(message));
SetMotorValue(LeftMotorPin, 0, GetParamValue2(message));
}
void Backward()
{
SetMotorValue(LeftMotorPin, 1, GetParamValue1(message));
SetMotorValue(RightMotorPin, 1, GetParamValue2(message));
}
