Можно и попроще
Вот скетч который я в шапке темы демонстрировал
Ты смотри на цикл Loop все остальное тебе не нужно
PHP код:
#include <SoftwareSerial.h>
#include "protocol.h"
#include <Servo.h>
#define rxPin 3 //// VRbot UART Init ETX
#define txPin 2 //// VRbot UART Init ERX
SoftwareSerial mySerial = SoftwareSerial(rxPin, txPin);
Servo myservo;
signed char cmd;
void setup()
{
VRbot_setup();
Serial.begin(115200);
myservo.attach(12,700,2300);
pinMode(10, OUTPUT);
}
//******************************************************
//******************************************************
void loop()
{
delay(20);
digitalWrite(10,LOW);
VRbot_RecognizeSD(1);
cmd = VRbot_CheckResult();
switch (cmd)
{
case -2: // Error
digitalWrite(10,HIGH);
break;
case -1: // Timout
digitalWrite(10,HIGH);
break;
case 0:
myservo.writeMicroseconds(700);
break;
case 1:
myservo.writeMicroseconds(700+160);
break;
case 2:
myservo.writeMicroseconds(700+160*2);
break;
case 3:
myservo.writeMicroseconds(700+160*3);
break;
case 4:
myservo.writeMicroseconds(700+160*4);
break;
case 5:
myservo.writeMicroseconds(700+160*5);
break;
case 6:
myservo.writeMicroseconds(700+160*6);
break;
case 7:
myservo.writeMicroseconds(700+160*7);
break;
case 8:
myservo.writeMicroseconds(700+160*8);
break;
case 9:
myservo.writeMicroseconds(700+160*9);
break;
default: // Other command
digitalWrite(10,HIGH);
break;
}
}
//******************************************************
//******************************************************
void VRbot_setup()
{
pinMode(rxPin, INPUT);
pinMode(txPin, OUTPUT);
mySerial.begin(9600);
delay(1000);
if (VRbot_Detect())
{
VRbot_SetTimeout(5);
VRbot_SetLanguage(0);
}
}
//******************************************************
unsigned char VRbot_read(void)
{
uint8_t val = 0;
val=mySerial.read();
Serial.println(val-64,HEX);
return val-64;
}
//******************************************************
void VRbot_write(uint8_t b)
{
mySerial.print(b, BYTE);
}
//******************************************************
unsigned char VRbot_Detect(void) {
unsigned char i;
for (i = 0; i < 5; ++i) {
delay(100);
VRbot_write(CMD_BREAK);
if ( VRbot_read() == STS_SUCCESS)
return 255;
}
return 0;
}
//******************************************************
unsigned char VRbot_SetLanguage(unsigned char lang)
{
VRbot_write(CMD_LANGUAGE);
delay(5);
VRbot_write(ARG_ZERO + lang);
if (VRbot_read() == STS_SUCCESS)
return 255;
return 0;
}
//******************************************************
void VRbot_RecognizeSD(unsigned char group)
{
VRbot_write(CMD_RECOG_SD);
delay(5);
VRbot_write(ARG_ZERO + group);
}
//******************************************************
void VRbot_RecognizeSI(unsigned char ws) {
VRbot_write(CMD_RECOG_SI);
delay(5);
VRbot_write(ARG_ZERO + ws);
}
//******************************************************
void VRbot_SetTimeout(unsigned char s) {
VRbot_write(CMD_TIMEOUT);
delay(5);
VRbot_write(ARG_ZERO + s);
delay(5);
}
//******************************************************
signed char VRbot_CheckResult(void) {
unsigned char rx;
rx = VRbot_read();
if (rx == STS_SIMILAR || rx == STS_RESULT)
{
delay(5);
VRbot_write(ARG_ACK);
return (VRbot_read() - ARG_ZERO);
}
if (rx == STS_TIMEOUT) return -1;
return -2;
}