DeskControl/src/lib/control.cpp

#include "./control.h"
#include <Wire.h>
#include "./debug.h"
#include "./motor.h"
#include "./state.h"
#include "./settings.h"
#include "include/config.h"


ControlManager Control = ControlManager();


VL53L0XInitResult ControlManager::init()
{
  //dln("[ CONTROL  ] Initializing");
  Wire.begin();

  this->heightSensor.setTimeout(500);
  auto result = this->heightSensor.init();
  if (result != VL53L0XInitResult::Success)
    return result;

  this->heightSensor.setMeasurementTimingBudget(Config::HeightSensorBudget);
  return result;
}


ControlUpdateResult ControlManager::update()
{
  // Always read the range if in async reading mode, even if we're not supposed to
  // be moving anymore, so it clears the status.
  bool asyncReading = heightSensor.asyncReading();

  uint16_t measurement = asyncReading
    ? heightSensor.asyncReadRangeSingleMillimeters()
    : VL53L0XRangeNotReady;


  //vl("[ CONTROL  ] Update: asyncReading = "); vl(asyncReading); vl(", measurement = "); vl(measurement);
  //vl(", moveDirection = "); vl((uint8_t)this->moveDirection); vl(", stabilizationStart = "); vln(this->stabilizationStart);


  bool moving = this->moveDirection != MoveDirection::None && this->stabilizationStart == 0;
  if (!moving && this->stabilizationStart == 0)
  {
    //vl("[ CONTROL  ] Idle");
    return ControlUpdateResult::Idle;
  }


  // Check for over-current
  if (moving && CurrentTime - lastCurrentCheck >= Config::MotorCurrentCheckInterval)
  {
    lastCurrentCheck = CurrentTime;

    if (motorIsOvercurrent())
    {
      //dln("[ CONTROL  ] Overcurrent detected!");
      this->moveStop();

      return ControlUpdateResult::Overcurrent;
    }
  }

  // Read sensor
  if (!asyncReading)
  {
    //vln("[ CONTROL  ] Starting async read");
    heightSensor.asyncStartReadRangeSingleMillimeters();
  }


  if (measurement != VL53L0XRangeNotReady)
  {
    if (measurement > Config::HeightMeasurementMax)
      // Treat invalid results as a timeout, so the checks further on are more readable
      measurement = VL53L0XRangeTimeout;


    //dl("[ CONTROL  ] Measurement: "); dln(measurement);
    this->lastMeasurement = measurement != VL53L0XRangeTimeout ? measurement : 0;
  }


  if (measurement != VL53L0XRangeNotReady && measurement != VL53L0XRangeTimeout)
  {
    this->currentMeasurement = measurement;
    this->lastValidMeasurement = CurrentTime;

    // TODO: PWM the motor when we're getting close, if overshoot turns out to be an issue in testing

    // Check if target has been reached
    if (moving && this->targetReached())
    {
      this->moveStop();

      return ControlUpdateResult::TargetReached;
    }
  }

  if (measurement == VL53L0XRangeTimeout && moving)
  {
    // While moving, allow incidental invalid results
    if (CurrentTime - lastValidMeasurement >= Config::HeightMeasurementAbortTimeout)
    {
      //dln("[ CONTROL  ] Timeout while moving!");
      this->moveStop();

      return ControlUpdateResult::SensorError;
    }
  }

  if (measurement == VL53L0XRangeTimeout && this->moveDirection == MoveDirection::None)
  {
    // In pure stabilization (not pre-move), immediately return the sensor error
    return ControlUpdateResult::SensorError;
  }



  if (this->stabilizationStart && this->moveDirection != MoveDirection::None)
  {
    // Pre-move stabilization
    if (this->stabilized())
    {
      //dln("[ CONTROL  ] Pre-move stabilization successful");

      // Sensor looks good, let's go!
      motorStart(this->moveDirection == MoveDirection::Up ? MotorDirection::Up : MotorDirection::Down);
      return ControlUpdateResult::Moving;
    }
    else if (CurrentTime - this->stabilizationStart >= Config::HeightMeasurementDeltaStableMoveTimeout)
    {
      //dln("[ CONTROL  ] Timeout in pre-move stabilization!");

      // Timeout expired, abort the move
      this->stabilizationStart = 0;
      this->moveDirection = MoveDirection::None;

      return ControlUpdateResult::SensorError;
    }
  }

  return moving ? ControlUpdateResult::Moving : ControlUpdateResult::Stabilizing;
}


