Keywish Hummer-Bot-1.0, Инструкция по эксплуатации

Страница: 99 / 113

96
#include "protocol.h"
#include "hummerbot.h"
#include "process.h"
int E1 = 5 ; //PWMA
int M1 = 9 ; //DIRA --- left
int E2 = 6 ; //PWMB
int M2 = 10 ; //DIRB --- right
byte readbuff [ 32 ] = {};
int readlen = 0 ;
ST_protocol recv ;
hummerbot hbot ( E1 , M1 , E2 , M2 , 13 , A0 );
void setup () {
Serial . begin ( 9600 );
hbot . init ();
}
void read_data ( void ) {
unsigned char avilable ;
byte * p = readbuff ;
memset ( p , 0 , 32 );
readlen = 0 ;
while ( Serial . available () > 0 ) {
if ( ! avilable && Serial . peek () == START_CODE ) {
avilable = 1 ;
}
if ( avilable ) {
if (( * p = Serial . read ()) == END_CODE ) {
avilable = 0 ;
readlen ++;
//Serial.print(*p,HEX);
break ;
}
//Serial.print(*p,HEX);
p ++;
readlen ++;
}
}
// Serial.print("\n");
}
void loop () {
read_data ();