FSM (was dysfunctional function)

Using Arduino Programming Questions
1

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;
    }

  }
}
2

That's strange, and would be an interesting starting point for a discussion about the switch syntax. I'm unprepared for that, but I notice that case 2 in your switch falls within the else part of the previous "if (sonarVal < 20)". This is confusing and makes me unable to understand what (else) can go wrong.

I believe however that you didn't mean to write obfuscated code, so you should probably review it in order to nest ifs and cases appropriately, add one break for each case, and eliminate unused assignments such as the following:

duration = millis() - lasttime2; 
duration = millis() - lasttime1;

I understand this housekeeping may be frustrating, but the logic of the sketch in its present form is a real challenge.