matthias
/
Micromouse
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

Compare commits

base: matthias:b7cd1d4dd88f12cb259c5604d2541d2e976d0697
Branches Tags
matthias:master
matthias:interrupt-on-0.12
matthias:rp2040-testing
...
compare: matthias:ea9f00344a0bd04984d72e26138f7c88af27978c
Branches Tags
matthias:interrupt-on-0.12
matthias:rp2040-testing
matthias:master

6 Commits
b7cd1d4dd8 ... ea9f00344a

Author SHA1 Message Date
Matthias Portzel ea9f00344a
Turn on other lidars, untested 2024-04-23 22:21:03 -04:00
Matthias Portzel d12da1a678
LiDAR test program working 2024-04-23 22:06:16 -04:00
Matthias Portzel a4a10ec3a1
Untested but possibly working LiDAR code 2024-04-20 23:12:49 -04:00
Matthias Portzel 675439c63a
Merge branch 'master' into rp2040-testing 2024-04-19 23:42:11 -04:00
Matthias Portzel 44da9e331c
Remove testing junk from main 2024-04-19 23:42:00 -04:00
Matthias Portzel d758fa5c33
Remove some things that aren't needed for rp2040 tests 2024-04-19 23:40:51 -04:00
8 changed files with 390 additions and 625 deletions
Show Stats Expand all files Collapse all files

6
README.md
View File

@ -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?

11
build.zig
View File

@ -17,14 +17,6 @@ pub fn build(b: *std.Build) void {
.source_file = .{ .path = "src/main.zig" },
});
// We also build a native executable for simulator testing
const exe = b.addExecutable(.{
.name = "micromouse",
.root_source_file = .{ .path = "src/main.zig" },
// Will be .target = b.host in 0.12
.target = b.standardTargetOptions(.{})
});
// `installFirmware()` is the MicroZig pendant to `Build.installArtifact()`
// and allows installing the firmware as a typical firmware file.
//
@ -33,7 +25,4 @@ pub fn build(b: *std.Build) void {
// For debugging, we also always install the firmware as an ELF file
microzig.installFirmware(b, firmware, .{ .format = .elf });
// Install the native executable into zig-out/bin/
b.installArtifact(exe);
}

281
src/algorithm.zig
View File

