From d12da1a678b7f6f2d4c29ec639f8c9c3f751c638 Mon Sep 17 00:00:00 2001 From: Matthias Portzel Date: Tue, 23 Apr 2024 22:06:16 -0400 Subject: [PATCH] LiDAR test program working --- README.md | 6 ++ src/rp2040-bot.zig | 238 ++++++++++++++++++++++++++++++++++++++------- 2 files changed, 211 insertions(+), 33 deletions(-) diff --git a/README.md b/README.md index 64c1d66..92ac488 100644 --- a/README.md +++ b/README.md @@ -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? diff --git a/src/rp2040-bot.zig b/src/rp2040-bot.zig index f70ef16..3663efc 100644 --- a/src/rp2040-bot.zig +++ b/src/rp2040-bot.zig @@ -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); }