Untested but possibly working LiDAR code
This commit is contained in:
parent
675439c63a
commit
a4a10ec3a1
17
src/main.zig
17
src/main.zig
|
|
@ -1,19 +1,12 @@
|
|||
const std = @import("std");
|
||||
|
||||
const algo = @import("algorithm.zig");
|
||||
const robot = algo.robot;
|
||||
const robot = @import("rp2040-bot.zig");
|
||||
const map = @import("map.zig");
|
||||
|
||||
pub fn main() !noreturn {
|
||||
// Setup
|
||||
algo.setup();
|
||||
robot.setup();
|
||||
|
||||
// Flood fill to the goal
|
||||
algo.floodFill(map.goals[0..]);
|
||||
|
||||
// Flood fill back to the start
|
||||
const starts = [_]map.Point{ map.start };
|
||||
algo.floodFill(starts[0..]);
|
||||
|
||||
robot.stall();
|
||||
while (true) {
|
||||
robot.testUpdate();
|
||||
}
|
||||
}
|
||||
|
|
|
|||
|
|
@ -2,6 +2,8 @@
|
|||
// This file is only for defining a hardware abstraction layer
|
||||
// The code in this file will be dirty, it will deal with setting up encoders and all sorts of BS
|
||||
|
||||
const std = @import("std");
|
||||
const panic = @import("std").debug.panic;
|
||||
const map = @import("map.zig");
|
||||
const Point = map.Point;
|
||||
const Cardinal = map.Cardinal;
|
||||
|
|
@ -13,6 +15,8 @@ const time = rp2040.time;
|
|||
const i2c = rp2040.i2c;
|
||||
const gpio = rp2040.gpio;
|
||||
|
||||
// --- Setup ---
|
||||
|
||||
const pin_config = rp2040.pins.GlobalConfiguration {
|
||||
// 6 colored LEDs
|
||||
.GPIO0 = .{
|
||||
|
|
@ -101,7 +105,7 @@ const pin_config = rp2040.pins.GlobalConfiguration {
|
|||
},
|
||||
|
||||
// Motor Encoders
|
||||
// Zach recommends triggering a double-edged interrupter on 1 and clocking on 2
|
||||
// Zach recommends triggering a double-edged interrupt on 1 and clocking on 2
|
||||
// You read the second to know if you're going forward or backwards
|
||||
.GPIO14 = .{
|
||||
// MAE2
|
||||
|
|
@ -198,32 +202,136 @@ const pin_config = rp2040.pins.GlobalConfiguration {
|
|||
.direction = .in,
|
||||
},
|
||||
};
|
||||
// comptime {
|
||||
// @compileLog(@typeInfo(@TypeOf(pin_config.apply)));
|
||||
// }
|
||||
// var pins: @typeInfo(@TypeOf(pin_config.apply)).Fn.return_type = undefined;
|
||||
|
||||
const LEFT_MOTOR = 1;
|
||||
const RIGHT_MOTOR = 2;
|
||||
|
||||
// 254mm square size
|
||||
const SQUARE_SIZE = 254;
|
||||
|
||||
var pins: rp2040.pins.Pins(pin_config) = undefined;
|
||||
|
||||
pub fn toggle_led_6 () void {
|
||||
// TODO: this does bad things if pins hasn't been defined by the first core yet
|
||||
pins.led_5.toggle();
|
||||
// Our main I2C, used for communicating with the LiDARs, is I2C 0
|
||||
const i2c0 = i2c.num(0);
|
||||
|
||||
const VL6180X_Status = enum (usize) {
|
||||
// Codes and descriptions from the VL6180X datasheet
|
||||
ValidMeasurement = 0, // Valid measurement
|
||||
SystemErrorOne = 1, // System error detected (can only happen on
|
||||
SystemErrorTwo = 2, // power on). No measurement possible
|
||||
SystemErrorThree = 3,
|
||||
SystemErrorFour = 4,
|
||||
SystemErrorFive = 5,
|
||||
EarlyConvergenceEstimateFailed = 6, // ECE check failed
|
||||
MaxConvergenceExceeded = 7, // System did not converge before the specified max. convergence time limit
|
||||
RangeIgnore = 8, // Ignore threshold check failed
|
||||
// 9 and 10 are "Not used"
|
||||
SignalToNoiseError = 11, // Ambient conditions too high. Measurement not valid
|
||||
RangeUnderflowTwelve = 12, // Range value < 0
|
||||
RangeOverflowThirteen = 13, // Range value out of range (A target is detected by the device, but at a distance that results in internal variable overflow)
|
||||
RangeUnderflowFourteen = 14, // Range value < 0
|
||||
RangeOverflowFifteen = 15, // Range value out of range
|
||||
// 16 and 18 are emitted by the ST's API, not the hardware, if I understand the datasheet correctly
|
||||
// .RangingFiltered = 16, // Distance filtered by Wrap Around Filter. Occurs when a high reflectance target is detected between 600mm to 1.2m
|
||||
// 17 is "Not used"
|
||||
// .DataNotReady = 18, // Returned from RangeGetMeasurementIfReady if not ready
|
||||
};
|
||||
|
||||
const LIDAR_RANGE_MAX = 110;
|
||||
|
||||
const LiDAR = struct {
|
||||
range: u8,
|
||||
status: enum (usize) { Valid, OutOfRange, ReadError },
|
||||
|
||||
i2c_address: i2c.Address, // = @enumFromInt(0x29); // The default I2C address is 0x29 (datasheet)
|
||||
|
||||
pub fn updateReading (self: *LiDAR) void {
|
||||
// Adapted from:
|
||||
// => https://github.com/adafruit/Adafruit_VL6180X/blob/a42df76/Adafruit_VL6180X.cpp#L187
|
||||
|
||||
// wait for device to be ready for range measurement
|
||||
const VL6180X_REG_RESULT_RANGE_STATUS = 0x04d;
|
||||
while ((self.read8(VL6180X_REG_RESULT_RANGE_STATUS) & 0x01) == 0) {}
|
||||
|
||||
// Start a range measurement
|
||||
const VL6180X_REG_SYSRANGE_START = 0x018;
|
||||
self.write8(VL6180X_REG_SYSRANGE_START, 0x01);
|
||||
|
||||
// Poll until bit 2 is set
|
||||
const VL6180X_REG_RESULT_INTERRUPT_STATUS_GPIO = 0x04f;
|
||||
while ((self.read8(VL6180X_REG_RESULT_INTERRUPT_STATUS_GPIO) & 0x04) == 0) {}
|
||||
|
||||
// read range in mm
|
||||
const VL6180X_REG_RESULT_RANGE_VAL = 0x062;
|
||||
const range: u8 = self.read8(VL6180X_REG_RESULT_RANGE_VAL);
|
||||
|
||||
// clear interrupt
|
||||
const VL6180X_REG_SYSTEM_INTERRUPT_CLEAR = 0x015;
|
||||
self.write8(VL6180X_REG_SYSTEM_INTERRUPT_CLEAR, 0x07);
|
||||
|
||||
// Check the readRangeStatus
|
||||
const status: VL6180X_Status = @enumFromInt(self.read8(VL6180X_REG_RESULT_RANGE_STATUS) >> 4);
|
||||
|
||||
if (status != .ValidMeasurement) {
|
||||
// Okay this is bad but the first is the status from the chip and the second is the status in us
|
||||
self.status = .ReadError;
|
||||
}else if (range > LIDAR_RANGE_MAX) {
|
||||
self.status = .OutOfRange;
|
||||
}else {
|
||||
self.status = .Valid;
|
||||
}
|
||||
}
|
||||
|
||||
// Underlying I2C communication functions
|
||||
pub 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};
|
||||
|
||||
_ = i2c0.write_blocking(self.i2c_address, &buf) catch @panic("Failed to write");
|
||||
}
|
||||
|
||||
// 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 {
|
||||
// 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 = ;
|
||||
// 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");
|
||||
|
||||
// From the microzig example
|
||||
var rx_data: [1]u8 = undefined;
|
||||
_ = i2c0.read_blocking(self.i2c_address, &rx_data) catch panic("I2C reply invalid/missing", .{});
|
||||
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 {
|
||||
// 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;
|
||||
}
|
||||
};
|
||||
|
||||
var lidar_front: LiDAR = undefined;
|
||||
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};
|
||||
|
||||
fn updateLidars () void {
|
||||
for (lidars) |lidar| {
|
||||
lidar.updateReading();
|
||||
}
|
||||
}
|
||||
|
||||
// Zach confirms i2c 0
|
||||
// const i2c0 = i2c.num(0);
|
||||
|
||||
// Pin stuff
|
||||
pub fn setup () void {
|
||||
pins = pin_config.apply();
|
||||
|
||||
// const lidar_ce_pins = [_]microzig.hal.gpio.Pin{
|
||||
// @compileLog(@typeInfo(microzig.hal.pins.GlobalConfiguration).Struct);
|
||||
// @compileLog(@typeInfo(@TypeOf(pins)));
|
||||
// Zig's a bad language ...
|
||||
// const lidar_ce_pins = [_]@TypeOf(pins.lidar_front_left_ce){
|
||||
// pins.lidar_front_left_ce,
|
||||
// pins.lidar_front_right_ce,
|
||||
// pins.lidar_back_right_ce,
|
||||
|
|
@ -231,17 +339,55 @@ pub fn setup () void {
|
|||
// pins.lidar_front_ce,
|
||||
// };
|
||||
|
||||
// _ = i2c0.apply(.{
|
||||
// .clock_config = rp2040.clock_config, // Zach says the default here is fine
|
||||
// .scl_pin = gpio.num(24),
|
||||
// .sda_pin = gpio.num(25),
|
||||
// });
|
||||
_ = i2c0.apply(.{
|
||||
.clock_config = rp2040.clock_config, // Zach says the default here is fine
|
||||
.scl_pin = gpio.num(24),
|
||||
.sda_pin = gpio.num(25),
|
||||
});
|
||||
|
||||
// LiDAR setup
|
||||
for (lidars) |lidar| {
|
||||
lidar.* = LiDAR {
|
||||
.range = undefined,
|
||||
.status = undefined,
|
||||
.i2c_address = @enumFromInt(0x29), // The default I2C address is 0x29 (datasheet)
|
||||
};
|
||||
}
|
||||
|
||||
var nextLiDARAddress: i2c.Address = @enumFromInt(0x2B); // The default is 0x2A so if we start at 2B and go up we should be fine
|
||||
|
||||
// Disable all of them
|
||||
pins.lidar_front_ce.put(0);
|
||||
pins.lidar_front_left_ce.put(0);
|
||||
pins.lidar_front_right_ce.put(0);
|
||||
pins.lidar_back_right_ce.put(0);
|
||||
pins.lidar_back_left_ce.put(0);
|
||||
|
||||
// front
|
||||
pins.lidar_front_ce.put(1);
|
||||
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);
|
||||
|
||||
// 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);
|
||||
|
||||
// // We need to setup the LiDAR addresses
|
||||
// // TODO: Create a Lidar struct with methods
|
||||
// // The way we're doing this right now, we are enabling every LiDAR
|
||||
// const LIDAR_ADDR_BASE: i2c.Address = @enumFromInt(0x50);
|
||||
// // Disable all of them
|
||||
// for (lidar_ce_pins) |pin| {
|
||||
// pin.set_direction(.out);
|
||||
// pin.put(0);
|
||||
|
|
@ -257,31 +403,16 @@ pub fn setup () void {
|
|||
// current_address = @enumFromInt(@intFromEnum(current_address) + 1);
|
||||
// }
|
||||
|
||||
|
||||
// pins.onboard_led.toggle();
|
||||
// pins.onboard_led.toggle();
|
||||
// TODO
|
||||
|
||||
// As elsewhere, blocked on upstream
|
||||
// start_alt_core()
|
||||
|
||||
// TODO: Motor controller setup
|
||||
}
|
||||
|
||||
pub fn moveForward() void {
|
||||
// TODO
|
||||
}
|
||||
// --- IO access functions ---
|
||||
|
||||
pub fn turn90(direction: map.TurningDirection) void {
|
||||
// TODO
|
||||
_ = direction;
|
||||
}
|
||||
|
||||
pub fn turn180() void {
|
||||
// TODO
|
||||
}
|
||||
|
||||
// pub fn readFront () !bool
|
||||
// pub fn readLeft () !bool
|
||||
// pub fn readRight () !bool
|
||||
// pub fn readBack () ?? throw I guess
|
||||
// TODO:
|
||||
// These are used by the algorithm to check if we have walls in the respective directions
|
||||
pub fn readRight() bool {
|
||||
return true;
|
||||
}
|
||||
|
|
@ -302,6 +433,24 @@ pub fn start_alt_core() !void {
|
|||
// multicore.launch_core1(alt_core_main_function);
|
||||
}
|
||||
|
||||
// --- Map and movement functions ---
|
||||
|
||||
// 254mm square size
|
||||
const SQUARE_SIZE = 254;
|
||||
|
||||
pub fn moveForward() void {
|
||||
// TODO
|
||||
}
|
||||
|
||||
pub fn turn90(direction: map.TurningDirection) void {
|
||||
// TODO
|
||||
_ = direction;
|
||||
}
|
||||
|
||||
pub fn turn180() void {
|
||||
// TODO
|
||||
}
|
||||
|
||||
pub fn stall() noreturn {
|
||||
|
||||
pins.motor_right_1.put(1);
|
||||
|
|
@ -357,3 +506,14 @@ pub fn recordWall(p: Point, direction: Cardinal) void {
|
|||
_ = p;
|
||||
_ = direction;
|
||||
}
|
||||
|
||||
// --- Test functions ---
|
||||
pub fn testUpdate () void {
|
||||
updateLidars();
|
||||
|
||||
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);
|
||||
}
|
||||
|
|
|
|||
Loading…
Reference in New Issue