Текст самой программы находится ниже. В принципе это все, что необходимо для прошивки платы arduino:
/*
* Version 1.2 2013
* http://tutrobot.blogspot.com/
* Робот на базе Arduino
*
*/
#define
MIN_speed 0
//Только выходы с PWM - 3,5,6,9,10,11
int PIN_ENB = 9;
int PIN_ENA
= 6;
int PIN_IN4
= 2;
int PIN_IN3
= 4;
int PIN_IN2
= 5;
int PIN_IN1
= 7;
int LED_PIN
= 13;
float
vel,ks,m1,m2;
float spl,
spr;
int quadr =
0;
int ver,
hor;
unsigned
char mode;
unsigned
char oldmode;
unsigned
char iSpeed;
unsigned
char rx_buf[8];
unsigned
char rxcnt;
boolean
binv = 0;
boolean
rx_ok;
// Правый мотор
void
RM_foward()
{
digitalWrite(PIN_IN1, LOW);
digitalWrite(PIN_IN2, HIGH);
}
void
RM_back()
{
digitalWrite(PIN_IN1, HIGH);
digitalWrite(PIN_IN2, LOW);
}
// Левый мотор
void
LM_foward()
{
digitalWrite(PIN_IN4, LOW);
digitalWrite(PIN_IN3, HIGH);
}
void
LM_back()
{
digitalWrite(PIN_IN4, HIGH);
digitalWrite(PIN_IN3, LOW);
}
void
setup()
{
Serial.begin(9600);
pinMode(PIN_ENB, OUTPUT);
pinMode(PIN_IN4, OUTPUT);
pinMode(PIN_IN3, OUTPUT);
pinMode(PIN_IN2, OUTPUT);
pinMode(PIN_IN1, OUTPUT);
pinMode(PIN_ENA, OUTPUT);
pinMode(LED_PIN, OUTPUT);
digitalWrite(PIN_IN1, LOW);
digitalWrite(PIN_IN2, LOW);
digitalWrite(PIN_IN4, LOW);
digitalWrite(PIN_IN3, LOW);
digitalWrite(PIN_ENA, LOW);
digitalWrite(PIN_ENB, LOW);
}
//Процедура задания скорости
void setspeed()
{
float sf1;
//левый двигатель
if (spl>0.0) LM_foward(); else
LM_back();
sf1=abs(spl);
sf1=sf1*(255-MIN_speed)+MIN_speed;
if (spl==0.0) sf1=0;
iSpeed = int(sf1);
analogWrite(PIN_ENA, iSpeed);
//правый двигатель
if (spr>0.0) RM_foward(); else
RM_back();
sf1=abs(spr);
sf1=sf1*(255-MIN_speed)+MIN_speed;
if (spr==0.0) sf1=0;
iSpeed = int(sf1);
analogWrite(PIN_ENB, iSpeed);
}
void loop()
{
if (Serial.available() == 7)
{
rxcnt=0;
digitalWrite(LED_PIN, binv);
binv=!binv;
while (Serial.available() > 0)
{
rx_buf[rxcnt] = Serial.read();
rxcnt++;
}
//Контроль
целостности пакета
rx_ok=true;
if (rx_buf[4]!=4) rx_ok=false;
//прочистка буфера
if (!rx_ok)
{
int
avi = Serial.available();
Serial.print("err");
while (Serial.available() > 0)
{
avi = Serial.read();
}
}
if (rx_ok)
{
hor = rx_buf[1];
ver = rx_buf[2];
quadr = 0;
//Вычисление
рабочего квадранта
float fhor=float(hor);
float fver=float(ver);
if (fhor>128.0)
{
fhor=256.0-fhor;
quadr = quadr+1;
}
if (fver>128.0)
{
fver=256.0-fver;
quadr = quadr+2;
}
//нормализация
скоростей
fhor=fhor/127.0;
fver=fver/127.0;
if
(fver>fhor)
{
m1 = fver;
m2 =
(1.0-fhor)*fver;
} else
{
m1 = fhor;
m2 = (fver-1.0)*fhor;
}
if
((fver<0.1)&&(fhor<0.1))
{
m1=0;
m2=0;
}
//Приведение
знаков скоростей по квадрантам
if (quadr==0)
{
spl=-m1;
spr=-m2;
}
if (quadr==1)
{
spr=-m1;
spl=-m2;
}
if (quadr==2)
{
spr=m1;
spl=m2;
}
if (quadr==3)
{
spl=m1;
spr=m2;
}
setspeed();
}
}
if (Serial.available() > 7)
{
int avi = Serial.available();
Serial.print("e");
while (Serial.available() > 0)
{
avi =
Serial.read();
}
}
}
