Help with code if possible

Projects Discussion and Showcase Robotics
9

Hi @rich_d_t,

it is obviously a nice task for a state machine ... See here:

/*
  Forum: https://forum.arduino.cc/t/help-with-code-if-possible/1201603
  Wokwi: https://wokwi.com/projects/384469984687635457

  At least for Wokwi we have to use

  #include "ServoEasing.hpp"



*/

#include "ServoEasing.hpp"
ServoEasing servotop;
ServoEasing servobottom;
const int action_pin = 2;
int location = 35;
// Below numbers should be adjusted in case the servos does not close/open as desired
int bottom_closed = 107;
int top_closed = 167;
int bottom_open = 20;
int top_open = 20;

/*
  WAITFOROPEN   Wait for button press
  OPENBOTTOM:   If button press then open bottom
  OPENTOP       Delay then open top
  WAITFORCLOSE  Wait for button press
  CLOSETOP      Close top
  CLOSEBOTTOM   Delay then close bottom
                Goto WAITFOROPEN
*/


enum stateTypes {WAITFOROPEN, OPENBOTTOM, OPENTOP, WAITFORCLOSE, CLOSEBOTH, CLOSETOP, CLOSEBOTTOM};
stateTypes state = WAITFOROPEN;


void setup()
{
  pinMode(action_pin, INPUT_PULLUP);
  servotop.attach(7);//missle
  servobottom.attach(8);//up and down
  setSpeedForAllServos(190);
  servotop.setEasingType(EASE_CUBIC_IN_OUT);
  servobottom.setEasingType(EASE_CUBIC_IN_OUT);
  synchronizeAllServosStartAndWaitForAllServosToStop();
}
void loop()
{
  stateMachine();
}

void stateMachine() {
  static unsigned long startTime = 0;
  switch (state) {
    case WAITFOROPEN:
      if (buttonPressed()) {
        state = OPENBOTTOM;
      }
      break;
    case OPENBOTTOM:
      servobottom.setEaseTo(bottom_open);
      synchronizeAllServosStartAndWaitForAllServosToStop();
      startTime = millis();
      state = OPENTOP;
      break;
    case OPENTOP:
      if (millis() - startTime > 300) {
        servotop.setEaseTo(top_open);
        synchronizeAllServosStartAndWaitForAllServosToStop();
        state = WAITFORCLOSE;
      }
      break;
    case WAITFORCLOSE:
      if (buttonPressed()) {
        state = CLOSETOP;
      }
      break;
    case CLOSETOP:
      servotop.setEaseTo(top_closed);
      synchronizeAllServosStartAndWaitForAllServosToStop();
      startTime = millis();
      state = CLOSEBOTTOM;
      break;
    case CLOSEBOTTOM:
      if (millis() - startTime > 300) {
        servobottom.setEaseTo(bottom_closed);
        synchronizeAllServosStartAndWaitForAllServosToStop();
        state = WAITFOROPEN;
      }
      break;
    default:
      state = WAITFOROPEN;
      break;
  }
}

boolean buttonPressed() {
  static unsigned long lastChange = 0;
  static byte buttonState = HIGH;
  static byte lastState   = HIGH;
  byte actState = digitalRead(action_pin);
  if (actState != lastState) {
    lastState = actState;
    lastChange = millis();
  }
  if (buttonState != lastState && millis() - lastChange > 20) {
    buttonState = lastState;
    return !buttonState;
  }
  return false;
}

On Wokwi: https://wokwi.com/projects/384469984687635457

It might not exactly do what you want, but the logic behind is

  WAITFOROPEN   Wait for button press
  OPENBOTTOM:   If button press then open bottom
  OPENTOP       Delay then open top
  WAITFORCLOSE  Wait for button press
  CLOSETOP      Close top
  CLOSEBOTTOM   Delay then close bottom
                Goto WAITFOROPEN
2 Likes