@ -1,281 +0,0 @@
const std = @import("std");
const assert = std.debug.assert;
const map = @import("map.zig");
const Point = map.Point;
const Cardinal = map.Cardinal;
const Mouse = @import("mouse.zig").Mouse;
// Doesn't have any state
// It's a hardware abstraction layer
pub const is_irl = @import("builtin").os.tag == .freestanding;
pub const robot = if (is_irl) @import("rp2040-bot.zig") else @import("simulated-bot.zig");
// It's a little weird to do this as a global, but it just returns
// an instance of Mouse with initial values
var mouse = @import("mouse.zig").initialize();
pub fn setup () void {
// Just pass through to robot.setup to do any robot-specific setup (hardware, allocations, and starting alt-core)
robot.setup();
// Any maze-related setup that can't be done at comptime can be done here
}
//// Node
const Node = struct {
p: Point,
score: usize,
// Return 0-4 Nodes, by checking if
// each of the four nodes around this node a) exists and b) is accessible (no wall)
// I maybe could've also returned a `[4:0]*Node` (sentinel terminated 4-long array of node pointers)
pub fn getAdjacentNodes(self: Node) std.BoundedArray(*Node, 4) {
var nodes = std.BoundedArray(*Node, 4).init(0) catch unreachable; // 0 is < 4
// I could clean this up with like a loop and a helper function
// North
// std.log.debug("North: y + 1 < height {}; not wall: {}", .{self.p.y + 1 < map.height, !isWall(self.p, .north)});
// std.log.debug("isWall(self.p '{}', .north): {}", .{self.p, isWall(self.p, .north)});
if (self.p.y + 1 < map.height and !isWall(self.p, .north)) {
// std.log.debug("Adding node", .{});
nodes.append(&mazeNodes[self.p.y + 1][self.p.x]) catch unreachable;
}
// South
if (self.p.y > 0 and !isWall(self.p, .south)) {
nodes.append(&mazeNodes[self.p.y - 1][self.p.x]) catch unreachable;
}
// East
if (self.p.x + 1 < map.width and !isWall(self.p, .east)) {
nodes.append(&mazeNodes[self.p.y][self.p.x + 1]) catch unreachable;
}
// West
if (self.p.x > 0 and !isWall(self.p, .west)) {
nodes.append(&mazeNodes[self.p.y][self.p.x - 1]) catch unreachable;
}
return nodes;
}
// Distance from true center (the post in the middle of the 2x2 square) rounded down
//
fn getCrowDistance(self: *const Node) usize {
return self.p.x + self.p.y;
}
pub fn updateScore(self: *Node, newScore: usize) void {
// newScore should be less than the current score
assert(newScore <= self.score);
self.score = newScore;
// Call updateScore on all adjacent nodes if they have a score which is worse than (newScore + 100)
// For some reason BoundedArrays aren't indexable, hence the .slice()
const adjacentNodes = self.getAdjacentNodes().slice();
for (adjacentNodes) |node| {
// 1 square away is a score of 100. This lets us play with tiebreaker metrics
if (self.score + 100 < node.score) {
node.updateScore(self.score + 100);
}
}
robot.writeCellScore(self.p, self.score);
}
};
//// Walls
// I tried to come up with a better data structure for this, and it just hurt my brain too much
// So I'm just going to go with the Matthias classic
// With a slight change for our coordinate system
// 11 x 11 list of "wall posts" with each wall post keeping track of the wall to its north and east
const WallPost = struct {
// Not using Cardinal or Point here to avoid confusing with the maze tile directions
// These are nullable to represent when we haven't seen them yet
up: ?bool,
right: ?bool,
};
// Might be a better way to do this initialization
var walls = init: {
var initialWalls: [map.height + 1][map.width + 1]WallPost = undefined;
var row: [map.width + 1]WallPost = undefined;
@memset(&row, WallPost { .up = null, .right = null });
@memset(&initialWalls, row);
break :init initialWalls;
};
fn isEdge(p: Point, direction: Cardinal) bool {
// std.log.debug("is Edge called with point: {} direction: {}\n", .{p, direction});
return (
(p.x == 0 and direction == .west)
or (p.y == 0 and direction == .south)
or (p.x == map.width - 1 and direction == .east)
or (p.y == map.height - 1 and direction == .north)
);
}
// Used by both isWall and setWall
fn getWall(p: Point, direction: Cardinal) *?bool {
// Should be handled by the caller
assert(!isEdge(p, direction));
// Direction is north, west, or south we look left.
const wallX = if (direction != .east) p.x else p.x + 1;
// We look down, unless it's north
const wallY = if (direction != .north) p.y else p.y + 1;
const wallPost = &walls[wallY][wallX];
return &(if (direction == .west or direction == .east) wallPost.up else wallPost.right);
}
fn isWall(p: Point, direction: Cardinal) bool {
// Boundaries are definitely there
if (isEdge(p, direction)) {
// std.log.debug("forcing true return from isWall for point {} and direction {}", .{p, direction});
return true;
}
const wall = getWall(p, direction);
// Now, because floodfill is optimistic, I'm going to default to assuming there's not a wall if we haven't seen one
return wall.* orelse false;
}
pub fn setWall(p: Point, direction: Cardinal, value: bool) void {
// If that direction is an outer wall, assert that we saw a wall
if (isEdge(p, direction)) {
// We should maybe have a better way of handling this if it actually happens to the robot
assert(value);
}
// Display that we saw a wall (e.g. on the simulator display)
if (value) {
robot.recordWall(p, direction);
}
// We don't keep track of the edges, we just assume they're there
if (isEdge(p, direction)) {
return;
}
const wall = getWall(p, direction);
// wall.* is a ?bool and value is a bool, but we can do type coercion
wall.* = value;
}
//// Maze
var mazeNodes = init: {
var nodes: [map.width][map.height]Node = undefined;
for (&nodes, 0..) |*row, y| {
for (row, 0..) |*n, x| {
n.* = Node {
.p = Point{ .x = x, .y = y },
.score = std.math.maxInt(@TypeOf(n.score))
};
}
}
break :init nodes;
};
// test {
// // y, x; and we start in the bottom left
// const s = mazeNodes[0][0];
// // @compileLog(s.p.x);
// // @compileLog(s.p.y);
// // @compileLog(map.start.x);
// // @compileLog(map.start.y);
// assert(s.p.x == map.start.x);
// assert(s.p.y == map.start.y);
// }
//// Algorithm
fn recalcScores(dest: []const Point) void {
// Reset every node to a high score
for (&mazeNodes) |*row| {
for (row) |*n| {
n.score = std.math.maxInt(@TypeOf(n.score));
}
}
// Create destination nodes for every destination point, with a score of 0
// Starting at each of the destination(s) and working out, give each node an increasing score
// updateScore handles updating the scores for all other nodes in the maze
for (dest) |p| {
const goalNode = &mazeNodes[p.y][p.x];
goalNode.updateScore(0);
}
}
// Just a helper function
fn hasPoint(l: []const Point, needle: Point) bool {
for (l) |p| {
if (needle.eq(p)) {
return true;
}
}
return false;
}
// Takes a list of destination points, and a mouse
// Blocks until the mouse is in one of the destination points
pub fn floodFill(dest: []const Point) void {
if (dest.len == 4) {
assert(hasPoint(dest, map.goals[0]));
assert(hasPoint(dest, map.goals[3]));
}
mouse.updateWalls();
// Calculate optimistic distances from the center
recalcScores(dest);
for (dest) |p| {
const goalNode = &mazeNodes[p.y][p.x];
std.log.debug("goal nodes: {}\n", .{ goalNode });
}
// While we aren't at dest,
while (!hasPoint(dest[0..], mouse.position)) {
const currentSquare = &mazeNodes[mouse.position.y][mouse.position.x];
// 1. get the square adjacent to the mouse which has the lowest score
var minAdjacent: ?*Node = null;
std.log.debug("currentSquare {} score: {}", .{ currentSquare.p, currentSquare.score });
const adjacentNodes = currentSquare.getAdjacentNodes().slice();
std.log.debug("adjacentNodes {}", .{adjacentNodes.len});
for (adjacentNodes) |n| {
std.log.debug("Node at ({}, {}) has score {}", .{ n.p.x, n.p.y, n.score });
assert(@TypeOf(n) == *Node);
// Replace minAdjacent if this new node is shorter
//
if (minAdjacent == null or n.score < minAdjacent.?.score) {
minAdjacent = n;
}
}
assert(minAdjacent != null);
// 2. if that square has a value the same or higher than the current score, then we re-do score calculation and go to 1
if (minAdjacent.?.score >= currentSquare.score) {
recalcScores(dest);
continue;
}
// 3. otherwise, move the mouse there
mouse.moveAdjacent(minAdjacent.?.p);
// 4. update the sensors on the mouse
mouse.updateWalls();
}
assert(hasPoint(dest, mouse.position));
if (dest.len == 4) {
// This isn't always true because sometimes dest isn't the goal
assert(hasPoint(map.goals[0..], mouse.position));
}
}

