Pages: [1]   Go Down
Author Topic: FSM stepper/servo problem  (Read 264 times)
0 Members and 1 Guest are viewing this topic.
Offline Offline
Full Member
***
Karma: 0
Posts: 146
View Profile
 Bigger Bigger  Smaller Smaller  Reset Reset

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


}










Logged

Seattle, WA USA
Offline Offline
Brattain Member
*****
Karma: 601
Posts: 48543
Seattle, WA USA
View Profile
 Bigger Bigger  Smaller Smaller  Reset Reset

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

Where are your Serial.print() statements?
Logged

Offline Offline
Full Member
***
Karma: 0
Posts: 146
View Profile
 Bigger Bigger  Smaller Smaller  Reset Reset

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?
Logged

Seattle, WA USA
Offline Offline
Brattain Member
*****
Karma: 601
Posts: 48543
Seattle, WA USA
View Profile
 Bigger Bigger  Smaller Smaller  Reset Reset

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

Offline Offline
Full Member
***
Karma: 0
Posts: 146
View Profile
 Bigger Bigger  Smaller Smaller  Reset Reset

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

Pages: [1]   Go Up
Jump to: