typedef enum {
Coast, /* IN1 = Low; IN2 = Low */
Reverse, /* IN1 = Low; IN2 = High */
Forward, /* IN1 = High; IN2 = Low */
Brake /* IN1 = High; IN2 = High */
} MP6515_eFunction_t;
typedef enum {
waiting_for_start_push_open,
debounce_end_push_open,
waiting_for_overload_open,
delay_after_stop_open,
waiting_for_overload_close,
waiting_for_start_push_close,
debounce_end_push_close
} eStateMachine_t;
typedef struct {
uint uiHall;
uint uiMm;
} tHall2mm;
void Motor_MP6513(MP6515_eFunction_t);
uint Motor_Current(void);
uint Hall_Read(void);
void PrintStatus(eStateMachine_t, uint, uint);
uint uiDebounce; // debounce counter variable
uint uiDelay; // delay timer variable
uint uiTimeout; // timeout for pressure too long
eStateMachine_t eState; // state machine value
int HallSensorPin = A0; // select the input pin for the Hall sensor
int CurrentSensorPin = A1; // select the input pin for the Motor Current
int MP6513_IN1_Pin = 16; // select the pin for the MP6513 IN1 pin
int MP6513_IN2_Pin = 17; // select the pin for the MP6513 IN2 pin
int Tick_Pin = 18; // select the pin for the tick check
unsigned long ulElapsed_us; // elapsed time since activation
#define DELAY_TICK_US 1000L // loop period in us
#define THRESHOLD_OVERLOAD 140 // analog threshold for the motor current sensor. TBD
#define THRESHOLD_PRESSURE 500 // pressure threshold level
#define THRESHOLD_DEBOUNCE_MS 300L // how long (ms) the input shall have a stable value before confirm it
#define THRESHOLD_DELAY_STOP_MS 300L // how long (ms) the stop command shall be active (brake)
#define THRESHOLD_TIMEOUT_MS 1000L // timeout (ms) for aborting the door opening command in case of too long pressure
#define THRESHOLD_BLIND_MOTOR_MS 100L // timeout (ms) for driving the motor without checking the overload. It is useful to de-angage the motor or to avoid overcurrent at first movment
#define THRESHOLD_TIMEOUT_OVERLOAD_MS 3000L // timeout (ms) after the motor drive in case the overload cannot be reached
#define THRESHOLD_DEBOUNCE_TICK (THRESHOLD_DEBOUNCE_MS * 1000L / DELAY_TICK_US) // how long (tick) the input shall have a stable value before confirm it
#define THRESHOLD_TIMEOUT_TICK (THRESHOLD_TIMEOUT_MS * 1000L / DELAY_TICK_US) // timeout (tick) for aborting the door opening command in case of too long pressure
#define THRESHOLD_DELAY_STOP_TICK (THRESHOLD_DELAY_STOP_MS * 1000L / DELAY_TICK_US) // how long (tick) the stop command shall be active (brake)
#define THRESHOLD_BLIND_MOTOR_TICK (THRESHOLD_BLIND_MOTOR_MS * 1000L / DELAY_TICK_US) // how long (tick) the motor can be driven without check on overvoltage
#define THRESHOLD_TIMEOUT_OVERLOAD_TICK (THRESHOLD_TIMEOUT_OVERLOAD_MS * 1000L / DELAY_TICK_US) // how long (tick) the motor can be driven without detectiong an overvoltage
#define FILTER_LOOP_CNT 8 // analog input filter parameter (average)
#define OPEN_IS_FORWARD false
#define CLOSE_SENSING false
#define DUMP_OUT true
#define CONTINUOUS_DUMP true
#define WAIT_FOR_USER_BEFORE_MOTOR_ACTION false
/*******************************************************************************
*
* Function name: setup
*
*******************************************************************************/
/*!
\brief setup
\version 1.0.0
\date 2022/05/05
\author FuzzyWaves Srl
\ingroup Init
\xrefitem main# "Init Functions Group" "Init Functions Group"
*******************************************************************************/
void setup()
{
pinMode(MP6513_IN1_Pin, OUTPUT);
pinMode(MP6513_IN2_Pin, OUTPUT);
pinMode(Tick_Pin, OUTPUT);
digitalWrite(Tick_Pin, LOW);
Motor_MP6513(Coast);
uiDebounce = 0;
uiDelay = 0;
eState = delay_after_stop_open;
Serial1.begin(115200);
Serial1.println("Pi Pico Serial Monitor example!");
Serial1.println("-------------------------------");
//#if DUMP_OUT == true
// Serial1.begin(115200);
// Serial1.println("Power On!");
//#endif
ulElapsed_us = micros();
}
/*******************************************************************************
*
* Function name: loop
*
*******************************************************************************/
/*!
\brief loop
This loop is called forever. It will manage the application state machine
\version 1.0.0
\date 2022/05/05
\author FuzzyWaves Srl
\ingroup Loop
\xrefitem main# "Loop Functions Group" "Loop Functions Group"
*******************************************************************************/
void loop()
{
uint uiHallValue;
uint uiMotorCurrent;
byte pucInput[100];
while (micros() - ulElapsed_us < (unsigned long)DELAY_TICK_US)
; // wait for the loop period
digitalWrite(Tick_Pin, HIGH);
ulElapsed_us += (unsigned long)DELAY_TICK_US;
uiHallValue = Hall_Read();
uiMotorCurrent = Motor_Current();
PrintStatus(eState, uiHallValue, uiMotorCurrent);
switch (eState) {
case waiting_for_start_push_open:
if (uiHallValue < THRESHOLD_PRESSURE) {
uiDebounce++;
if (uiDebounce >= THRESHOLD_DEBOUNCE_TICK) {
uiDebounce = 0;
uiTimeout = 0;
eState = debounce_end_push_open;
}
} else
uiDebounce = 0;
break;
case debounce_end_push_open:
if (uiHallValue < THRESHOLD_PRESSURE) {
uiDebounce = 0;
uiTimeout++;
if (uiTimeout >= THRESHOLD_TIMEOUT_TICK) {
#if DUMP_OUT == true
Serial1.println("abort opening");
#endif
eState = debounce_end_push_close;
}
} else {
uiDebounce++;
if (uiDebounce >= THRESHOLD_DEBOUNCE_TICK) {
uiDebounce = 0;
uiDelay = 0;
#if OPEN_IS_FORWARD == true
Motor_MP6513(Forward);
#else
Motor_MP6513(Reverse);
#endif
eState = waiting_for_overload_open;
}
}
break;
case waiting_for_overload_open:
uiDelay++;
if (uiDelay >= THRESHOLD_BLIND_MOTOR_TICK) {
if ((uiMotorCurrent >= THRESHOLD_OVERLOAD) || (uiDelay >= THRESHOLD_TIMEOUT_OVERLOAD_TICK)) {
Motor_MP6513(Brake);
uiDelay = 0;
eState = delay_after_stop_open;
}
}
break;
case delay_after_stop_open:
uiDelay++;
if (uiDelay >= THRESHOLD_DELAY_STOP_TICK) {
uiDelay = 0;
#if OPEN_IS_FORWARD == true
Motor_MP6513(Reverse);
#else
Motor_MP6513(Forward);
#endif
eState = waiting_for_overload_close;
}
break;
case waiting_for_overload_close:
uiDelay++;
if (uiDelay >= THRESHOLD_BLIND_MOTOR_TICK) {
if ((uiMotorCurrent >= THRESHOLD_OVERLOAD) || (uiDelay >= THRESHOLD_TIMEOUT_OVERLOAD_TICK)) {
Motor_MP6513(Coast);
uiDebounce = 0;
if (CLOSE_SENSING == true)
eState = waiting_for_start_push_close;
else
eState = waiting_for_start_push_open;
}
}
break;
case waiting_for_start_push_close:
if (uiHallValue < THRESHOLD_PRESSURE) {
uiDebounce++;
if (uiDebounce >= THRESHOLD_DEBOUNCE_TICK) {
uiDebounce = 0;
eState = debounce_end_push_close;
}
} else
uiDebounce = 0;
break;
case debounce_end_push_close:
if (uiHallValue < THRESHOLD_PRESSURE)
uiDebounce = 0;
else {
uiDebounce++;
if (uiDebounce >= THRESHOLD_DEBOUNCE_TICK) {
uiDebounce = 0;
eState = waiting_for_start_push_open;
}
}
break;
default:
break;
}
digitalWrite(Tick_Pin, LOW);
}
/*******************************************************************************
*
* Function name: Motor_MP6513
*
*******************************************************************************/
/*!
\brief Motor_MP6513
This function controls the motor via MP6513 driver.
Two control lines (IN1 & IN2) are used as follows:
Coast = IN1 = Low; IN2 = Low
Reverse = IN1 = Low; IN2 = High
Forward = IN1 = High; IN2 = Low
Brake = IN1 = High; IN2 = High
\version 1.0.0
\date 2022/05/05
\author FuzzyWaves Srl
\ingroup Motor
\xrefitem main# "Motor Functions Group" "Motor Functions Group"
*******************************************************************************/
void Motor_MP6513(MP6515_eFunction_t efunction)
{
#if WAIT_FOR_USER_BEFORE_MOTOR_ACTION == true
while (!Serial1.available()) {
}
while (Serial1.available()) {
// get the new byte:
char inChar = (char)Serial1.read();
}
#endif
switch (efunction) {
case Coast:
#if DUMP_OUT == true
Serial1.println("-> Coast");
#endif
digitalWrite(MP6513_IN1_Pin, LOW);
digitalWrite(MP6513_IN2_Pin, LOW);
break;
case Reverse:
#if DUMP_OUT == true
Serial1.println("-> Reverse");
#endif
digitalWrite(MP6513_IN1_Pin, LOW);
digitalWrite(MP6513_IN2_Pin, HIGH);
break;
case Forward:
#if DUMP_OUT == true
Serial1.println("-> Forward");
#endif
digitalWrite(MP6513_IN1_Pin, HIGH);
digitalWrite(MP6513_IN2_Pin, LOW);
break;
case Brake:
#if DUMP_OUT == true
Serial1.println("-> Brake");
#endif
digitalWrite(MP6513_IN1_Pin, HIGH);
digitalWrite(MP6513_IN2_Pin, HIGH);
break;
default:
break;
}
}
/*******************************************************************************
*
* Function name: Motor_Current
*
*******************************************************************************/
/*!
\brief Motor_Current
This function reads the analog value of the current used by the motor.
\version 1.0.0
\date 2022/05/05
\author FuzzyWaves Srl
\ingroup Motor
\xrefitem main# "Motor Functions Group" "Motor Functions Group"
*******************************************************************************/
uint Motor_Current(void)
{
uint uiFilterLoop;
uint uiReadOut;
uiReadOut = 0;
for (uiFilterLoop = 0; uiFilterLoop < FILTER_LOOP_CNT; uiFilterLoop++) {
uiReadOut += analogRead(CurrentSensorPin);
}
return (uiReadOut / FILTER_LOOP_CNT);
}
/*******************************************************************************
*
* Function name: Hall_Read
*
*******************************************************************************/
/*!
\brief Hall_Read
This function reads the hall sensor averaging it.
\return uiReadOut (bin)
\version 1.0.0
\date 2022/05/05
\author FuzzyWaves Srl
\ingroup PressureDetection
\xrefitem main# "PressureDetection Functions Group" "PressureDetection Functions Group"
*******************************************************************************/
uint Hall_Read(void)
{
uint uiFilterLoop;
uint uiReadOut;
uiReadOut = 0;
for (uiFilterLoop = 0; uiFilterLoop < FILTER_LOOP_CNT; uiFilterLoop++) {
uiReadOut += analogRead(HallSensorPin);
}
return (uiReadOut / FILTER_LOOP_CNT);
}
/*******************************************************************************
*
* Function name: PrintStatus
*
*******************************************************************************/
/*!
\brief PrintStatus
Debug function that send over the debug serial line the state machine phase
and the read values of Hall sensor and motor current.
\version 1.0.0
\date 2022/05/05
\author FuzzyWaves Srl
\ingroup Debug
\xrefitem main# "Debug Functions Group" "Debug Functions Group"
*******************************************************************************/
void PrintStatus(eStateMachine_t eStateMachine, uint uiHallValue, uint uiMotorCurrent)
{
#if DUMP_OUT == true
static eStateMachine_t eLocState = debounce_end_push_close;
#if CONTINUOUS_DUMP == true
if (eLocState != eStateMachine)
#endif
{
eLocState = eStateMachine;
switch (eStateMachine) {
case waiting_for_start_push_open:
Serial1.print("waiting_for_start_push_open");
break;
case debounce_end_push_open:
Serial1.print("debounce_end_push_open");
break;
case waiting_for_overload_open:
Serial1.print("wait_for_overload_open");
break;
case delay_after_stop_open:
Serial1.print("delay_after_stop_open");
break;
case waiting_for_overload_close:
Serial1.print("waiting_for_overload_close");
break;
case waiting_for_start_push_close:
Serial1.print("waiting_for_start_push_close");
break;
case debounce_end_push_close:
Serial1.print("debounce_end_push_close");
break;
default:
break;
}
Serial1.print(" Hall: ");
Serial1.print(uiHallValue);
Serial1.print(" Motor Current: ");
Serial1.println(uiMotorCurrent);
}
#endif
}