Migrated to Polulu VL53L0X library

Saves around 7.5kb of precious program space
This commit is contained in:
Mark van Renswoude 2020-01-30 15:16:52 +01:00
parent 5d013b4273
commit b1600ab1d6
11 changed files with 1361 additions and 185 deletions

View File

@ -38,3 +38,14 @@ upload_protocol = usbtiny
upload_flags = -e upload_flags = -e
upload_speed = 19200 upload_speed = 19200
; This environment is not actually one I use for the hardware, but
; PlatformIO's Home includes a very useful Inspect feature that
; requires a debug build. These builds are significantly larger and
; will cause the build to fail on the "checkprogsize" step.
; Targetting a microprocessor with more space allows the build to succeed.
[env:platformio-home-debug]
platform = atmelavr
board = atmega2560
board_build.f_cpu = 16000000L
framework = arduino

View File

@ -52,8 +52,12 @@ class Config
Settings for a VL53L0X time-of-flight sensor. Settings for a VL53L0X time-of-flight sensor.
*/ */
// Note that this must correspond to the default address of the module,
// the initialisation code skips reassigning the address to save calls.
static const uint8_t HeightSensorI2CAddress = 0x29; static const uint8_t HeightSensorI2CAddress = 0x29;
static const uint16_t HeightSensorBudget = 33000;
// Values above this will be disregarded as invalid // Values above this will be disregarded as invalid
static const uint16_t HeightMeasurementMax = 1500; static const uint16_t HeightMeasurementMax = 1500;

View File