void ControlManager::stabilizeStart()
{
  //dln("[ CONTROL  ] Starting stabilization");

  this->stabilizationStart = CurrentTime;
  this->stabilizeTargetMeasurement = 0;
}


bool ControlManager::stabilized()
{
  if (this->stabilizeTargetMeasurement == 0)
  {
    this->stabilizeTargetMeasurement = this->currentMeasurement;
    this->stabilizationLastCheck = this->lastValidMeasurement;
    return false;
  }

  // Only count if we have a new measurement
  if (this->stabilizationLastCheck == this->lastValidMeasurement)
    return false;

  this->stabilizationLastCheck = this->lastValidMeasurement;


  int16_t delta = this->currentMeasurement - this->stabilizeTargetMeasurement;
  if (abs(delta) <= Config::HeightMeasurementDeltaStable)
  {
    this->stabilizeCount++;

    if (this->stabilizeCount >= Config::HeightMeasurementDeltaStableCount)
    {
      //dln("[ CONTROL  ] Stable");
      this->stabilizationStart = 0;
      return true;
    }
  }
  else
  {
    // If it's this much off, chances are the actual value is somewhere in the middle
    this->stabilizeTargetMeasurement += (delta / 2);
    this->stabilizeCount = 0;
  }

  return false;
}


void ControlManager::moveStart(uint16_t height)
{
  //dl("[ CONTROL  ] Starting move to: "); dln(height);

  if (height < Settings.Height.Minimum)
  {
    //dl("[ CONTROL  ] Target is below minimum, changing to: "); dln(Settings.Height.Minimum);
    height = Settings.Height.Minimum;
  }
  else if (height > Settings.Height.Maximum)
  {
    //dl("[ CONTROL  ] Target is above maximum, changing to: "); dln(Settings.Height.Maximum);
    height = Settings.Height.Maximum;
  }

  this->moveTargetHeight = height;
  this->moveDirection = height > this->getCurrentHeight() ? MoveDirection::Up : MoveDirection::Down;

  // Wait for a stable result
  this->stabilizeStart();
}


void ControlManager::moveStop()
{
  //dln("[ CONTROL  ] Stopping move");

  motorStop();
  this->moveDirection = MoveDirection::None;
}



void ControlManager::snapToPreset()
{
  for (uint8_t i = 0; i < 2; i++)
  {
    if (abs(this->getCurrentHeight() - Settings.Height.Preset[i]) <= Config::HeightMeasurementDeltaOnTarget)
    {
      this->currentMeasurement = Settings.Height.Preset[i] - Settings.Height.Offset;
      break;
    }
  }
}


bool ControlManager::targetReached()
{
  auto currentHeight = this->getCurrentHeight();

  // Checks depend on the direction, so it returns true immediately if we've overshot the target
  switch (this->moveDirection)
  {
    case MoveDirection::Up:
      if (currentHeight >= this->moveTargetHeight - Config::HeightMeasurementDeltaStop)
      {
        // Snap to the target
        if (currentHeight - this->moveTargetHeight <= Config::HeightMeasurementDeltaOnTarget)
          this->currentMeasurement = this->moveTargetHeight - Settings.Height.Offset;

        //dln("[ CONTROL  ] Target reached");
        return true;
      }
      break;

    case MoveDirection::Down:
      if (currentHeight <= this->moveTargetHeight + Config::HeightMeasurementDeltaStop)
      {
        // Snap to the target
        if (this->moveTargetHeight - currentHeight <= Config::HeightMeasurementDeltaOnTarget)
          this->currentMeasurement = this->moveTargetHeight - Settings.Height.Offset;

        //dln("[ CONTROL  ] Target reached");
        return true;
      }
      break;

    default:
      break;
  }

  return false;
}


void ControlManager::getDisplayHeight(char* buffer, uint16_t value)
{
  if (value == 0)
  {
    buffer[0] = '-';
    buffer[1] = 0;
    return;
  }

  uint8_t displayValue = value / 10;

  if (displayValue > 99)
    buffer[0] = '0' + ((displayValue / 100) % 10);
  else
    buffer[0] = '0';

  buffer[1] = '.';
  buffer[2] = '0' + ((displayValue / 10) % 10);
  buffer[3] = '0' + (displayValue % 10);
  buffer[4] = 'm';
  buffer[5] = 0;
}