Fixed noise and unreliability issues, cleaned up code

This commit is contained in:
Mark van Renswoude 2019-01-07 20:07:50 +01:00
parent fa53deb9d0
commit fb32be56d3

View File

@ -34,8 +34,9 @@ static const uint16_t ServoMinPulse = 544;
static const uint16_t ServoMaxPulse = 2400; static const uint16_t ServoMaxPulse = 2400;
static const uint16_t ServoPulseInterval = 20; static const uint16_t ServoPulseInterval = 20; // How often servo pulses are sent (around 20 ms is the protocol)
static const uint16_t ServoStabilizeDelay = 100; static const uint16_t ServoHoldDelay = 500; // How long the position is held after moving, to account for the servo catching up
static const uint16_t ServoStabilizeDelay = 200; // How long to wait for the voltage to stabilize before checking calibration
int offPosition; // 0 - 180 degrees, read from "Off" potentiometer int offPosition; // 0 - 180 degrees, read from "Off" potentiometer
@ -44,16 +45,28 @@ int speed; // 0 - 1023, read from "Speed" potentiometer
float degreesPerMilli; // Degrees per millisecond float degreesPerMilli; // Degrees per millisecond
enum State
{
Idle,
Holding,
Stabilizing,
Moving,
Calibrating,
ZzZzZz
};
State state; // The current state
bool lastOn; // If the switch was on when it was last flipped bool lastOn; // If the switch was on when it was last flipped
int startPosition; // The position the servo was in when the switch was flipped int startPosition; // The position the servo was in when the switch was flipped
unsigned long startTime; // The last time the switch was flipped unsigned long startTime; // The last time the switch was flipped
unsigned long idleTime; // The last time anything happened unsigned long lastChangeTime; // The last time anything happened
bool servoEnabled; // Whether or not pulses should be sent to the servo
int servoPosition; // The desired position of the servo int servoPosition; // The desired position of the servo
unsigned long servoPulseTime; // The last time the servo pulse was sent unsigned long servoPulseTime; // The last time the servo pulse was sent
#ifdef DEBUG #ifdef DEBUG
SoftwareSerial debug(-1,3); SoftwareSerial debug(-1,3);
#endif #endif
@ -68,6 +81,13 @@ void sleepUntilSwitch();
float floatMap(float x, float in_min, float in_max, float out_min, float out_max); float floatMap(float x, float in_min, float in_max, float out_min, float out_max);
// Macros
#define setState(newState) { state = newState; lastChangeTime = currentTime; }
#define isServoEnabled() (state == Holding || state == Moving || state == Calibrating)
#define timePassed() (currentTime - lastChangeTime)
void setup() void setup()
{ {
#ifdef DEBUG #ifdef DEBUG
@ -86,12 +106,11 @@ void setup()
updateSettings(); updateSettings();
lastOn = false; state = Holding;
startPosition = offPosition; startPosition = offPosition;
servoPosition = offPosition; servoPosition = offPosition;
servoPulseTime = 0; servoPulseTime = 0;
servoEnabled = true;
#ifdef DEBUG #ifdef DEBUG
debug.println("Started"); debug.println("Started");
@ -103,7 +122,7 @@ void loop()
{ {
unsigned long currentTime = millis(); unsigned long currentTime = millis();
if (servoEnabled) if (isServoEnabled())
pulseServo(currentTime); pulseServo(currentTime);
bool isOn = (digitalRead(PinSwitch) == LOW); bool isOn = (digitalRead(PinSwitch) == LOW);
@ -118,56 +137,82 @@ void loop()
lastOn = isOn; lastOn = isOn;
startPosition = servoPosition; startPosition = servoPosition;
startTime = currentTime; startTime = currentTime;
idleTime = startTime;
setState(Moving);
} }
int targetPosition = isOn ? onPosition : offPosition;
if (servoPosition != targetPosition) switch (state)
{ {
servoPosition = getNewPosition(currentTime, targetPosition); case Idle:
servoEnabled = true;
idleTime = currentTime;
}
else if (servoEnabled)
{ {
// Ensure we pulsed the last position // Only check the settings first when the servo is idle, as the
currentTime = millis();
delay(ServoPulseInterval);
pulseServo(currentTime);
delay(ServoPulseInterval);
// Stop sending pulses when we reach the destination, to prevent
// power consumption and possible buzzing
servoEnabled = false;
idleTime = currentTime;
}
// Don't update the settings while the servo is moving, as the
// voltage drop can cause fluctuating results // voltage drop can cause fluctuating results
if (!servoEnabled && currentTime - idleTime >= ServoStabilizeDelay)
{
if (updateSettings()) if (updateSettings())
{ {
// Immediately go to the new position setState(Calibrating);
currentTime = millis();
servoPosition = isOn ? onPosition : offPosition; servoPosition = isOn ? onPosition : offPosition;
delay(ServoPulseInterval);
pulseServo(currentTime);
delay(ServoPulseInterval);
idleTime = currentTime;
} }
} else if (timePassed() >= SleepTime)
{
// Stay awake for a bit to allow changes to the on and off positions state = ZzZzZz;
// to take effect immediately
if (currentTime - idleTime >= SleepTime)
sleepUntilSwitch(); sleepUntilSwitch();
} }
break;
}
case Holding:
{
// Keep the servo signal on until ServoHoldDelay has passed
if (timePassed() >= ServoHoldDelay)
setState(Stabilizing);
break;
}
case Stabilizing:
{
// Keep the servo signal off until ServoStabilizeDelay has passed
if (timePassed() >= ServoStabilizeDelay)
setState(Idle);
break;
}
case Moving:
{
int targetPosition = isOn ? onPosition : offPosition;
servoPosition = getNewPosition(currentTime, targetPosition);
if (servoPosition == targetPosition)
setState(Holding);
break;
}
case Calibrating:
{
// While calibrating, update immediatly, slight variations
// aren't that much of a concern at this point
if (updateSettings())
{
lastChangeTime = currentTime;
servoPosition = isOn ? onPosition : offPosition;
}
else if (timePassed() >= ServoHoldDelay)
setState(Idle);
break;
}
case ZzZzZz:
// Apparantly we were woken up without a switch change, go back to sleep
sleepUntilSwitch();
break;
}
}
void pulseServo(unsigned long currentTime) void pulseServo(unsigned long currentTime)
{ {