40
src/main.zig
View File

@ -1,44 +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");
const rp2040 = @import("microzig").hal;
// const multicore = rp2040.multicore;
pub fn main() !noreturn {
robot.setup();
// TODO: move into algo
pub fn alt_core () noreturn {
// while true
// Flash pin 6 for now
while (true) {
if (is_robot) {
robot.toggle_led_6();
}
robot.testUpdate();
}
}
const is_robot = algo.is_robot;
const time = @import("microzig").hal.time;
// Entry point for second core
fn core1 () noreturn {
time.sleep_ms(1000);
alt_core();
}
pub fn main() !noreturn {
// Power On test
algo.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();
}

138
src/mouse.zig
View File

@ -1,138 +0,0 @@
const assert = @import("std").debug.assert;
const abs = @import("std").math.absCast; // builtin in 0.12 please
const intDist = @import("util.zig").intDist;
const map = @import("map.zig");
const Cardinal = map.Cardinal;
const Point = map.Point;
const TurningDirection = map.TurningDirection;
const algo = @import("algorithm.zig");
const robot = algo.robot;
const setWall = algo.setWall;
pub const Mouse = struct {
position: map.Point,
facing: Cardinal,
pub fn spinTo(self: *Mouse, direction: Cardinal) void {
if (self.facing == direction) {
// return, we're done!
}else if (
(self.facing == .north and direction == .east) or
(self.facing == .east and direction == .south) or
(self.facing == .south and direction == .west) or
(self.facing == .west and direction == .north)
) {
//Rotate 90º Right
robot.turn90(.right);
}else if (
(self.facing == .north and direction == .west) or
(self.facing == .west and direction == .south) or
(self.facing == .south and direction == .east) or
(self.facing == .east and direction == .north)
) {
//Rotate 90º Left
robot.turn90(.left);
}else if (
(self.facing == .north and direction == .south) or
(self.facing == .south and direction == .north) or
(self.facing == .east and direction == .west) or
(self.facing == .west and direction == .east)
) {
// Rotate 180º
robot.turn180();
}
self.facing = direction;
}
pub fn moveAdjacent (self: *Mouse, p: Point) void {
// |self.position.x - p.x| + |self.position.y - p.y| == 1
assert(intDist(self.position.x, p.x) + intDist(self.position.y, p.y) == 1);
// The direction we need to move to get to this point
var direction: Cardinal = undefined;
// - 1 has a chance to underflow if it's 0.
if (p.x + 1 == self.position.x) {
direction = .west;
}else if (p.y + 1 == self.position.y) {
direction = .south;
}else if (p.x > 0 and p.x - 1 == self.position.x) {
direction = .east;
}else if (p.y > 0 and p.y - 1 == self.position.y) {
direction = .north;
}
self.spinTo(direction);
// I haven't decided if this is going to be implicit 1 square or what
robot.moveForward();
// Update our position
self.position = p;
}
// These functions return null if that direction is the back (where we don't have a sensor)
// There might be a better way to do this, but eh
fn readNorth(self: Mouse) ?bool {
switch (self.facing) {
.north => return robot.readFront(),
.east => return robot.readLeft(),
.south => return null,
.west => return robot.readRight()
}
}
fn readEast(self: Mouse) ?bool {
switch (self.facing) {
.north => return robot.readRight(),
.east => return robot.readFront(),
.south => return robot.readLeft(),
.west => return null
}
}
fn readSouth(self: Mouse) ?bool {
switch (self.facing) {
.north => return null,
.east => return robot.readRight(),
.south => return robot.readFront(),
.west => return robot.readLeft()
}
}
fn readWest(self: Mouse) ?bool {
switch (self.facing) {
.north => return robot.readLeft(),
.east => return null,
.south => return robot.readRight(),
.west => return robot.readFront()
}
}
pub fn updateWalls(self: Mouse) void {
// TODO: Write to the simulator for cosmetics
// I could probably come up with a read function that takes a direction and yada yada
if (self.readNorth()) |isWall| {
setWall(self.position, .north, isWall);
}
if (self.readEast()) |isWall| {
setWall(self.position, .east, isWall);
}
if (self.readSouth()) |isWall| {
setWall(self.position, .south, isWall);
}
if (self.readWest()) |isWall| {
setWall(self.position, .west, isWall);
}
}
};
pub fn initialize() Mouse {
return Mouse {
.position = map.start,
.facing = .north
};
}

430
src/rp2040-bot.zig
View File

@ -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,8 +15,11 @@ const time = rp2040.time;
const i2c = rp2040.i2c;
const gpio = rp2040.gpio;
// --- Setup ---
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,
@ -101,7 +106,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 +203,272 @@ 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();
// 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);
}
}
// Zach confirms i2c 0
// const i2c0 = i2c.num(0);
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);
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 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
// 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) {}
pins.led_3.put(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) {}
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);
// clear interrupt
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);
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;
self.range = range;
}
}
// Underlying I2C communication functions
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 |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 {
// 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 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 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, new_address: i2c.Address) void {
// Write to the old address
const VL6180X_REG_SLAVE_DEVICE_ADDRESS = 0x212;
// 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;
}
};
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};
// const lidars = [_]*LiDAR{&lidar_front};
fn updateLidars () void {
for (lidars) |lidar| {
lidar.updateReading();
}
}
// 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 +476,84 @@ 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),
// });
// 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 {
.range = undefined,
.status = undefined,
.i2c_address = @enumFromInt(0x29), // The default I2C address is 0x29 (datasheet)
};
}
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);
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);
// 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);
// // Turn on LED 4
// These 4 are not yet connected
// front left
pins.lidar_front_left_ce.put(1);
time.sleep_ms(1);
lidar_front_left.setAddress(nextLiDARAddress);
nextLiDARAddress = @enumFromInt(@intFromEnum(nextLiDARAddress) + 1);
// front right
pins.lidar_front_right_ce.put(1);
time.sleep_ms(1);
lidar_front_right.setAddress(nextLiDARAddress);
nextLiDARAddress = @enumFromInt(@intFromEnum(nextLiDARAddress) + 1);
// back left
pins.lidar_back_left_ce.put(1);
time.sleep_ms(1);
lidar_back_left.setAddress(nextLiDARAddress);
nextLiDARAddress = @enumFromInt(@intFromEnum(nextLiDARAddress) + 1);
// back right
pins.lidar_back_right_ce.put(1);
time.sleep_ms(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 +569,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,8 +599,25 @@ pub fn start_alt_core() !void {
// multicore.launch_core1(alt_core_main_function);
}
pub fn stall() noreturn {
// --- 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);
pins.motor_right_2.put(0);
pins.motor_left_1.put(1);
@ -357,3 +671,19 @@ pub fn recordWall(p: Point, direction: Cardinal) void {
_ = p;
_ = direction;
}
// --- Test functions ---
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);
time.sleep_ms(100);
}

