This sketch has two problems; while fast to start, it is very slow to escape the loop after the sensor value is beyond the threshold. Secondly, the stepper runs one revolution, then stops rather than continuous until the threshold is met.
#include <Servo.h>
#include <Stepper.h>
#define STEPS1 200
#define STEPS2 200
#define STOP2 0
int leftSwitchPin = 2;
int rightSwitchPin = 3;
int buttonState1 = 0;
int buttonState2 = 0;
//const int stepsPerRevolution1 = 44;
Stepper myStepper1(STEPS1, 44,42,40,38);
int stepCount = 0;
//const int stepsPerRevolution2 = 44;
Stepper myStepper2(STEPS2, 53,51,49,47);
//LED's
int ledLeft = 35;
int ledMiddle = 36;
int ledRight = 37;
int ledFrontRow [10] = {
20,21,22,23,24,25,26,27,28,29};
//Motors
int motorLeft = 29; //dc motors
int motorMiddle = 31;
int motorRight = 33;
//Servo
Servo myservo1;
Servo myservo2;
Servo myservo3;
unsigned int duration = 0;
int lasttime1 = 0;
int lasttime2 = 0;
int randstart = 0;
int randend = 0;
int pos1 = 0;
int pos2 = 0;
int pos3 = 0;
int sonarPin = A1;
int sonarVal =0;
void setup()
{
pinMode (leftSwitchPin,INPUT); //shift to pin 2
pinMode (rightSwitchPin,INPUT); //shift to pin 3
myStepper1.setSpeed(60);
myStepper2.setSpeed(60);
pinMode(motorLeft, OUTPUT); //dc motors
pinMode(motorMiddle, OUTPUT);
pinMode(motorRight, OUTPUT);
//pinMode(sonarVal,INPUT);
myservo1.attach(9);
myservo2.attach(10);
myservo3.attach(11);
Serial.begin(9600);
pinMode(sonarPin,INPUT);
}
void loop()
{
static int state1 = 1;
static int state2 = 1;
sonarVal = analogRead (sonarPin);
Serial.println (sonarVal);
switch (state1)
{
case 1:
if (sonarVal < 20)
{
myStepper2.step(STEPS2); // [color=red]Turns one revolution ONLY[/color]
digitalWrite (motorLeft, HIGH);
digitalWrite (motorMiddle,HIGH);
digitalWrite (motorRight, HIGH);
lasttime1 = millis();
randstart = random(554, 1054);
randend = random(500, 2400);
while(pos1 <= randend)
{
pos1 += 1;
myservo1.writeMicroseconds(pos1);
delay(2); //2
}
while(pos2 <= randend)
{
pos2 += 1;
myservo2.writeMicroseconds(pos2);
delay(2); //2
}
while(pos3 <= randend)
{
pos3 += 1;
myservo3.writeMicroseconds(pos3);
delay(2); //2
}
duration = millis() - lasttime1;
lasttime2 = millis();
while(pos1 >= randstart)
{
pos1-=1;
myservo1.write(pos1);
delay(2);
}
while(pos2 >= randstart)
{
pos2-=1;
myservo2.write(pos2);
delay(2);
}
while(pos3 >= randstart)
{
pos3-=1;
myservo3.write(pos3);
delay(2);
}
duration = millis() - lasttime2;
duration = millis() - lasttime1;
}
else
{
state1=2;
{
}
// break;
case 2:
if (sonarVal > 20)
{
myStepper2.step(STOP2 ); // [color=red]Should turn the stepper off[/color].
digitalWrite (motorLeft, LOW);
digitalWrite (motorMiddle,LOW);
digitalWrite (motorRight,LOW);
myservo1.write(0);
myservo2.write(0);
myservo3.write(0);
}
else
{
state1 = 1;
{
break;
break;
}
break;
}
break;
}
}
}