Electronics/DeskControl
1
0
Fork 0
Code Issues 2 Pull Requests Releases Activity

Migrated to Polulu VL53L0X library

Browse Source 
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
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

11
platformio.ini
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

4
src/include/config.h
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;

1
src/lib/motor.h
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

36
src/lib/screen/home.cpp
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;

2
src/lib/screen/home.h
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();
};

3
src/lib/screen/move.cpp
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>();
}

1059
src/lib/vl53l0x.cpp
View File

File diff suppressed because it is too large Load Diff

216
src/lib/vl53l0x.h
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,
private:
VL53L0X_Dev_t device;
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:
// 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

115
src/main.cpp
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");
}