hey guys im building a robot that we are going to race with mostley left habd turns. i am having a few issues with the code and would like to get your opinions. the main thing is it dont respond quick enough and runs into walls and well flat out i havent gotten it to turn yet heres the code. i have a ping sensor as my distance finder and im sweeping it on a servo
#include <Servo.h>
Servo myservo; // create servo object to control a servo
const int pingPin = 8; //ping sensor pin number
int pos = 90; // variable to set the initial position of the servo
const int ping = 8; //ping pin number
int reset = 2;// motordriver reset pin number
void setup()
{
Serial.begin(9600); //sets serial communication bps
pinMode (reset, OUTPUT);
digitalWrite(reset,LOW);
delay(50);
digitalWrite(reset,HIGH);
delay(50);
myservo.attach(10); // attaches the servo on pin 10 to the servo object
myservo.write(90); //center servo
delay(500); //delay 1000ms
}
void loop()
{
long duration, inches, inches2, inches3; // establish variables for duration of ping, inches = 90; inches2 = 20; inches3 = 140
myservo.write(90); //move servo to pos 0
delay(5);
pinMode(ping, OUTPUT);
digitalWrite(ping, LOW);
delayMicroseconds(2);
digitalWrite(ping, HIGH);
delayMicroseconds(5);
digitalWrite(ping, LOW);
pinMode(ping, INPUT);
duration = pulseIn(ping, HIGH);
inches = microsecondsToInches(duration);
delay(500);
myservo.write(20); //move servo to pos 20
delay(5);
pinMode(ping, OUTPUT);
digitalWrite(ping, LOW);
delayMicroseconds(2);
digitalWrite(ping, HIGH);
delayMicroseconds(5);
digitalWrite(ping, LOW);
pinMode(ping, INPUT);
duration = pulseIn(ping, HIGH);
inches2 = microsecondsToInches(duration);
delay(500);
myservo.write(90); //move servo to pos 0
delay(5);
pinMode(pingPin, OUTPUT);
digitalWrite(pingPin, LOW);
delayMicroseconds(2);
digitalWrite(pingPin, HIGH);
delayMicroseconds(5);
digitalWrite(pingPin, LOW);
pinMode(pingPin, INPUT);
duration = pulseIn(pingPin, HIGH);
inches = microsecondsToInches(duration);
delay(500);
myservo.write(140); //move servo to pos 0
delay(5);
pinMode(pingPin, OUTPUT);
digitalWrite(pingPin, LOW);
delayMicroseconds(2);
digitalWrite(pingPin, HIGH);
delayMicroseconds(5);
digitalWrite(pingPin, LOW);
pinMode(pingPin, INPUT);
duration = pulseIn(pingPin, HIGH);
inches = microsecondsToInches(duration);
delay(500);
myservo.write(90); //move servo to pos 0
delay(5);
pinMode(pingPin, OUTPUT);
digitalWrite(pingPin, LOW);
delayMicroseconds(2);
digitalWrite(pingPin, HIGH);
delayMicroseconds(5);
digitalWrite(pingPin, LOW);
pinMode(pingPin, INPUT);
duration = pulseIn(pingPin, HIGH);
inches3 = microsecondsToInches(duration);
delay(500);
if(inches > 50 ){
//left motor
unsigned char buff1[6];
buff1[0]=0x80; //start byte
buff1[1]=0x00; //device type
buff1[2]=0x01; //motor number and directiobyte; motor one =00,01
buff1[3]=0x7F; //motor speed 0 to 128 in hex (ex 100 is 64 hex)
for (int i=0; i<4; i++) {Serial.print(buff1[i],BYTE);}
//right motor
unsigned char buff2[6];
buff2[0]=0x80;
buff2[1]=0x00;
buff2[2]=0x03; //motor and direction byte; motor two=02.03
buff2[3]=0x7E;//7E is 126 in hex; 7D is 125 in hex set to smaller value to compensate for uneven wheel speed???
for(int i=0; i<4; i++) {Serial.print(buff2[i],BYTE);}
}
if (inches2 < 12 ){
//left motor
unsigned char buff3[6];
buff3[0]=0x80;
buff3[1]=0x00;
buff3[2]=0x00;
buff3[3]=0x40;
for(int i=0; i<4; i++) {Serial.print(buff3[i], BYTE);}
//right motor
unsigned char buff2[6];
buff2[0]=0x80;
buff2[1]=0x00;
buff2[2]=0x02; //motor and direction byte; motor two=02.03
buff2[3]=0x7D;
for(int i=0; i<4; i++) {Serial.print(buff2[i],BYTE);}
}
if ( inches < 50 && inches3 < 12 ){
//left motor
unsigned char buff3[6];
buff3[0]=0x80;
buff3[1]=0x00;
buff3[2]=0x00;
buff3[3]=0x40;
for(int i=0; i<4; i++) {Serial.print(buff3[i], BYTE);}
//right motor
unsigned char buff2[6];
buff2[0]=0x80;
buff2[1]=0x00;
buff2[2]=0x02; //motor and direction byte; motor two=02.03
buff2[3]=0x7D;
for(int i=0; i<4; i++) {Serial.print(buff2[i],BYTE);}
}
if (inches3 < 12){
//left motor
unsigned char buff3[6];
buff3[0]=0x80;
buff3[1]=0x00;
buff3[2]=0x00;
buff3[3]=0x7F;
for(int i=0; i<4; i++) {Serial.print(buff3[i], BYTE);}
//right motor
unsigned char buff2[6];
buff2[0]=0x80;
buff2[1]=0x00;
buff2[2]=0x02; //motor and direction byte; motor two=02.03
buff2[3]=0x7D;
for(int i=0; i<4; i++) {Serial.print(buff2[i],BYTE);}
}
}
long microsecondsToInches(long microseconds)
{
return microseconds / 74 / 2;
}