#include <SoftwareSerial.h>
#include <Wire.h>
SoftwareSerial BT(2,3); //HC-05 (TX,RX)
unsigned long VL_1[7];
unsigned long VL_2[7];
unsigned long VR_1[7];
unsigned long VR_2[7];
float vr,vl;
int x,y;
int set=128;
void back(){
analogWrite(5,255);
analogWrite(6,0);
analogWrite(9,0);
analogWrite(10,255);
}
void right(){
analogWrite(5,0);
analogWrite(6,0);
analogWrite(9,255);
analogWrite(10,0);
}
void left(){
analogWrite(5,0);
analogWrite(6,255);
analogWrite(9,0);
analogWrite(10,0);
}
void stand(){
analogWrite(5,0);
analogWrite(6,0);
analogWrite(9,0);
analogWrite(10,0);
}
void scan(){
for(int i=0 ; i<6 ; i++){
VR_1[i]=pulseIn(7,1);
VL_1[i]=pulseIn(8,1);
VR_2[i]=pulseIn(7,0);
VL_2[i]=pulseIn(8,0);
}
vr=0;
vl=0;
for(int i=0 ; i<6 ; i++){
vr=vr+VR_1[i]+VR_2[i];
vl=vl+VL_1[i]+VL_2[i];
}
vr=90000000/vr;
vl=90000000/vl;
Serial.print("vl=");
Serial.print(vl);
Serial.print(" , vr=");
Serial.println(vr);
}
void go(){
analogWrite(5,0);
analogWrite(6,set+y);
analogWrite(9,set+x);
analogWrite(10,0);
}
void setup() {
Serial.begin(9600);
BT.begin(9600);
pinMode(5,OUTPUT);
pinMode(6,OUTPUT);
pinMode(9,OUTPUT);
pinMode(10,OUTPUT);
}
void loop() {
go();
scan();
if(vl>vr){
y=y+2;
x=x+1;
if(y>255){
y=255;
}
if(x>255){
x=255;
}
}
if(vl<vr){
x=x+2;
y=y+1;
if(x>255){
x=255;
}
if(y>255){
y=255;
}
}
}