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_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.
*/
// 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 uint16_t HeightSensorBudget = 33000;
// Values above this will be disregarded as invalid
static const uint16_t HeightMeasurementMax = 1500;

View File

@ -2,6 +2,7 @@
#define __motor
// Low-level functions to control the motor
enum class MotorDirection
{
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 "./move.h"
#include "fonts/FreeSansBold18pt7b.trimmed.h"
#include "include/config.h"
#include "lib/settings.h"
#include "lib/motorstate.h"
#define HOME_FONT_BASELINE 25
@ -16,22 +18,6 @@
#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()
{
@ -52,6 +38,22 @@ void HomeScreen::onShow()
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();
uint16_t numberColor;

View File

@ -23,7 +23,7 @@ class HomeScreen : public BaseScreen
void drawPreset1();
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();
};

View File

@ -3,6 +3,7 @@
#include "fonts/FreeSansBold18pt7b.trimmed.h"
#include "include/config.h"
#include "lib/settings.h"
#include "lib/motorstate.h"
#define MOVE_FONT_BASELINE 25
@ -24,7 +25,7 @@ void MoveScreen::onShow()
void MoveScreen::onButton(Button button)
{
// TODO stop the motor
motorStateStop();
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
access to the VL53L0X API headers. I will not be using the Adafruit library
however because it lacks a way to set the timing budget, which I found to be
very much required for accurate results.
#ifndef VL53L0X_h
#define VL53L0X_h
*/
#include <Arduino.h>
#ifndef __vl53l0x
#define __vl53l0x
#include "Arduino.h"
#include "Wire.h"
#include "vl53l0x_api.h"
typedef int8_t VL53L0XPosition;
typedef struct
enum class VL53L0XInitResult
{
VL53L0X_Error error;
VL53L0XPosition position;
} VL53L0XResult;
Success = 0,
InvalidIdentification = 1,
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
{
public:
bool init(uint8_t address, VL53L0XResult* result);
bool setMeasurementTimingBudget(uint32_t budget_us, VL53L0X_Error* error);
bool getSingleRangingMeasurement(VL53L0X_RangingMeasurementData_t* data, VL53L0X_Error* error);
bool getSingleRangingMeasurement(uint16_t* distanceMillimeters, VL53L0X_Error* error, uint16_t maxValidHeight);
// register addresses from API vl53l0x_device.h (ordered as listed there)
enum regAddr
{
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:
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

View File

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