@ -2,6 +2,7 @@
#define __motor #define __motor
// Low-level functions to control the motor
enum class MotorDirection enum class MotorDirection
{ {
Up = 0, Up = 0,

80
src/lib/motorstate.cpp Normal file
View File

@ -0,0 +1,80 @@
#include "./motorstate.h"
#include "./motor.h"
#include "./state.h"
#include "./settings.h"
#include "include/config.h"
void motorStateMoveTo(uint16_t height)
{
}
bool motorStateCheckTargetReached()
{
switch (State.MoveDirection)
{
case Direction::Up:
if (State.CurrentHeight >= State.MoveTarget - Config::HeightMeasurementDeltaStop)
{
if (State.CurrentHeight - State.MoveTarget <= Config::HeightMeasurementDeltaOnTarget)
State.CurrentHeight = State.MoveTarget;
motorStateStop();
return true;
}
break;
case Direction::Down:
if (State.CurrentHeight <= State.MoveTarget + Config::HeightMeasurementDeltaStop)
{
if (State.MoveTarget - State.CurrentHeight <= Config::HeightMeasurementDeltaOnTarget)
State.CurrentHeight = State.MoveTarget;
motorStateStop();
return true;
}
break;
default:
break;
}
return false;
}
bool motorStateCheckOverCurrent()
{
if (motorIsOverCurrent())
{
motorStateStop();
return true;
}
return false;
}
void motorStateStop()
{
motorStop();
State.MoveDirection = Direction::None;
}
void getDisplayHeight(char* buffer, uint16_t value)
{
uint8_t displayValue = (value + Settings.Height.Offset) / 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';
}

19
src/lib/motorstate.h Normal file
View File

@ -0,0 +1,19 @@
#ifndef __motorstate
#define __motorstate
#include <stdint.h>
// High-level functions to control the motor and update the global state
extern void motorStateMoveTo(uint16_t height);
extern bool motorStateCheckTargetReached();
extern bool motorStateCheckOverCurrent();
extern void motorStateStop();
// Formats a height value as "0.00m" (always exactly 5 characters long).
// Buffer must be at least 5 bytes long. No null character is added.
// The value is the raw height sensor value, the offset is added by this function.
extern void getDisplayHeight(char* buffer, uint16_t value);
#endif

View File

@ -1,7 +1,9 @@
#include "./home.h" #include "./home.h"
#include "./move.h"
#include "fonts/FreeSansBold18pt7b.trimmed.h" #include "fonts/FreeSansBold18pt7b.trimmed.h"
#include "include/config.h" #include "include/config.h"
#include "lib/settings.h" #include "lib/settings.h"
#include "lib/motorstate.h"
#define HOME_FONT_BASELINE 25 #define HOME_FONT_BASELINE 25
@ -16,22 +18,6 @@
#define PRESET_LINEHEIGHT (HOME_FONT_BASELINE + (2 * PRESET_MARGIN)) #define PRESET_LINEHEIGHT (HOME_FONT_BASELINE + (2 * PRESET_MARGIN))
// TODO move to shared file
// Formats a height value as "0.00m" (always exactly 5 characters long).
// Buffer must be at least 5 bytes long. No null character is added.
void getDisplayHeight(char* buffer, uint8_t value)
{
if (value > 99)
buffer[0] = '0' + ((value / 100) % 10);
else
buffer[0] = '0';
buffer[1] = '.';
buffer[2] = '0' + ((value / 10) % 10);
buffer[3] = '0' + (value % 10);
buffer[4] = 'm';
}
void HomeScreen::onShow() void HomeScreen::onShow()
{ {
@ -52,6 +38,22 @@ void HomeScreen::onShow()
void HomeScreen::onButton(Button button) void HomeScreen::onButton(Button button)
{ {
switch (button)
{
case Button::Up:
motorStateMoveTo(Settings.Height.Preset[0]);
this->getScreenManager()->show<MoveScreen>();
break;
case Button::Down:
motorStateMoveTo(Settings.Height.Preset[1]);
this->getScreenManager()->show<MoveScreen>();
break;
case Button::Menu:
// TODO show menu
break;
}
} }
@ -74,7 +76,7 @@ void HomeScreen::drawPreset2()
} }
void HomeScreen::drawPreset(uint8_t number, uint8_t y, uint8_t value) void HomeScreen::drawPreset(uint8_t number, uint8_t y, uint16_t value)
{ {
auto display = this->getDisplay(); auto display = this->getDisplay();
uint16_t numberColor; uint16_t numberColor;

View File

@ -23,7 +23,7 @@ class HomeScreen : public BaseScreen
void drawPreset1(); void drawPreset1();
void drawPreset2(); void drawPreset2();
void drawPreset(uint8_t number, uint8_t y, uint8_t value); void drawPreset(uint8_t number, uint8_t y, uint16_t value);
void drawMenu(); void drawMenu();
}; };

View File

@ -3,6 +3,7 @@
#include "fonts/FreeSansBold18pt7b.trimmed.h" #include "fonts/FreeSansBold18pt7b.trimmed.h"
#include "include/config.h" #include "include/config.h"
#include "lib/settings.h" #include "lib/settings.h"
#include "lib/motorstate.h"
#define MOVE_FONT_BASELINE 25 #define MOVE_FONT_BASELINE 25
@ -24,7 +25,7 @@ void MoveScreen::onShow()
void MoveScreen::onButton(Button button) void MoveScreen::onButton(Button button)
{ {
// TODO stop the motor motorStateStop();
this->getScreenManager()->show<HomeScreen>(); this->getScreenManager()->show<HomeScreen>();
} }

File diff suppressed because it is too large Load Diff

View File

@ -1,53 +1,189 @@
/* // Slightly modified version of https://github.com/pololu/vl53l0x-arduino
// which returns an error code if initialization fails.
The Adafruit VL53L0X library is included because it's an easy way to get #ifndef VL53L0X_h
access to the VL53L0X API headers. I will not be using the Adafruit library #define VL53L0X_h
however because it lacks a way to set the timing budget, which I found to be
very much required for accurate results.
*/ #include <Arduino.h>
#ifndef __vl53l0x
#define __vl53l0x
#include "Arduino.h" enum class VL53L0XInitResult
#include "Wire.h"
#include "vl53l0x_api.h"
typedef int8_t VL53L0XPosition;
typedef struct
{ {
VL53L0X_Error error; Success = 0,
VL53L0XPosition position; InvalidIdentification = 1,
} VL53L0XResult; GetSpadInfoFailed = 2,
VHVCalibrationFailed = 3,
PhaseCalibrationFailed = 4,
};
#define VL53L0X_POSITION_UNKNOWN ((VL53L0XPosition) 0)
// Positions for the init method
#define VL53L0X_POSITION_DATAINIT ((VL53L0XPosition) 1)
#define VL53L0X_POSITION_SETDEVICEADDRESS ((VL53L0XPosition) 2)
#define VL53L0X_POSITION_GETDEVICEINFO ((VL53L0XPosition) 3)
#define VL53L0X_POSITION_STATICINIT ((VL53L0XPosition) 4)
#define VL53L0X_POSITION_REFSPAD ((VL53L0XPosition) 5)
#define VL53L0X_POSITION_REFCAL ((VL53L0XPosition) 6)
#define VL53L0X_POSITION_DEVICEMODE ((VL53L0XPosition) 7)
#define VL53L0X_POSITION_LIMITRANGE ((VL53L0XPosition) 8)
#define VL53L0X_POSITION_LIMITRATE ((VL53L0XPosition) 9)
#define VL53L0X_POSITION_TRESHOLD1 ((VL53L0XPosition) 10)
#define VL53L0X_POSITION_TRESHOLD2 ((VL53L0XPosition) 11)
class VL53L0X class VL53L0X
{ {
public: public:
bool init(uint8_t address, VL53L0XResult* result); // register addresses from API vl53l0x_device.h (ordered as listed there)
bool setMeasurementTimingBudget(uint32_t budget_us, VL53L0X_Error* error); enum regAddr
bool getSingleRangingMeasurement(VL53L0X_RangingMeasurementData_t* data, VL53L0X_Error* error); {
bool getSingleRangingMeasurement(uint16_t* distanceMillimeters, VL53L0X_Error* error, uint16_t maxValidHeight); SYSRANGE_START = 0x00,
SYSTEM_THRESH_HIGH = 0x0C,
SYSTEM_THRESH_LOW = 0x0E,
SYSTEM_SEQUENCE_CONFIG = 0x01,
SYSTEM_RANGE_CONFIG = 0x09,
SYSTEM_INTERMEASUREMENT_PERIOD = 0x04,
SYSTEM_INTERRUPT_CONFIG_GPIO = 0x0A,
GPIO_HV_MUX_ACTIVE_HIGH = 0x84,
SYSTEM_INTERRUPT_CLEAR = 0x0B,
RESULT_INTERRUPT_STATUS = 0x13,
RESULT_RANGE_STATUS = 0x14,
RESULT_CORE_AMBIENT_WINDOW_EVENTS_RTN = 0xBC,
RESULT_CORE_RANGING_TOTAL_EVENTS_RTN = 0xC0,
RESULT_CORE_AMBIENT_WINDOW_EVENTS_REF = 0xD0,
RESULT_CORE_RANGING_TOTAL_EVENTS_REF = 0xD4,
RESULT_PEAK_SIGNAL_RATE_REF = 0xB6,
ALGO_PART_TO_PART_RANGE_OFFSET_MM = 0x28,
I2C_SLAVE_DEVICE_ADDRESS = 0x8A,
MSRC_CONFIG_CONTROL = 0x60,
PRE_RANGE_CONFIG_MIN_SNR = 0x27,
PRE_RANGE_CONFIG_VALID_PHASE_LOW = 0x56,
PRE_RANGE_CONFIG_VALID_PHASE_HIGH = 0x57,
PRE_RANGE_MIN_COUNT_RATE_RTN_LIMIT = 0x64,
FINAL_RANGE_CONFIG_MIN_SNR = 0x67,
FINAL_RANGE_CONFIG_VALID_PHASE_LOW = 0x47,
FINAL_RANGE_CONFIG_VALID_PHASE_HIGH = 0x48,
FINAL_RANGE_CONFIG_MIN_COUNT_RATE_RTN_LIMIT = 0x44,
PRE_RANGE_CONFIG_SIGMA_THRESH_HI = 0x61,
PRE_RANGE_CONFIG_SIGMA_THRESH_LO = 0x62,
PRE_RANGE_CONFIG_VCSEL_PERIOD = 0x50,
PRE_RANGE_CONFIG_TIMEOUT_MACROP_HI = 0x51,
PRE_RANGE_CONFIG_TIMEOUT_MACROP_LO = 0x52,
SYSTEM_HISTOGRAM_BIN = 0x81,
HISTOGRAM_CONFIG_INITIAL_PHASE_SELECT = 0x33,
HISTOGRAM_CONFIG_READOUT_CTRL = 0x55,
FINAL_RANGE_CONFIG_VCSEL_PERIOD = 0x70,
FINAL_RANGE_CONFIG_TIMEOUT_MACROP_HI = 0x71,
FINAL_RANGE_CONFIG_TIMEOUT_MACROP_LO = 0x72,
CROSSTALK_COMPENSATION_PEAK_RATE_MCPS = 0x20,
MSRC_CONFIG_TIMEOUT_MACROP = 0x46,
SOFT_RESET_GO2_SOFT_RESET_N = 0xBF,
IDENTIFICATION_MODEL_ID = 0xC0,
IDENTIFICATION_REVISION_ID = 0xC2,
OSC_CALIBRATE_VAL = 0xF8,
GLOBAL_CONFIG_VCSEL_WIDTH = 0x32,
GLOBAL_CONFIG_SPAD_ENABLES_REF_0 = 0xB0,
GLOBAL_CONFIG_SPAD_ENABLES_REF_1 = 0xB1,
GLOBAL_CONFIG_SPAD_ENABLES_REF_2 = 0xB2,
GLOBAL_CONFIG_SPAD_ENABLES_REF_3 = 0xB3,
GLOBAL_CONFIG_SPAD_ENABLES_REF_4 = 0xB4,
GLOBAL_CONFIG_SPAD_ENABLES_REF_5 = 0xB5,
GLOBAL_CONFIG_REF_EN_START_SELECT = 0xB6,
DYNAMIC_SPAD_NUM_REQUESTED_REF_SPAD = 0x4E,
DYNAMIC_SPAD_REF_EN_START_OFFSET = 0x4F,
POWER_MANAGEMENT_GO1_POWER_FORCE = 0x80,
VHV_CONFIG_PAD_SCL_SDA__EXTSUP_HV = 0x89,
ALGO_PHASECAL_LIM = 0x30,
ALGO_PHASECAL_CONFIG_TIMEOUT = 0x30,
};
enum vcselPeriodType { VcselPeriodPreRange, VcselPeriodFinalRange };
uint8_t last_status; // status of last I2C transmission
VL53L0X(void);
void setAddress(uint8_t new_addr);
inline uint8_t getAddress(void) { return address; }
VL53L0XInitResult init(bool io_2v8 = true);
void writeReg(uint8_t reg, uint8_t value);
void writeReg16Bit(uint8_t reg, uint16_t value);
void writeReg32Bit(uint8_t reg, uint32_t value);
uint8_t readReg(uint8_t reg);
uint16_t readReg16Bit(uint8_t reg);
uint32_t readReg32Bit(uint8_t reg);
void writeMulti(uint8_t reg, uint8_t const * src, uint8_t count);
void readMulti(uint8_t reg, uint8_t * dst, uint8_t count);
bool setSignalRateLimit(float limit_Mcps);
float getSignalRateLimit(void);
bool setMeasurementTimingBudget(uint32_t budget_us);
uint32_t getMeasurementTimingBudget(void);
bool setVcselPulsePeriod(vcselPeriodType type, uint8_t period_pclks);
uint8_t getVcselPulsePeriod(vcselPeriodType type);
void startContinuous(uint32_t period_ms = 0);
void stopContinuous(void);
uint16_t readRangeContinuousMillimeters(void);
uint16_t readRangeSingleMillimeters(void);
inline void setTimeout(uint16_t timeout) { io_timeout = timeout; }
inline uint16_t getTimeout(void) { return io_timeout; }
bool timeoutOccurred(void);
private: private:
VL53L0X_Dev_t device; // TCC: Target CentreCheck
// MSRC: Minimum Signal Rate Check
// DSS: Dynamic Spad Selection
struct SequenceStepEnables
{
boolean tcc, msrc, dss, pre_range, final_range;
};
struct SequenceStepTimeouts
{
uint16_t pre_range_vcsel_period_pclks, final_range_vcsel_period_pclks;
uint16_t msrc_dss_tcc_mclks, pre_range_mclks, final_range_mclks;
uint32_t msrc_dss_tcc_us, pre_range_us, final_range_us;
};
uint8_t address;
uint16_t io_timeout;
bool did_timeout;
uint16_t timeout_start_ms;
uint8_t stop_variable; // read by init and used when starting measurement; is StopVariable field of VL53L0X_DevData_t structure in API
uint32_t measurement_timing_budget_us;
bool getSpadInfo(uint8_t * count, bool * type_is_aperture);
void getSequenceStepEnables(SequenceStepEnables * enables);
void getSequenceStepTimeouts(SequenceStepEnables const * enables, SequenceStepTimeouts * timeouts);
bool performSingleRefCalibration(uint8_t vhv_init_byte);
static uint16_t decodeTimeout(uint16_t value);
static uint16_t encodeTimeout(uint32_t timeout_mclks);
static uint32_t timeoutMclksToMicroseconds(uint16_t timeout_period_mclks, uint8_t vcsel_period_pclks);
static uint32_t timeoutMicrosecondsToMclks(uint32_t timeout_period_us, uint8_t vcsel_period_pclks);
}; };
#endif #endif

View File

@ -10,6 +10,7 @@
#include "./lib/vl53l0x.h" #include "./lib/vl53l0x.h"
#include "./lib/state.h" #include "./lib/state.h"
#include "./lib/motor.h" #include "./lib/motor.h"
#include "./lib/motorstate.h"
#include "./lib/screen/home.h" #include "./lib/screen/home.h"
@ -17,8 +18,7 @@ enum class InitSequenceStep
{ {
EEPROM = 0, EEPROM = 0,
HeightSensorInit = 1, HeightSensorInit = 1,
HeightSensorBudget = 2, HeightSensorTest = 2,
HeightSensorTest = 3,
Last = HeightSensorTest Last = HeightSensorTest
}; };
@ -26,7 +26,7 @@ enum class InitSequenceStep
// Forward declarations // Forward declarations
inline void setupHeightSensor(); inline void setupHeightSensor();
inline uint8_t testHeightSensor(); inline uint16_t testHeightSensor();
void initSequenceStart(); void initSequenceStart();
void initSequenceSuccess(InitSequenceStep step); void initSequenceSuccess(InitSequenceStep step);
@ -34,6 +34,7 @@ void initSequenceError(InitSequenceStep step);
void initSequenceDisplayHeight(uint16_t measurement); void initSequenceDisplayHeight(uint16_t measurement);
void initSequenceEnd(); void initSequenceEnd();
bool heightSensorGetRange(uint16_t* measurement);
auto display = Adafruit_ST7789(Config::DisplayPortCS, Config::DisplayPortDC, Config::DisplayPortRST); auto display = Adafruit_ST7789(Config::DisplayPortCS, Config::DisplayPortDC, Config::DisplayPortRST);
@ -88,45 +89,55 @@ inline void setupHeightSensor()
{ {
Wire.begin(); Wire.begin();
VL53L0XResult result; heightSensor.setTimeout(500);
if (!heightSensor.init(Config::HeightSensorI2CAddress, &result)) auto error = heightSensor.init();
if (error != VL53L0XInitResult::Success)
{ {
initSequenceError(InitSequenceStep::HeightSensorInit); initSequenceError(InitSequenceStep::HeightSensorInit);
display.print(result.error); switch (error)
display.print(" @ "); {
display.print(result.position); case VL53L0XInitResult::InvalidIdentification:
display.print("Invalid identification");
break;
case VL53L0XInitResult::GetSpadInfoFailed:
display.print("GetSpadInfo failed");
break;
case VL53L0XInitResult::VHVCalibrationFailed:
display.print("VHV calibration failed");
break;
case VL53L0XInitResult::PhaseCalibrationFailed:
display.print("Phase calibration failed");
break;
default:
display.print("Unknown error");
break;
}
while(1); while(1);
} }
heightSensor.setMeasurementTimingBudget(Config::HeightSensorBudget);
initSequenceSuccess(InitSequenceStep::HeightSensorInit); initSequenceSuccess(InitSequenceStep::HeightSensorInit);
VL53L0X_Error error;
if (!heightSensor.setMeasurementTimingBudget(33000, &error))
{
initSequenceError(InitSequenceStep::HeightSensorBudget);
display.print(result.error);
while(1);
}
initSequenceSuccess(InitSequenceStep::HeightSensorBudget);
} }
inline uint8_t testHeightSensor() inline uint16_t testHeightSensor()
{ {
VL53L0X_Error error;
uint16_t reference = 0; uint16_t reference = 0;
uint8_t closeCount = 0; uint8_t closeCount = 0;
uint16_t measurement;
while (closeCount < 3) while (closeCount < 3)
{ {
uint16_t measurement; if (heightSensorGetRange(&measurement))
if (heightSensor.getSingleRangingMeasurement(&measurement, &error, Config::HeightMeasurementMax))
{ {
initSequenceDisplayHeight(measurement); initSequenceDisplayHeight(measurement);
@ -150,7 +161,7 @@ inline uint8_t testHeightSensor()
} }
initSequenceSuccess(InitSequenceStep::HeightSensorTest); initSequenceSuccess(InitSequenceStep::HeightSensorTest);
return reference / 10; return reference;
} }
@ -171,11 +182,8 @@ void loop()
if (State.MoveDirection != Direction::None) if (State.MoveDirection != Direction::None)
{ {
if (motorIsOverCurrent()) if (motorStateCheckOverCurrent())
{ {
motorStop();
State.MoveDirection = Direction::None;
// TODO go to overcurrent screen // TODO go to overcurrent screen
} }
else else
@ -191,54 +199,32 @@ uint32_t lastValidMeasurement;
void updateHeight() void updateHeight()
{ {
VL53L0X_Error error;
uint16_t measurement; uint16_t measurement;
if (heightSensor.getSingleRangingMeasurement(&measurement, &error, Config::HeightMeasurementMax)) if (heightSensorGetRange(&measurement))
{ {
State.CurrentHeight = measurement; State.CurrentHeight = measurement;
lastValidMeasurement = State.CurrentTime; lastValidMeasurement = State.CurrentTime;
if (motorStateCheckTargetReached())
// Check if we've reached the target screenManager.show<HomeScreen>();
switch (State.MoveDirection)
{
case Direction::Up:
if (measurement >= State.MoveTarget - Config::HeightMeasurementDeltaStop)
{
if (measurement - State.MoveTarget <= Config::HeightMeasurementDeltaOnTarget)
State.CurrentHeight = State.MoveTarget;
motorStop();
State.MoveDirection = Direction::None;
}
break;
case Direction::Down:
if (measurement <= State.MoveTarget + Config::HeightMeasurementDeltaStop)
{
if (State.MoveTarget - measurement <= Config::HeightMeasurementDeltaOnTarget)
State.CurrentHeight = State.MoveTarget;
motorStop();
State.MoveDirection = Direction::None;
}
break;
default:
break;
}
} }
else if (State.CurrentTime - lastValidMeasurement >= Config::HeightMeasurementAbortTimeout) else if (State.CurrentTime - lastValidMeasurement >= Config::HeightMeasurementAbortTimeout)
{ {
motorStop(); motorStateStop();
State.MoveDirection = Direction::None;
// TODO go to height sensor error screen // TODO go to height sensor error screen
} }
} }
bool heightSensorGetRange(uint16_t* measurement)
{
*measurement = heightSensor.readRangeSingleMillimeters();
return !heightSensor.timeoutOccurred() && *measurement <= Config::HeightMeasurementMax;
}
/* /*
For display sleep: For display sleep:
@ -283,9 +269,6 @@ void initSequenceStart()
display.print(" height sensor"); display.print(" height sensor");
display.setCursor(0, initSequenceTextY(2)); display.setCursor(0, initSequenceTextY(2));
display.print(" timing budget");
display.setCursor(0, initSequenceTextY(3));
display.print(" sensor test"); display.print(" sensor test");
} }