Hi,
Do you have a link or schematic (or picture) for the motor shield? What pins is it using to drive the motors? What driver chip L293? and what other pins for servos, U/S etc.
Looking at your code it makes little sense to me, but that's because of how AFMotor.h works and I don't know that!!
Here's how my code works:
/*
;*******************************
; Filename: DoubleRound
; Date: 12/08/2013
; File Version: Ex ZumoNanoLCD
; Written by : Mel Saunders
; Function: DR-BuggyDrive
; Last Revision: 18/09/2014
; Target Uno/Nano Double Round chassis, etc.
; *******************************
This program demonstrates the use of a TI DRV8833 dual H bridge (Module).
This can run 2x DC motors control their speed and direction.
Also uses a U/S Detecter, etc.
*/
//#include <FastIO.h>
//#include <I2CIO.h>
//#include <LiquidCrystal_I2C.h>
//#include <Wire.h>
//LiquidCrystal_I2C lcd(0x27,2,1,0,4,5,6,7,3, POSITIVE); // Set the LCD I2C address
// Define constants and varibles
const int LMpos=3,LMneg=5,RMneg=6,RMpos=11; //Motor pins
int distance,cm,FRQ,TD,A,B,MotorTime,switches,action,Lspeed,Rspeed,Tilt;
long int randNumber,motorSpeed,TwirlTime;
float unit,val,bv,factor;
boolean go;
void setup()
{
// lcd.begin(16,2);
// battCheck();
// lcd.clear();
// lcd.setCursor(2,0);
// lcd.print("I am Waiting");
delay(1000);
// lcd.setCursor(3,1);
// lcd.print("Wave me off");
pinMode(LMpos, OUTPUT); // sets the digital pin as output
pinMode(LMneg, OUTPUT); // sets the digital pin as output
pinMode(RMneg, OUTPUT); // sets the digital pin as output
pinMode(RMpos, OUTPUT); // sets the digital pin as output
go = true;
Lspeed=200;
Rspeed=245;
}
// * * * start of Main program * * * *
void loop()
{
if (go == true)
{
distance=scanner(cm);
do
{
distance=scanner(cm);
delay(200);
sound(1800,6);
delay(200);
}
while(distance > 25);
// lcd.clear();
// lcd.setCursor(2,0);
// lcd.print("Here we GO!");
Startup();
}
// If go=false, start here.
distance=scanner(cm);
if(distance <=18)
{
Radar();
}
distance=scanner(cm);
if(distance >10)
{
analogWrite(LMpos,Lspeed); //Left motor forward
digitalWrite(LMneg,LOW);
digitalWrite(RMpos,LOW); //Right motor forward
analogWrite(RMneg,Rspeed);
// lcd.clear();
// lcd.print(" Forward go!");
randomSeed(millis());
MotorTime = random(450,750); // Speed for turning, reverese, etc.
}
go = false;
}// end Main loop
//------------------------------------------------------------------
void Startup()
{
for(int i=80; i <= 220; i++) //Slow start, ramp up speed
{
analogWrite(LMpos,i); //Left motor forward.
digitalWrite(LMneg,LOW);
digitalWrite(RMpos,LOW);
analogWrite(RMneg,i+45); //Right motor forward, +25 to compensate for slower motor!!
delay(30);
}
go = false;
}
//-----------------------------------------------------------------
void Halt()//Both motors STOP
{
digitalWrite(LMpos,LOW);
digitalWrite(LMneg,LOW);
digitalWrite(RMpos,LOW);
digitalWrite(RMneg,LOW);
delay(300);
}
//------------------------------------------------------------------
void Rturn()//Turn right
{
digitalWrite(LMpos,LOW); //Left motor forward
digitalWrite(LMneg,HIGH);
digitalWrite(RMpos,LOW); //Right motor reverse
digitalWrite(RMneg,HIGH);
delay(200);
}
//------------------------------------------------------------------
void Lturn()//Turn left
{
digitalWrite(LMpos,HIGH); //Left motor reverse
digitalWrite(LMneg,LOW);
digitalWrite(RMpos,HIGH); //Right motor forward
digitalWrite(RMneg,LOW);
delay(250);
}
//------------------------------------------------------------------
void Forward()//forward
{
analogWrite(LMpos,Lspeed); //Left motor forward
digitalWrite(LMneg,LOW);
digitalWrite(RMpos,LOW); //Right motor forward
analogWrite(RMneg,Rspeed);
}
//------------------------------------------------------------------
int Back(int TD)//Reverse
{
digitalWrite(LMpos,LOW); //Left motor reverse
digitalWrite(LMneg,HIGH);
digitalWrite(RMpos,HIGH); //Right motor reverse
digitalWrite(RMneg,LOW);
delay(TD);
}
//------------------------------------------------------------------
void BackOff() //back off!
{
battCheck();
// lcd.clear();
// lcd.print(" Back off ");
int speed=190; //Reversing, so larger numbers = slower speed??
{
for(int i=0; i <= 5; i++)
{
sound(1500,20);
delay(100);
}
analogWrite(LMpos,speed); //Left motor reverse
digitalWrite(LMneg,HIGH);
digitalWrite(RMpos,HIGH); //Right motor reverse
analogWrite(RMneg,speed);
delay(3000);
}
}
//------------------------------------------------------------------
void sound(int FRQ,int TD)
{
tone(9,FRQ,TD);
}
//------------------------------------------------------------------
long scanner(long cm)
{
const int pingPin=7, EchoPin=8;
long duration;
// The SRF005 is triggered by a HIGH pulse of 2 or more microseconds.
// Give a short LOW pulse before to ensure a clean HIGH pulse:
pinMode(pingPin, OUTPUT);
pinMode(EchoPin, INPUT);
digitalWrite(pingPin, LOW);
delayMicroseconds(2);
digitalWrite(pingPin, HIGH);
delayMicroseconds(2);
digitalWrite(pingPin, LOW);
duration = pulseIn(EchoPin, HIGH);
delay(100);
// convert the time into a distance
// inches = microsecondsToInches(duration);
cm = microsecondsToCentimeters(duration);
return (cm);
}
//------------------------------------------------------------------
long microsecondsToCentimeters(long microseconds)
{
// The speed of sound is 340 m/s or 29 microseconds per centimeter.
// The ping travels out and back, so to find the distance of the
// object we take half of the distance travelled.
return microseconds / 29 / 2;
}
//------------------------------------------------------------------
void Radar()
{
Halt();
BackOff();
Lturn();
sound(2200,20);
Halt();
A=scanner(cm);
delay(100);
Rturn();
Rturn();
sound(2200,20);
Halt();
B=scanner(cm);
delay(100);
Lturn();
delay(50);
if (A>B)
{
Lturn(); //Then turn left
A=0;
}
else
{
Rturn(); //Then turn right
B = 0;
}
}
//------------------------------------------------------------------
void Twirl()
{
action=MotorTime & 0000000000000001;
if(action==1)
// Left twirl
{
digitalWrite(LMpos,LOW); //Left motor reverse
digitalWrite(LMneg,HIGH);
digitalWrite(RMpos,LOW); //Right motor forward
digitalWrite(RMneg,HIGH);
delay(TwirlTime);
}
else
// Right twirl
{
digitalWrite(LMpos,HIGH); //Left motor forward
digitalWrite(LMneg,LOW);
digitalWrite(RMpos,HIGH); //Right motor reverse
digitalWrite(RMneg,LOW);
delay(TwirlTime);
}
}
void battCheck()
{
unit=0.00488; //4.88mV per analoge step
val=analogRead(A2); //Analogue value
bv=(unit*val)*2;
// lcd.clear();
// lcd.setCursor(1,0);
// lcd.print("Battery Status");
val=bv;
if(val>4.80)
{
// lcd.setCursor(1,1);
// lcd.print(val);
// lcd.print(" V+ -Full");
delay(2000);
factor=1;
}
if(val<=4.80)
{
// lcd.setCursor(1,1);
// lcd.print(val);
// lcd.print(" V -Good");
delay(1000);
factor=1.2;
}
if (val <4.50 and val>4.0)
{
// lcd.setCursor(1,1);
// lcd.print(val);
// lcd.print(" V -OK ?");
delay(1000);
factor=1.4;
}
if (val <4.0 and val>3.70)
{
// lcd.setCursor(1,1);
// lcd.print(val);
// lcd.print(" V -Low");
delay(1000);
factor=1.6;
}
if (val <3.70)
{
// lcd.setCursor(0,1);
// lcd.print(val);
// lcd.print(" V -Recharge!");
delay(3000);
}
}
You really need to give us some more info, identify the shield, which U/S sensor SR05.HCS04, what motors, power arangements, wiring, etc.
Regards
Mel.