// Controlling multiple servo motors and pumps using the Arduino Mega.
// Version 1: https://wokwi.com/projects/369705616834606081
// Version 2: https://wokwi.com/projects/370198618373197825
// Version 3: https://wokwi.com/projects/370260137667236865
// Version 4: https://wokwi.com/projects/370269233463345153 Added separate callback functions for each button.
// Version 4.1: https://wokwi.com/projects/370440521607948289 Added buttons and servos for turntables.
// Version 5: https://wokwi.com/projects/370269233463345153 Modified turnLeft() and turnRight() to remember the current turn.
// Version 6: https://wokwi.com/projects/372320472308699137 Added extra buttons to control pumps. Changed to Arduino Mega for extra I/O.
// Version 7: https://wokwi.com/projects/372320472308699137 Added a state machine.
// Added a timer.
// Replaced mechButton with Debouncer to allow 500 ms for debouncing the reed switches.
// Added blinker class to blink the inbuilt LED.
#include <slowServo.h>
const byte TURN_1_DEGREES = 180;
const byte TURN_2_DEGREES = 0;
// Edit these angles for your mechanical layout.
const byte CLOSE_A1_DEGREES = 30;
const byte CLOSE_A2_DEGREES = 150;
const byte OPEN_A1_DEGREES = 90;
const byte OPEN_A2_DEGREES = 90;
const byte CLOSE_B1_DEGREES = 30;
const byte CLOSE_B2_DEGREES = 150;
const byte OPEN_B1_DEGREES = 90;
const byte OPEN_B2_DEGREES = 90;
const byte MS_PER_DEG = 30;
const uint32_t CLOSE_GATES_DELAY = 3000; // ms.
const uint32_t OPEN_GATES_DELAY = 3000; // ms.
const uint32_t TURNTABLE_DELAY = 7000; // ms.
// Edit these delays for your buttons and reed switches.
const uint16_t BUTTON_DEBOUNCE_DELAY = 50; // ms.
const uint16_t REED_SWITCH_DEBOUNCE_DELAY = 500; // ms.
// Edit these speeds for your pumps.
const byte PUMP_FLOW_SPEED = 192;
const byte PUMP_FILL_LOCK_SPEED = 128;
class Timer
{
private:
uint32_t timestamp;
uint32_t timespan;
public:
Timer()
{
}
void Start(uint32_t timespan)
{
this->timespan = timespan;
timestamp = millis();
}
bool Expired()
{
return (millis() - timestamp) >= timespan;
}
};
// Blinker to provide a heartbeat signal.
class Blinker : public idler
{
private:
Timer timer;
const uint32_t timespan = 500;
byte pin;
public:
Blinker(byte pin)
{
this->pin = pin;
}
void Begin()
{
pinMode(pin, OUTPUT);
timer.Start(timespan);
}
void idle()
{
if (timer.Expired())
{
digitalWrite(pin, !digitalRead(pin));
timer.Start(timespan);
}
}
};
class Debouncer : public idler
{
private:
Timer timer;
uint32_t debounceDelay;
byte pin;
byte state;
public:
Debouncer(byte pin, uint16_t debounceDelay)
{
this->pin = pin;
this->debounceDelay = debounceDelay;
}
void Begin()
{
pinMode(pin, INPUT_PULLUP);
state = digitalRead(pin);
}
byte State() const
{
return state;
}
void Update()
{
const bool newState = digitalRead(pin);
// Hysteresis:
// If there is no change, reset the debounce timer.
// Else, compare the time difference with the debounce delay.
if (newState == state)
{
timer.Start(debounceDelay);
}
else if (timer.Expired())
{
// Successfully debounced, so reset the debounce timer and update the state.
state = newState;
timer.Start(debounceDelay);
}
}
void idle()
{
Update();
}
};
class Pump
{
private:
byte pin;
byte speed;
public:
Pump(byte pin, byte initialSpeed)
{
this->pin = pin;
speed = initialSpeed;
}
void Begin()
{
analogWrite(pin, 0);
}
void SetSpeed(byte newSpeed)
{
speed = newSpeed;
analogWrite(pin, newSpeed);
}
void Start()
{
analogWrite(pin, speed);
}
void Stop()
{
analogWrite(pin, 0);
}
};
class Solenoid
{
private:
byte pin;
public:
Solenoid(byte pin)
{
this->pin = pin;
}
void Begin(byte initialState = HIGH)
{
pinMode(pin, OUTPUT);
digitalWrite(pin, initialState);
}
void Open()
{
digitalWrite(pin, LOW);
}
void Close()
{
digitalWrite(pin, HIGH);
}
};
class StateMachine : public idler
{
public:
StateMachine(void (*function)(void))
{
Callback = function;
}
void (*Callback)(void);
void idle()
{
if (Callback != nullptr)
{
Callback();
}
}
};
// Blinker.
Blinker blinker(LED_BUILTIN);
// Water pump control for flow speed.
Pump pump1(8, PUMP_FLOW_SPEED); // Red LED represents DC water Pump 1.
Pump pump2(9, PUMP_FLOW_SPEED); // Blue LED represent DC water Pump 2.
Pump pump3(10, PUMP_FLOW_SPEED); // Green LED represents DC water Pump 3.
Pump pump5(12, PUMP_FLOW_SPEED); // Yellow LED represents DC water Pump 5.
// Pump and solenoid control to fill and empty the lock.
Pump pump4(11, PUMP_FILL_LOCK_SPEED); // White LED represents DC water Pump 4.
Solenoid solenoid(36); // Limegreen LED represents 12 V solenoid to empty the lock.
// Sensors and timers to move the boat from the higher water level right side to the lower water level left side.
Debouncer startButton(34, BUTTON_DEBOUNCE_DELAY);
Debouncer closeGatesAReedSwitch(24, REED_SWITCH_DEBOUNCE_DELAY);
Timer closeGatesATimer;
Debouncer openGatesBReedSwitch(26, REED_SWITCH_DEBOUNCE_DELAY);
Timer openGatesBTimer;
Debouncer turntableLeftReedSwitch(30, REED_SWITCH_DEBOUNCE_DELAY);
Timer turntableLeftTimer;
// Sensors and timers to move the boat from the lower water level left side to the higher water level on the right side.
Debouncer closeGatesBReedSwitch(28, REED_SWITCH_DEBOUNCE_DELAY);
Timer closeGatesBTimer;
Debouncer openGatesAReedSwitch(22, REED_SWITCH_DEBOUNCE_DELAY);
Timer openGatesATimer;
Debouncer turntableRightReedSwitch(32, REED_SWITCH_DEBOUNCE_DELAY);
Timer turntableRightTimer;
// Servos to control the lock gates and turntables.
slowServo gatesA1Servo(5);
slowServo gatesA2Servo(4);
slowServo gatesB1Servo(6);
slowServo gatesB2Servo(7);
slowServo turntableLeftServo(3);
slowServo turntableRightServo(2);
void TurnServo(slowServo &servo, byte turn)
{
Serial.println("TurnServo()");
int deg = turn == 1 ? TURN_1_DEGREES : TURN_2_DEGREES;
servo.setDeg(deg);
}
void CloseGatesA()
{
Serial.println("CloseGatesA()");
gatesA1Servo.setDeg(CLOSE_A1_DEGREES);
gatesA2Servo.setDeg(CLOSE_A2_DEGREES);
}
void CloseGatesB()
{
Serial.println("CloseGatesB()");
gatesB1Servo.setDeg(CLOSE_B1_DEGREES);
gatesB2Servo.setDeg(CLOSE_B2_DEGREES);
}
void OpenGatesA()
{
Serial.println("OpenGatesA()");
gatesA1Servo.setDeg(OPEN_A1_DEGREES);
gatesA2Servo.setDeg(OPEN_A2_DEGREES);
}
void OpenGatesB()
{
Serial.println("OpenGatesB()");
gatesB1Servo.setDeg(OPEN_B1_DEGREES);
gatesB2Servo.setDeg(OPEN_B2_DEGREES);
}
// Create some states for the state machine.
enum class States : byte
{
S00_IDLE,
S01_FLOW_LEFT_UPPER,
S02_CLOSE_GATES_A,
S03_EMPTY_LOCK,
S04_OPEN_GATES_B,
S05_FLOW_LEFT_LOWER,
S06_TURN_LEFT,
S07_FLOW_RIGHT_LOWER,
S08_CLOSE_GATES_B,
S09_FILL_LOCK,
S10_OPEN_GATES_A,
S11_FLOW_RIGHT_UPPER,
S12_TURN_RIGHT
};
// The state machine logic that is periodically called by the idler update.
void StateMachineCallback()
{
static States presentState = States::S00_IDLE;
//
// Next state logic.
//
// The next state defaults to the present state;
States nextState = presentState;
switch (presentState)
{
case States::S00_IDLE:
if (!startButton.State())
{
nextState = States::S01_FLOW_LEFT_UPPER;
}
break;
case States::S01_FLOW_LEFT_UPPER:
if (!closeGatesAReedSwitch.State())
{
nextState = States::S02_CLOSE_GATES_A;
}
break;
case States::S02_CLOSE_GATES_A:
if (closeGatesATimer.Expired())
{
nextState = States::S03_EMPTY_LOCK;
}
break;
case States::S03_EMPTY_LOCK:
if (!openGatesBReedSwitch.State())
{
nextState = States::S04_OPEN_GATES_B;
}
break;
case States::S04_OPEN_GATES_B:
if (openGatesBTimer.Expired())
{
nextState = States::S05_FLOW_LEFT_LOWER;
}
break;
case States::S05_FLOW_LEFT_LOWER:
if (!turntableLeftReedSwitch.State())
{
nextState = States::S06_TURN_LEFT;
}
break;
case States::S06_TURN_LEFT:
if (turntableLeftTimer.Expired())
{
nextState = States::S07_FLOW_RIGHT_LOWER;
}
break;
case States::S07_FLOW_RIGHT_LOWER:
if (!closeGatesBReedSwitch.State())
{
nextState = States::S08_CLOSE_GATES_B;
}
break;
case States::S08_CLOSE_GATES_B:
if (closeGatesBTimer.Expired())
{
nextState = States::S09_FILL_LOCK;
}
break;
case States::S09_FILL_LOCK:
if (!openGatesAReedSwitch.State())
{
nextState = States::S10_OPEN_GATES_A;
}
break;
case States::S10_OPEN_GATES_A:
if (openGatesATimer.Expired())
{
nextState = States::S11_FLOW_RIGHT_UPPER;
}
break;
case States::S11_FLOW_RIGHT_UPPER:
if (!turntableRightReedSwitch.State())
{
nextState = States::S12_TURN_RIGHT;
}
break;
case States::S12_TURN_RIGHT:
if (turntableRightTimer.Expired())
{
nextState = States::S00_IDLE;
}
break;
default:
break;
}
if (nextState != presentState)
{
// Update the present state with the next state.
presentState = nextState;
//
// Output logic.
//
switch (presentState)
{
case States::S00_IDLE:
Serial.println("S00_IDLE");
// Switch everything off.
break;
case States::S01_FLOW_LEFT_UPPER:
Serial.println("S01_FLOW_LEFT_UPPER");
pump1.Start();
break;
case States::S02_CLOSE_GATES_A:
Serial.println("S02_CLOSE_GATES_A");
pump1.Stop();
CloseGatesA();
closeGatesATimer.Start(CLOSE_GATES_DELAY);
break;
case States::S03_EMPTY_LOCK:
Serial.println("S03_EMPTY_LOCK");
solenoid.Open();
break;
case States::S04_OPEN_GATES_B:
Serial.println("S04_OPEN_GATES_B");
solenoid.Close();
OpenGatesB();
openGatesBTimer.Start(OPEN_GATES_DELAY);
break;
case States::S05_FLOW_LEFT_LOWER:
Serial.println("S05_FLOW_LEFT_LOWER");
pump2.Start();
break;
case States::S06_TURN_LEFT:
Serial.println("S06_TURN_LEFT");
pump2.Stop();
{
static byte turn = 1;
turn = turn == 1 ? 2 : 1;
TurnServo(turntableLeftServo, turn);
turntableLeftTimer.Start(TURNTABLE_DELAY);
}
break;
case States::S07_FLOW_RIGHT_LOWER:
Serial.println("S07_FLOW_RIGHT_LOWER");
pump3.Start();
break;
case States::S08_CLOSE_GATES_B:
Serial.println("S08_CLOSE_GATES_B");
pump3.Stop();
CloseGatesB();
closeGatesBTimer.Start(CLOSE_GATES_DELAY);
break;
case States::S09_FILL_LOCK:
Serial.println("S09_FILL_LOCK");
pump4.Start();
break;
case States::S10_OPEN_GATES_A:
Serial.println("S10_OPEN_GATES_A");
pump4.Stop();
OpenGatesA();
openGatesATimer.Start(OPEN_GATES_DELAY);
break;
case States::S11_FLOW_RIGHT_UPPER:
Serial.println("S11_FLOW_RIGHT_UPPER");
pump5.Start();
break;
case States::S12_TURN_RIGHT:
Serial.println("S12_TURN_RIGHT");
pump5.Stop();
{
static byte turn = 1;
turn = turn == 1 ? 2 : 1;
TurnServo(turntableRightServo, turn);
turntableRightTimer.Start(TURNTABLE_DELAY);
}
break;
default:
break;
}
}
}
StateMachine stateMachine(StateMachineCallback);
void setup()
{
Serial.begin(115200);
startButton.Begin();
blinker.Begin();
closeGatesAReedSwitch.Begin();
closeGatesBReedSwitch.Begin();
openGatesAReedSwitch.Begin();
openGatesBReedSwitch.Begin();
turntableLeftReedSwitch.Begin();
turntableRightReedSwitch.Begin();
pump1.Begin();
pump2.Begin();
pump3.Begin();
pump4.Begin();
pump5.Begin();
solenoid.Begin();
// Add the blinker to the idler update.
blinker.hookup();
// Add the start button to the idler update.
startButton.hookup();
// Add the reed switches to the idler update.
closeGatesAReedSwitch.hookup();
openGatesBReedSwitch.hookup();
turntableLeftReedSwitch.hookup();
closeGatesBReedSwitch.hookup();
openGatesAReedSwitch.hookup();
turntableRightReedSwitch.hookup();
// Add the state machine to the idler update.
stateMachine.hookup();
Serial.println("S00_IDLE");
turntableLeftServo.begin();
turntableRightServo.begin();
turntableLeftServo.setDeg(TURN_1_DEGREES);
turntableRightServo.setDeg(TURN_1_DEGREES);
sleep(1000);
turntableLeftServo.setMsPerDeg(MS_PER_DEG);
turntableRightServo.setMsPerDeg(MS_PER_DEG);
gatesA1Servo.begin();
gatesA2Servo.begin();
gatesA1Servo.setDeg(OPEN_A1_DEGREES);
gatesA2Servo.setDeg(OPEN_A2_DEGREES);
sleep(1000);
gatesA1Servo.setMsPerDeg(MS_PER_DEG);
gatesA2Servo.setMsPerDeg(MS_PER_DEG);
gatesB1Servo.begin();
gatesB2Servo.begin();
gatesB1Servo.setDeg(CLOSE_B1_DEGREES);
gatesB2Servo.setDeg(CLOSE_B2_DEGREES);
sleep(1000);
gatesB1Servo.setMsPerDeg(MS_PER_DEG);
gatesB2Servo.setMsPerDeg(MS_PER_DEG);
}
void loop()
{
idle();
}
Left Turntable
Gates B
Lock
Gates A
Right Turntable