#include <MobaTools.h> // Include MobaTools
#include <ezButton.h>
MoToSoftLed myLed;
MoToTimer myTimer;
MoToServo myServoLeft;
MoToServo myServoRight;
MoToServo myServoJaw;
ezButton legsButton(4);
ezButton jawButton(5);
ezButton ledsButton(7);
//******************************* LED FSM *********************************
// LEDs on Dinosaur's back light up
// 'const' are all values that never change in the sketch
const byte maxBlink = 8; // blinking 3 times
const int waitTime = 250; // wait 250ms
const int blinkTime = 50; // time on for blinks
const int solidOnTime = 4000; // solid state
const int ledPin = 12; // led pin
// our FSM needs the following states:
// Later that should be defined with an enum
const byte IDLE = 1; // waiting for the button press
const byte WAIT1 = 2; // first delay until blinking
const byte BLINK_ON = 3; // Blink ON state
const byte BLINK_OFF = 4; // Blink off state
const byte BLINK_OFF2 = 5; // Off
const byte BLINK_AGAIN = 6; // Series of short blinks
const byte WAIT2 = 7; //
byte blinkState = IDLE; // FSM starts in first state
byte blinkCount; // count the number of blink cycles
byte blinkCount2;
//******************************* MOTOR FSM *******************************
// Legs on Dinosaur move
const int servoLeftLegMinPos = 9; //set initial Left Leg pos as 90
const int servoRightLegMinPos = 9; //set initial Right Leg pos as 90
const int servoLeftLegMaxPos = 130; //set max position of Left Leg as 130 deg
const int servoRightLegMaxPos = 130; //set max position of Right Leg as 130 deg
const int servoLeftLegInitialPos = 110; //set Left Leg initial pos at 110 deg
const int servoRightLegInitialPos = 110; //set Right Leg initial pos at 110 deg
const int legServoSpeed = 400;
const byte IDLE2 = 1; // waiting for the button press, move from resting pos
const byte SWING1 = 2; // leg swings back
const byte SWING2 = 3; // leg swings forward
const byte SWING3 = 4; // leg swings back
const byte SWING4 = 5; // leg swings forward
const byte SWING5 = 6; // leg swings back
const byte BACK_TO_START = 9; // leg swings to initial/resting pos
byte motorState = IDLE2; // FSM starts in first state
//*************************************************************************
//******************************* SETUP ***********************************
void setup() {
// set the digital pin as output:
Serial.begin(115200); // serial monitor for debugging
myLed.attach(ledPin); // led pin attach
legsButton.setDebounceTime(50); // debounce button
jawButton.setDebounceTime(50); // debounce button
ledsButton.setDebounceTime(50); // debounce button
myServoLeft.attach(9); // attach left servo motor
myServoRight.attach(10); // attach right servo motor
myServoJaw.attach(8); // attach jaw motor
}
//******************************* LOOP ************************************
void loop() {
legsButton.loop(); // call button function legs
ledsButton.loop(); // call button function leds
//*************************************************************************
//******************************* MOTOR FSM *******************************
// The objective is to move a servo motor back and forth. It will have a leg attached and
// is supposed to look like the dinosaur is walking. The legs will hanging straight down to start
// and then swing back and forth with servo motor between certain angles (min and max). The angles I'm using are
// just starting points.
switch ( motorState ) {
case IDLE2:
if ( legsButton.isPressed() ) {
Serial.println("Move from resting/initial pos");
myServoLeft.setSpeedTime(legServoSpeed);
myServoLeft.write(servoLeftLegMaxPos);
motorState=SWING1;
}
break; //>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>
case SWING1:
if ( myServoLeft.read() == servoLeftLegMaxPos) {
Serial.println("Swing forward 1");
myServoLeft.setSpeedTime(legServoSpeed);
myServoLeft.write(servoLeftLegMinPos);
motorState=SWING2;
}
break; //>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>
case SWING2:
if ( myServoLeft.read() == servoLeftLegMinPos) {
Serial.println("Swing back 1");
myServoLeft.setSpeedTime(legServoSpeed);
myServoLeft.write(servoLeftLegMaxPos);
motorState=SWING3;
}
break; //>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>
case SWING3:
if ( myServoLeft.read() == servoLeftLegMaxPos) {
Serial.println("Swing forward 2");
myServoLeft.setSpeedTime(legServoSpeed);
myServoLeft.write(servoLeftLegMinPos);
motorState=SWING4;
}
break; //>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>
case SWING4:
if ( myServoLeft.read() == servoLeftLegMinPos) {
Serial.println("Swing back 2");
myServoLeft.setSpeedTime(legServoSpeed);
myServoLeft.write(servoLeftLegMaxPos);
motorState=SWING5;
}
break; //>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>
case SWING5:
if ( myServoLeft.read() == servoLeftLegMaxPos) {
Serial.println("Swing forward 3");
myServoLeft.setSpeedTime(legServoSpeed);
myServoLeft.write(servoLeftLegInitialPos);
motorState=BACK_TO_START;
}
break; //>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>
case BACK_TO_START:
if ( myServoLeft.read() == servoLeftLegInitialPos) {
Serial.println("Return to resting/initial pos. Goto Idle2");
myServoLeft.setSpeedTime(legServoSpeed);
motorState=IDLE2;
}
break; //>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>
}
//*************************************************************************
//******************************* LED FSM *********************************
// within every loop() cycle only the statements of the active
// FSM state ( = switch case ) are executed, all others are ignored
switch ( blinkState ) {
case IDLE: // here we are waiting for the button press
if ( ledsButton.isPressed() ) { // if ( digitalRead(ledsButton) == LOW ) {
// Start button has been pressed
Serial.println("Start button pressed, wait a moment ... ");
myTimer.setTime( waitTime );
blinkState=WAIT1; // now we only wait for the timer to expire
}
break; //>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>
case WAIT1: // waiting the first delaytime
if ( myTimer.expired() ) {
Serial.println("Start blinking now...");
blinkCount= maxBlink; // counting the blinking
blinkCount2 = maxBlink;
myLed.on();
myTimer.setTime(blinkTime); // start time for on time
blinkState=BLINK_ON;
}
break; //>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>
case BLINK_ON: // Led is on, wait until on time is over
if ( myTimer.expired() ) {
// now switch off and wait off time
myLed.off();
myTimer.setTime(blinkTime); // start time for off time
blinkState=BLINK_OFF;
}
break; //>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>
case BLINK_OFF: // Led is off, wait until off time is over
if ( myTimer.expired() ) {
myLed.on();
// now we must decide wether there have been enough blink cycles
blinkCount--; // decrement counter
if ( blinkCount > 0 ) {
// still blinking ... so switch to BLINK_ON again
myTimer.setTime(blinkTime); // start time for on time
blinkState=BLINK_ON;
} else {
// blinking finished, switch to solid on
Serial.println("Blinking finished, now solid on");
myTimer.setTime(solidOnTime);
blinkState = BLINK_OFF2;
}
}
break; //>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>
case BLINK_OFF2: // Led is solid on, wait until everything is finished
if ( myTimer.expired() ) {
Serial.println("Solid on over");
myLed.off();
myTimer.setTime(blinkTime);
blinkState = BLINK_AGAIN;
}
break; //>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>
case BLINK_AGAIN: // Led is off, wait until off time is over
if ( myTimer.expired() ) {
myLed.on();
// now we must decide wether there have been enough blink cycles
blinkCount2--; // decrement counter
if ( blinkCount2 > 0 ) {
// still blinking ... so switch to BLINK_ON again
myTimer.setTime(blinkTime); // start time for on time
blinkState=BLINK_OFF2;
} else {
// blinking finished, wait for next button press
Serial.println("Blinking finished, now wait for button press.");
myLed.off();
blinkState = IDLE; //
}
}
break; //>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>
} //end FSM
//*************************************************************************
}