LiDAR test program working
This commit is contained in:
parent
a4a10ec3a1
commit
d12da1a678
|
|
@ -37,3 +37,9 @@ git format-patch HEAD~2 -o outbox
|
|||
This creates patchfiles for the last 2 commits before HEAD. The files that Git places in the "outbox" folder are a standardized plaintext representation of a commit. You can inspect them to ensure they're correct. You can then attach these files to an email in your GUI email application of choice, or other messenger application. (Note: Maintainers of professional projects that claim to accept email patches will not be happy if you do this, but I think it's much easier.)
|
||||
|
||||
Once I receive your changes, I'll review them and merge them into the repository. The resulting commits will still list you as the author.
|
||||
|
||||
|
||||
TODO: Did I ever fix the issue where we weren't passing the correct optimization option?
|
||||
The first one is using the correct
|
||||
|
||||
TODO: document error codes?
|
||||
|
|
|
|||
|
|
@ -19,6 +19,7 @@ const gpio = rp2040.gpio;
|
|||
|
||||
const pin_config = rp2040.pins.GlobalConfiguration {
|
||||
// 6 colored LEDs
|
||||
// These are not in the order that they are on the board!
|
||||
.GPIO0 = .{
|
||||
.name = "led_4",
|
||||
.direction = .out,
|
||||
|
|
@ -205,6 +206,55 @@ const pin_config = rp2040.pins.GlobalConfiguration {
|
|||
|
||||
var pins: rp2040.pins.Pins(pin_config) = undefined;
|
||||
|
||||
// const Led = struct {
|
||||
// pin: hal.gpio.Pin,
|
||||
|
||||
// fn on (self: Led) void {
|
||||
// self.pin.put(0);
|
||||
// }
|
||||
// fn off () void {
|
||||
// self.pin.put(1);
|
||||
// }
|
||||
|
||||
|
||||
// }
|
||||
|
||||
pub fn pin_error(error_code: u6) noreturn {
|
||||
// In order on the board, 4, 3, 5, 2, 6, 1
|
||||
var mut_error_code = error_code;
|
||||
// TODO: invert
|
||||
pins.led_1.put(@truncate(mut_error_code & 0x01));
|
||||
mut_error_code >>= 1;
|
||||
pins.led_6.put(@truncate(mut_error_code & 0x01));
|
||||
mut_error_code >>= 1;
|
||||
pins.led_2.put(@truncate(mut_error_code & 0x01));
|
||||
mut_error_code >>= 1;
|
||||
pins.led_5.put(@truncate(mut_error_code & 0x01));
|
||||
mut_error_code >>= 1;
|
||||
pins.led_3.put(@truncate(mut_error_code & 0x01));
|
||||
mut_error_code >>= 1;
|
||||
pins.led_4.put(@truncate(mut_error_code & 0x01));
|
||||
while (true) {
|
||||
time.sleep_ms(500);
|
||||
}
|
||||
}
|
||||
|
||||
test {
|
||||
var mut_error_code: u6 = 0b010101;
|
||||
std.log.warn("\n{}", .{ @as(u1, @truncate(mut_error_code & 0x1)) });
|
||||
mut_error_code >>= 1;
|
||||
std.log.warn("{}", .{ @as(u1, @truncate(mut_error_code & 0x1)) });
|
||||
mut_error_code >>= 1;
|
||||
std.log.warn("{}", .{ @as(u1, @truncate(mut_error_code & 0x1)) });
|
||||
mut_error_code >>= 1;
|
||||
std.log.warn("{}", .{ @as(u1, @truncate(mut_error_code & 0x1)) });
|
||||
mut_error_code >>= 1;
|
||||
std.log.warn("{}", .{ @as(u1, @truncate(mut_error_code & 0x1)) });
|
||||
mut_error_code >>= 1;
|
||||
std.log.warn("{}", .{ @as(u1, @truncate(mut_error_code & 0x1)) });
|
||||
}
|
||||
|
||||
|
||||
// Our main I2C, used for communicating with the LiDARs, is I2C 0
|
||||
const i2c0 = i2c.num(0);
|
||||
|
||||
|
|
@ -239,6 +289,77 @@ const LiDAR = struct {
|
|||
|
||||
i2c_address: i2c.Address, // = @enumFromInt(0x29); // The default I2C address is 0x29 (datasheet)
|
||||
|
||||
pub fn setup(self: LiDAR) void {
|
||||
// Loop until we're alive
|
||||
var res: u8 = 0x00;
|
||||
const VL6180X_REG_SYSTEM_FRESH_OUT_OF_RESET: u16 = 0x0016;
|
||||
while (res != 0x01) {
|
||||
res = self.read8(VL6180X_REG_SYSTEM_FRESH_OUT_OF_RESET);
|
||||
time.sleep_ms(1);
|
||||
// The datasheet says I should really toggle the ce pin but I don't feel like it and it wasn't needed in the test program
|
||||
}
|
||||
|
||||
// Copied from https://github.com/adafruit/Adafruit_VL6180X/blob/a42df76ee9d469111de0875b11f26ddb37608ea2/Adafruit_VL6180X.cpp#L121
|
||||
self.write8(0x0207, 0x01);
|
||||
self.write8(0x0208, 0x01);
|
||||
self.write8(0x0096, 0x00);
|
||||
self.write8(0x0097, 0xfd);
|
||||
self.write8(0x00e3, 0x00);
|
||||
self.write8(0x00e4, 0x04);
|
||||
self.write8(0x00e5, 0x02);
|
||||
self.write8(0x00e6, 0x01);
|
||||
self.write8(0x00e7, 0x03);
|
||||
self.write8(0x00f5, 0x02);
|
||||
self.write8(0x00d9, 0x05);
|
||||
self.write8(0x00db, 0xce);
|
||||
self.write8(0x00dc, 0x03);
|
||||
self.write8(0x00dd, 0xf8);
|
||||
self.write8(0x009f, 0x00);
|
||||
self.write8(0x00a3, 0x3c);
|
||||
self.write8(0x00b7, 0x00);
|
||||
self.write8(0x00bb, 0x3c);
|
||||
self.write8(0x00b2, 0x09);
|
||||
self.write8(0x00ca, 0x09);
|
||||
self.write8(0x0198, 0x01);
|
||||
self.write8(0x01b0, 0x17);
|
||||
self.write8(0x01ad, 0x00);
|
||||
self.write8(0x00ff, 0x05);
|
||||
self.write8(0x0100, 0x05);
|
||||
self.write8(0x0199, 0x05);
|
||||
self.write8(0x01a6, 0x1b);
|
||||
self.write8(0x01ac, 0x3e);
|
||||
self.write8(0x01a7, 0x1f);
|
||||
self.write8(0x0030, 0x00);
|
||||
|
||||
self.write8(0x0011, 0x10); // Enables polling for 'New Sample ready'
|
||||
// when measurement completes
|
||||
self.write8(0x010a, 0x30); // Set the averaging sample period
|
||||
// (compromise between lower noise and
|
||||
// increased execution time)
|
||||
self.write8(0x003f, 0x46); // Sets the light and dark gain (upper
|
||||
// nibble). Dark gain should not be
|
||||
// changed.
|
||||
self.write8(0x0031, 0xFF); // sets the # of range measurements after
|
||||
// which auto calibration of system is
|
||||
// performed
|
||||
self.write8(0x0041, 0x63); // Set ALS integration time to 100ms
|
||||
self.write8(0x002e, 0x01); // perform a single temperature calibration
|
||||
// of the ranging sensor
|
||||
|
||||
// Optional: Public registers - See data sheet for more detail
|
||||
// self.write8(SYSRANGE__INTERMEASUREMENT_PERIOD, 0x09); // Set default ranging inter-measurement
|
||||
// period to 100ms
|
||||
self.write8(0x003e, 0x31); // Set default ALS inter-measurement period
|
||||
// to 500ms
|
||||
self.write8(0x0014, 0x24); // Configures interrupt on 'New Sample
|
||||
// Ready threshold event'
|
||||
|
||||
const VL6180X_REG_IDENTIFICATION_MODEL_ID = 0x000;
|
||||
if (self.read8(VL6180X_REG_IDENTIFICATION_MODEL_ID) != 0xB4) {
|
||||
pin_error(0b111111);
|
||||
}
|
||||
}
|
||||
|
||||
pub fn updateReading (self: *LiDAR) void {
|
||||
// Adapted from:
|
||||
// => https://github.com/adafruit/Adafruit_VL6180X/blob/a42df76/Adafruit_VL6180X.cpp#L187
|
||||
|
|
@ -247,6 +368,8 @@ const LiDAR = struct {
|
|||
const VL6180X_REG_RESULT_RANGE_STATUS = 0x04d;
|
||||
while ((self.read8(VL6180X_REG_RESULT_RANGE_STATUS) & 0x01) == 0) {}
|
||||
|
||||
pins.led_3.put(0);
|
||||
|
||||
// Start a range measurement
|
||||
const VL6180X_REG_SYSRANGE_START = 0x018;
|
||||
self.write8(VL6180X_REG_SYSRANGE_START, 0x01);
|
||||
|
|
@ -255,6 +378,8 @@ const LiDAR = struct {
|
|||
const VL6180X_REG_RESULT_INTERRUPT_STATUS_GPIO = 0x04f;
|
||||
while ((self.read8(VL6180X_REG_RESULT_INTERRUPT_STATUS_GPIO) & 0x04) == 0) {}
|
||||
|
||||
pins.led_4.put(0);
|
||||
|
||||
// read range in mm
|
||||
const VL6180X_REG_RESULT_RANGE_VAL = 0x062;
|
||||
const range: u8 = self.read8(VL6180X_REG_RESULT_RANGE_VAL);
|
||||
|
|
@ -263,6 +388,8 @@ const LiDAR = struct {
|
|||
const VL6180X_REG_SYSTEM_INTERRUPT_CLEAR = 0x015;
|
||||
self.write8(VL6180X_REG_SYSTEM_INTERRUPT_CLEAR, 0x07);
|
||||
|
||||
pins.led_5.put(0);
|
||||
|
||||
// Check the readRangeStatus
|
||||
const status: VL6180X_Status = @enumFromInt(self.read8(VL6180X_REG_RESULT_RANGE_STATUS) >> 4);
|
||||
|
||||
|
|
@ -273,42 +400,51 @@ const LiDAR = struct {
|
|||
self.status = .OutOfRange;
|
||||
}else {
|
||||
self.status = .Valid;
|
||||
self.range = range;
|
||||
}
|
||||
}
|
||||
|
||||
// Underlying I2C communication functions
|
||||
pub fn write8 (self: *LiDAR, write_index: u16, value: u8) void {
|
||||
fn write8 (self: LiDAR, write_index: u16, value: u8) void {
|
||||
// Doesn't work, I'm not smart
|
||||
// var buf: [3]u8 = undefined;
|
||||
// @memset(buf[0..2], std.mem.asBytes(&std.mem.nativeToBig(u16, write_index)));
|
||||
// buf[2] = value;
|
||||
const buf: [3]u8 = [_]u8{@truncate(write_index >> 8), @truncate(write_index & 0xFF), value};
|
||||
const buf: [3]u8 = [_]u8{@truncate(write_index >> 8), @truncate(write_index & 0xFF), value};
|
||||
|
||||
_ = i2c0.write_blocking(self.i2c_address, &buf) catch @panic("Failed to write");
|
||||
_ = i2c0.write_blocking(self.i2c_address, &buf) catch |err| switch (err) {
|
||||
error.DeviceNotPresent => pin_error(0b000101),
|
||||
error.Unexpected => pin_error(0b000110),
|
||||
error.NoAcknowledge => pin_error(0b000111),
|
||||
// error.Timeout => pin_error(0b001000),
|
||||
};
|
||||
}
|
||||
|
||||
// read_index is the register address internal to the sensor
|
||||
// We write what address/index we want to read, and then it writes back the value at that location
|
||||
pub fn read8 (self: *LiDAR, read_index: u16) u8 {
|
||||
pub fn read8 (self: LiDAR, read_index: u16) u8 {
|
||||
// Big endian. We just shifted/masked so it's fine to truncate.
|
||||
// const buf: [2]u8 = [_]u8{@truncate(read_index >> 8), @truncate(read_index & 0xFF)};
|
||||
const buf: [2]u8 = [_]u8{@truncate(read_index >> 8), @truncate(read_index & 0xFF)};
|
||||
// const buf: [2]u8 = ;
|
||||
// byteSwap to convert the native (ARM) little-endian to the big-endian expected by the LiDAR
|
||||
_ = i2c0.write_blocking(self.i2c_address, std.mem.asBytes(&std.mem.nativeToBig(u16, read_index))) catch @panic("Failed to write pre-read");
|
||||
|
||||
// _ = i2c0.write_blocking(self.i2c_address, std.mem.asBytes(&std.mem.nativeToBig(u16, read_index))) catch pin_error(0b000010);
|
||||
_ = i2c0.write_blocking(self.i2c_address, &buf) catch pin_error(0b000010);
|
||||
|
||||
// From the microzig example
|
||||
var rx_data: [1]u8 = undefined;
|
||||
_ = i2c0.read_blocking(self.i2c_address, &rx_data) catch panic("I2C reply invalid/missing", .{});
|
||||
_ = i2c0.read_blocking(self.i2c_address, &rx_data) catch pin_error(0b000011);
|
||||
return rx_data[0];
|
||||
}
|
||||
|
||||
// TODO: is Zig camel or snake case?
|
||||
// Assumes the pins are set so that this is on
|
||||
pub fn setAddress(self: *LiDAR, newAddress: i2c.Address) void {
|
||||
pub fn setAddress(self: *LiDAR, new_address: i2c.Address) void {
|
||||
// Write to the old address
|
||||
const VL6180X_REG_SLAVE_DEVICE_ADDRESS = 0x212;
|
||||
self.write8(VL6180X_REG_SLAVE_DEVICE_ADDRESS, @intFromEnum(newAddress) & 0x7F);
|
||||
self.i2c_address = newAddress;
|
||||
// TODO: the & 0x7F here is copy-pasted from Adafruit, I think to coerce into 7 bits, but in Zig we shouldn't need it
|
||||
self.write8(VL6180X_REG_SLAVE_DEVICE_ADDRESS, @intFromEnum(new_address));
|
||||
self.i2c_address = new_address;
|
||||
}
|
||||
};
|
||||
|
||||
|
|
@ -317,7 +453,8 @@ var lidar_front_right: LiDAR = undefined;
|
|||
var lidar_back_right: LiDAR = undefined;
|
||||
var lidar_front_left: LiDAR = undefined;
|
||||
var lidar_back_left: LiDAR = undefined;
|
||||
const lidars = [_]*LiDAR{&lidar_front, &lidar_front_right, &lidar_back_right, &lidar_front_left, &lidar_back_left};
|
||||
// const lidars = [_]*LiDAR{&lidar_front, &lidar_front_right, &lidar_back_right, &lidar_front_left, &lidar_back_left};
|
||||
const lidars = [_]*LiDAR{&lidar_front};
|
||||
|
||||
fn updateLidars () void {
|
||||
for (lidars) |lidar| {
|
||||
|
|
@ -339,12 +476,23 @@ pub fn setup () void {
|
|||
// pins.lidar_front_ce,
|
||||
// };
|
||||
|
||||
// Turn off all the LEDs, which are on by default since they're active low
|
||||
pins.led_1.put(1);
|
||||
pins.led_2.put(1);
|
||||
pins.led_3.put(1);
|
||||
pins.led_4.put(1);
|
||||
pins.led_5.put(1);
|
||||
pins.led_6.put(1);
|
||||
|
||||
_ = i2c0.apply(.{
|
||||
.clock_config = rp2040.clock_config, // Zach says the default here is fine
|
||||
.scl_pin = gpio.num(24),
|
||||
.sda_pin = gpio.num(25),
|
||||
});
|
||||
|
||||
// Turn on LED 2
|
||||
pins.led_2.put(0);
|
||||
|
||||
// LiDAR setup
|
||||
for (lidars) |lidar| {
|
||||
lidar.* = LiDAR {
|
||||
|
|
@ -354,7 +502,7 @@ pub fn setup () void {
|
|||
};
|
||||
}
|
||||
|
||||
var nextLiDARAddress: i2c.Address = @enumFromInt(0x2B); // The default is 0x2A so if we start at 2B and go up we should be fine
|
||||
var nextLiDARAddress: i2c.Address = @enumFromInt(0x2A); // The default is 0x29 so if we start at 2A and go up we should be fine
|
||||
|
||||
// Disable all of them
|
||||
pins.lidar_front_ce.put(0);
|
||||
|
|
@ -363,30 +511,50 @@ pub fn setup () void {
|
|||
pins.lidar_back_right_ce.put(0);
|
||||
pins.lidar_back_left_ce.put(0);
|
||||
|
||||
// TODO: Instead of writing 1 to the CE pins, we should set them as inputs and change them to pull-high
|
||||
|
||||
time.sleep_ms(1000);
|
||||
|
||||
// Turn on LED 3
|
||||
pins.led_3.put(0);
|
||||
|
||||
// front
|
||||
pins.lidar_front_ce.put(1);
|
||||
time.sleep_ms(1); // From the datasheet, 1ms to boot
|
||||
lidar_front.setup();
|
||||
// Zach has sanity-checked that these CE are all off except for the front CE one
|
||||
// So this is failing.
|
||||
lidar_front.setAddress(nextLiDARAddress);
|
||||
nextLiDARAddress = @enumFromInt(@intFromEnum(nextLiDARAddress) + 1);
|
||||
|
||||
// front left
|
||||
pins.lidar_front_left_ce.put(1);
|
||||
lidar_front_left.setAddress(nextLiDARAddress);
|
||||
nextLiDARAddress = @enumFromInt(@intFromEnum(nextLiDARAddress) + 1);
|
||||
if (lidar_front.read8(0x00) == 0xB4) {
|
||||
pins.led_4.put(0);
|
||||
}else {
|
||||
pin_error(0b011111);
|
||||
}
|
||||
|
||||
// front right
|
||||
pins.lidar_front_right_ce.put(1);
|
||||
lidar_front_right.setAddress(nextLiDARAddress);
|
||||
nextLiDARAddress = @enumFromInt(@intFromEnum(nextLiDARAddress) + 1);
|
||||
// // Turn on LED 4
|
||||
|
||||
// back left
|
||||
pins.lidar_back_left_ce.put(1);
|
||||
lidar_back_left.setAddress(nextLiDARAddress);
|
||||
nextLiDARAddress = @enumFromInt(@intFromEnum(nextLiDARAddress) + 1);
|
||||
// These 4 are not yet connected
|
||||
// // front left
|
||||
// pins.lidar_front_left_ce.put(1);
|
||||
// lidar_front_left.setAddress(nextLiDARAddress);
|
||||
// nextLiDARAddress = @enumFromInt(@intFromEnum(nextLiDARAddress) + 1);
|
||||
|
||||
// back right
|
||||
pins.lidar_back_right_ce.put(1);
|
||||
lidar_back_right.setAddress(nextLiDARAddress);
|
||||
nextLiDARAddress = @enumFromInt(@intFromEnum(nextLiDARAddress) + 1);
|
||||
// // front right
|
||||
// pins.lidar_front_right_ce.put(1);
|
||||
// lidar_front_right.setAddress(nextLiDARAddress);
|
||||
// nextLiDARAddress = @enumFromInt(@intFromEnum(nextLiDARAddress) + 1);
|
||||
|
||||
// // back left
|
||||
// pins.lidar_back_left_ce.put(1);
|
||||
// lidar_back_left.setAddress(nextLiDARAddress);
|
||||
// nextLiDARAddress = @enumFromInt(@intFromEnum(nextLiDARAddress) + 1);
|
||||
|
||||
// // back right
|
||||
// pins.lidar_back_right_ce.put(1);
|
||||
// lidar_back_right.setAddress(nextLiDARAddress);
|
||||
// nextLiDARAddress = @enumFromInt(@intFromEnum(nextLiDARAddress) + 1);
|
||||
|
||||
// for (lidar_ce_pins) |pin| {
|
||||
// pin.set_direction(.out);
|
||||
|
|
@ -452,7 +620,6 @@ pub fn turn180() void {
|
|||
}
|
||||
|
||||
pub fn stall() noreturn {
|
||||
|
||||
pins.motor_right_1.put(1);
|
||||
pins.motor_right_2.put(0);
|
||||
pins.motor_left_1.put(1);
|
||||
|
|
@ -511,9 +678,14 @@ pub fn recordWall(p: Point, direction: Cardinal) void {
|
|||
pub fn testUpdate () void {
|
||||
updateLidars();
|
||||
|
||||
// LEDs are on by default
|
||||
pins.led_6.toggle();
|
||||
|
||||
pins.led_1.put(if (lidar_front.range > 50) 1 else 0);
|
||||
pins.led_2.put(if (lidar_front_right.range > 50) 1 else 0);
|
||||
pins.led_3.put(if (lidar_front_left.range > 50) 1 else 0);
|
||||
pins.led_4.put(if (lidar_back_left.range > 50) 1 else 0);
|
||||
pins.led_5.put(if (lidar_back_right.range > 50) 1 else 0);
|
||||
// pins.led_2.put(if (lidar_front_right.range > 50) 1 else 0);
|
||||
// pins.led_3.put(if (lidar_front_left.range > 50) 1 else 0);
|
||||
// pins.led_4.put(if (lidar_back_left.range > 50) 1 else 0);
|
||||
// pins.led_5.put(if (lidar_back_right.range > 50) 1 else 0);
|
||||
|
||||
time.sleep_ms(100);
|
||||
}
|
||||
|
|
|
|||
Loading…
Reference in New Issue