103
src/simulated-bot.zig
View File

@ -1,103 +0,0 @@
const std = @import("std");
const assert = std.debug.assert;
const map = @import("map.zig");
const Point = map.Point;
const Cardinal = map.Cardinal;
const stdin = std.io.getStdIn().reader();
const stdout = std.io.getStdOut().writer();
// Sim docs
// https://github.com/mackorone/mms?tab=readme-ov-file#example
// Since this only runs on my computer, we can do whatever we want here
pub fn setup () void {
// TODO: Create alt thread and start multi-core
// // Blocked on upstream multi-core support
// robot.start_alt_core();
}
// TODO: create a utility function that takes an allocator and reads from std in and returns a &[]u8
fn waitForAck () void {
var input = std.BoundedArray(u8, 128).init(0) catch unreachable;
_ = stdin.streamUntilDelimiter(input.writer(), '\n', input.capacity()) catch unreachable;
assert(std.mem.eql(u8, input.slice(), "ack"));
}
pub fn moveForward() void {
// print "moveForward" to std out
_ = stdout.write("moveForward\n") catch unreachable;
// wait for "ack" on std in
waitForAck();
}
pub fn turn90(direction: map.TurningDirection) void {
_ = stdout.write(switch (direction) {
.right => "turnRight\n",
.left => "turnLeft\n"
}) catch unreachable;
// wait for "ack" on std in
waitForAck();
}
pub fn turn180() void {
turn90(.left);
turn90(.left);
}
pub fn stall() noreturn {
std.debug.print("Program has completed...", .{});
while (true) {
std.time.sleep(500_000000);
std.debug.print(".", .{});
}
}
// TODO: Style all of these
// TODO: figure out what the above comment means. They're pretty stylish already
pub fn readRight() bool {
_ = stdout.write("wallRight\n") catch unreachable;
var input = std.BoundedArray(u8, 128).init(0) catch unreachable;
_ = stdin.streamUntilDelimiter(input.writer(), '\n', input.capacity()) catch unreachable;
return std.mem.eql(u8, input.slice(), "true");
}
pub fn readLeft() bool {
_ = stdout.write("wallLeft\n") catch unreachable;
var input = std.BoundedArray(u8, 128).init(0) catch unreachable;
_ = stdin.streamUntilDelimiter(input.writer(), '\n', input.capacity()) catch unreachable;
return std.mem.eql(u8, input.slice(), "true");
}
pub fn readFront() bool {
_ = stdout.write("wallFront\n") catch unreachable;
var input = std.BoundedArray(u8, 128).init(0) catch unreachable;
_ = stdin.streamUntilDelimiter(input.writer(), '\n', input.capacity()) catch unreachable;
return std.mem.eql(u8, input.slice(), "true");
}
pub fn readBack() bool {
// What?
std.debug.panic();
}
pub fn recordWall(p: Point, direction: Cardinal) void {
const dir :u8 = switch (direction) {
.north => 'n',
.south => 's',
.east => 'e',
.west => 'w',
};
stdout.print("setWall {d} {d} {c}\n", .{ p.x, p.y, dir }) catch unreachable;
}
pub fn writeCellScore(p: Point, score: usize) void {
stdout.print("setText {d} {d} {d}\n", .{p.x, p.y, score}) catch unreachable;
}

6
src/util.zig
View File

@ -1,6 +0,0 @@
// Returns the distance between two integers
// Ideally, would take any integer type and
// return the same type but unsigned, since this is always positive or 0
pub fn intDist (int1: usize, int2: usize) usize {
return @max(int1, int2) - @min(int1, int2);
}