Go Down

Topic: FSM stepper/servo problem (Read 392 times) previous topic - next topic

ifugaopapercraft

This sketch has two problems; stepper1 runs independently of the sensor, turning on, but not off. None of the servos are firing, though they were fine before adding the stepper code. Is there a conflict here with Millis?

Code: [Select]
#include <Servo.h>
#include <Stepper.h>

//stepper
#define STEPS1 200
#define STEPS2 200

#define motorCycle1 200U
#define motorCycle2 5000U

#define STOP 0


//stepper

unsigned long motorLastMillis1 = 0;
unsigned long motorLastMillis2 = 0;

boolean motorState1 = false;
boolean motorState2 = false;

Stepper stepper1(STEPS1, 2,3,4,5);//bipolar
Stepper stepper2(STEPS2,53,51,49,47);//unipolar

boolean cycleCheck(unsigned long *lastMillis, unsigned int cycle)

{
  unsigned long currentMillis = millis();
  if(currentMillis - *lastMillis >= cycle)
  {
    *lastMillis = currentMillis;
    return true;
  }
  else
    return false;
}

//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()
{

  stepper1.setSpeed(30);
  stepper2.setSpeed(30);

  pinMode(motorLeft, OUTPUT);  //dc motors
  pinMode(motorMiddle, OUTPUT);
  pinMode(motorRight, OUTPUT);

  myservo1.attach(9);
  myservo2.attach(10);
  myservo3.attach(11);

  Serial.begin(9600);
  pinMode(sonarPin,INPUT);
}

void loop()
{
  static int state1 =1;
  sonarVal = analogRead (sonarPin);

  switch (state1)

  {

    //ALL STOP
  case 1:   
    if (sonarVal > 15)
    {
      stepper1.step(STOP);  ////not responding to analog input
      stepper2.step(STOP);

      digitalWrite (motorLeft,LOW);
      digitalWrite (motorMiddle,LOW);
      digitalWrite (motorRight,LOW);

      myservo1.write (0);
      myservo2.write (0);
      myservo3.write (0);

    }
    else
    {
      state1 = (4,3,2);   

    }

    //STEPPERS
  case 2:
    if (sonarVal < 15)
    {
      if(cycleCheck(&motorLastMillis1, motorCycle1))
        // {
        stepper1.step(300);
      stepper1.step(-300);

    }

    if(cycleCheck(&motorLastMillis2, motorCycle2))  ////2
    {
      stepper2.step(200);   
    }
    else
    {
      state1 = 1;         
    }


    //MOTORS

  case 3:

    if (sonarVal < 15)
    {
      digitalWrite (motorLeft, HIGH);
      digitalWrite (motorMiddle,HIGH);
      digitalWrite (motorRight, HIGH);
    }
    else
    {
      state1 = 1;
      {       

        //SERVO  ---not responding
      case 4:
        if (sonarVal < 15)
        {
          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 = 1;
        }
        break;
      }
      break;
    }
    break;
  }


}











PaulS

Code: [Select]
      state1 = (4,3,2);   
What is this supposed to be doing?

Where are your Serial.print() statements?

ifugaopapercraft

If the sensor returns more than 15, the FSM is supposed to run the motors, servos, and steppers. Case 4,3,and two.  I had the serial monitor checking sensor values but removed those lines. Is there another parameter to check?

PaulS

You should perhaps check on what the comma operator does. It does NOT do what you hope it does.

ifugaopapercraft

I found use of the comma in several texts and web sites, but these have to do with parsing a string.

Go Up