Pages: [1]   Go Down
Author Topic: FSM stepper/servo problem  (Read 293 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
Online Online
Brattain Member
*****
Karma: 610
Posts: 49037
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
Online Online
Brattain Member
*****
Karma: 610
Posts: 49037
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: