Skip to content

Commit

Permalink
chore: bump rp-hal to v0.9 and run cargo update (#120)
Browse files Browse the repository at this point in the history
  • Loading branch information
antbern authored Aug 22, 2024
1 parent 33db869 commit 5397c28
Show file tree
Hide file tree
Showing 15 changed files with 284 additions and 291 deletions.
394 changes: 199 additions & 195 deletions slamrs-robot-rtic/firmware/Cargo.lock

Large diffs are not rendered by default.

23 changes: 10 additions & 13 deletions slamrs-robot-rtic/firmware/Cargo.toml
Original file line number Diff line number Diff line change
Expand Up @@ -11,30 +11,27 @@ defmt = "0.3"
defmt-rtt = "0.4"
panic-probe = { version = "0.3", features = ["print-defmt"] }

# embedded-hal = "1.0.0"
embedded-hal = "0.2.7"
embedded-hal = "1.0.0"
# embedded-hal-async = "1.0.0"
# embedded-io = "0.6.1"
# embedded-io = "0.6"
# embedded-io-async = "0.6.1"
#
# nb = "1.1"

cortex-m = { version = "0.7" }
cortex-m-rt = "0.7"

rtic = { version = "2.0.0", features = [ "thumbv6-backend" ] }
rtic = { version = "2.1", features = [ "thumbv6-backend" ] }
# portable-atomic = {version = "1.6.0", features=["critical-section"]}
rp-pico = "0.8"
rtic-monotonics = { version = "1.5", features = ["rp2040"] }
rp-pico = "0.9"
rtic-monotonics = { version = "2", features = ["rp2040"] }
rtic-sync = "1.3"
# nb= { version = "1.0", features = ["defmt-0-3"]}
nb = "0.1.3"
futures = { version = "0.3", default-features = false, features = ["async-await"] }

# for usb serial connection (update when the hal is updated)
usb-device = "0.2"
usbd-hid = "0.5.0"
usbd-serial = "0.1"
usb-device = "0.3"
usbd-serial = "0.2"

pwm-pca9685 = "0.3"
pwm-pca9685 = "1.0"

pio-proc = "0.2"
pio = "0.2"
Expand Down
2 changes: 1 addition & 1 deletion slamrs-robot-rtic/firmware/src/encoder.rs
Original file line number Diff line number Diff line change
Expand Up @@ -97,7 +97,7 @@ fn initialize<
program: InstalledProgram<PIO>,
target: Target,
) {
let (mut sm, pio_rx, _tx) = PIOBuilder::from_program(program)
let (mut sm, pio_rx, _tx) = PIOBuilder::from_installed_program(program)
.in_pin_base(pin_in_a.num)
.autopush(false)
.push_threshold(32)
Expand Down
25 changes: 15 additions & 10 deletions slamrs-robot-rtic/firmware/src/main.rs
Original file line number Diff line number Diff line change
Expand Up @@ -11,6 +11,10 @@ mod ws2812b;
use defmt_rtt as _;
use panic_probe as _;

use rtic_monotonics::rp2040::prelude::*;

rp2040_timer_monotonic!(Mono);

#[rtic::app(
device = rp_pico::hal::pac,
// Replace the `FreeInterrupt1, ...` with free interrupt vectors if software tasks are used
Expand All @@ -29,8 +33,8 @@ mod app {
use crate::util::channel_send;

use core::sync::atomic::Ordering;
use defmt::{debug, error, info, warn};
use embedded_hal::digital::v2::OutputPin;
use defmt::{error, info, warn};
use embedded_hal::digital::OutputPin;
use futures::FutureExt;
use library::event::Event;
use library::neato::RunningParser;
Expand All @@ -49,7 +53,7 @@ mod app {
Clock,
};
use rp_pico::XOSC_CRYSTAL_FREQ;
use rtic_monotonics::rp2040::*;
use rtic_monotonics::Monotonic;

use rtic_sync::portable_atomic::AtomicU8;
// USB Device support
Expand Down Expand Up @@ -211,9 +215,8 @@ mod app {
// TODO setup monotonic if used
// Initialize the interrupt for the RP2040 timer and obtain the token
// proving that we have.
let rp2040_timer_token = rtic_monotonics::create_rp2040_monotonic_token!();
// Configure the clocks, watchdog - The default is to generate a 125 MHz system clock
Timer::start(ctx.device.TIMER, &mut ctx.device.RESETS, rp2040_timer_token); // default rp2040 clock-rate is 125MHz
crate::Mono::start(ctx.device.TIMER, &mut ctx.device.RESETS); // default rp2040 clock-rate is 125MHz
let mut watchdog = Watchdog::new(ctx.device.WATCHDOG);
let clocks = clocks::init_clocks_and_plls(
XOSC_CRYSTAL_FREQ,
Expand Down Expand Up @@ -289,9 +292,11 @@ mod app {

// Create a USB device with a fake VID and PID
let usb_dev = UsbDeviceBuilder::new(&bus_ref, UsbVidPid(0x16c0, 0x27dd))
.manufacturer("Fake company")
.product("Serial port")
.serial_number("TEST")
.strings(&[StringDescriptors::default()
.manufacturer("Fake company")
.product("Serial port")
.serial_number("TEST")])
.unwrap()
.device_class(usbd_serial::USB_CLASS_CDC)
.build();

Expand All @@ -311,7 +316,7 @@ mod app {
let scl_pin: hal::gpio::Pin<_, hal::gpio::FunctionI2C, PullNone> = pins.gpio1.reconfigure();

// Create the I²C drive
let i2c = hal::I2C::i2c0(
let i2c = hal::I2C::i2c0_with_external_pull_up(
ctx.device.I2C0,
sda_pin,
scl_pin,
Expand Down Expand Up @@ -452,7 +457,7 @@ mod app {
loop {
futures::select_biased! {

_ = Timer::delay(1000.millis()).fuse() => {
_ = crate::Mono::delay(1000.millis()).fuse() => {
if is_connected {
// Send a ping message to the robot
channel_send(cx.local.robot_message_sender, RobotMessage::Pong, "event_loop");
Expand Down
29 changes: 12 additions & 17 deletions slamrs-robot-rtic/firmware/src/motor.rs
Original file line number Diff line number Diff line change
@@ -1,10 +1,7 @@
//! Module for controling the motors through the Afafruit Featherwing Motor Driver
//!

// embedded_hal::blocking::i2c:

use core::marker::PhantomData;
use embedded_hal::blocking::i2c;
use embedded_hal::i2c;
use pwm_pca9685::{Channel, Pca9685};

/// All possible errors
Expand Down Expand Up @@ -64,12 +61,9 @@ pub struct Motor<I2C> {
pwm: Channel,
}

impl<I2C, E> MotorDriver<I2C>
where
I2C: i2c::Write<Error = E> + i2c::WriteRead<Error = E>,
{
impl<I2C: i2c::I2c> MotorDriver<I2C> {
/// Create and initialize a new motor driver instance
pub fn new(i2c: I2C, address: u8, mut frequency_hz: f32) -> Result<Self, Error<E>> {
pub fn new(i2c: I2C, address: u8, mut frequency_hz: f32) -> Result<Self, Error<I2C::Error>> {
let mut pwm = Pca9685::new(i2c, address)?;

// initialize the driver
Expand All @@ -92,7 +86,7 @@ where
}

/// Get a motor instance
pub fn motor(&mut self, motor: MotorId) -> Result<Motor<I2C>, Error<E>> {
pub fn motor(&mut self, motor: MotorId) -> Result<Motor<I2C>, Error<I2C::Error>> {
// make sure the motor is not already taken
if self.taken[motor.as_u8() as usize] {
return Err(Error::MotorAlreadyTaken);
Expand All @@ -117,12 +111,13 @@ where
}
}

impl<I2C, E> Motor<I2C>
where
I2C: i2c::Write<Error = E> + i2c::WriteRead<Error = E>,
{
impl<I2C: i2c::I2c> Motor<I2C> {
/// Set the speed of the motor
pub fn set_speed(&mut self, mc: &mut MotorDriver<I2C>, speed: u16) -> Result<(), Error<E>> {
pub fn set_speed(
&mut self,
mc: &mut MotorDriver<I2C>,
speed: u16,
) -> Result<(), Error<I2C::Error>> {
mc.pwm.set_channel_on_off(self.pwm, 0, speed)?;
Ok(())
}
Expand All @@ -132,7 +127,7 @@ where
&mut self,
mc: &mut MotorDriver<I2C>,
speed: i16,
) -> Result<(), Error<E>> {
) -> Result<(), Error<I2C::Error>> {
let (direction, speed) = if speed > 0 {
(MotorDirection::Forward, speed as u16)
} else if speed < 0 {
Expand All @@ -150,7 +145,7 @@ where
&mut self,
mc: &mut MotorDriver<I2C>,
direction: MotorDirection,
) -> Result<(), Error<E>> {
) -> Result<(), Error<I2C::Error>> {
match direction {
MotorDirection::Forward => {
mc.pwm.set_channel_on_off(self.in2, 0, 0)?; // take low first to avoid brake'
Expand Down
11 changes: 6 additions & 5 deletions slamrs-robot-rtic/firmware/src/tasks/esp.rs
Original file line number Diff line number Diff line change
@@ -1,18 +1,19 @@
use defmt::{error, info, warn};
use embedded_hal::digital::v2::OutputPin;
use embedded_hal::digital::OutputPin;
use futures::FutureExt;
use library::{
event::Event,
parse_at::{EspMessage, ParsedMessage},
};
use rp_pico::hal::fugit::ExtU64;
use rtic::Mutex;
use rtic_monotonics::rp2040::Timer;
use rtic_monotonics::Monotonic;

use crate::{
app::{init_esp, uart1_esp32, DATA_PACKET_SIZE},
tasks::heartbeat::{Color, LedStatus, Speed},
util::{channel_send, wait_for_message},
Mono,
};

/// Task that initializes and handles the ESP WIFI connection
Expand All @@ -26,9 +27,9 @@ pub async fn init_esp(mut cx: init_esp::Context<'_>) {
cx.local.esp_mode.set_high().ok();
cx.local.esp_reset.set_low().ok();

Timer::delay(1.secs()).await;
Mono::delay(1.secs()).await;
cx.local.esp_reset.set_high().ok();
Timer::delay(1.secs()).await;
Mono::delay(1.secs()).await;

// read messages from the device and advance the inner state machine

Expand Down Expand Up @@ -97,7 +98,7 @@ pub async fn init_esp(mut cx: init_esp::Context<'_>) {
info!("Enabling Multiple Connections");
cx.local.uart1_tx.write_full_blocking(b"AT+CIPMUX=1\r\n");
wait_for_message(cx.local.esp_receiver, EspMessage::Ok).await;
Timer::delay(1.secs()).await;
Mono::delay(1.secs()).await;

cx.local
.uart1_tx
Expand Down
9 changes: 4 additions & 5 deletions slamrs-robot-rtic/firmware/src/tasks/heartbeat.rs
Original file line number Diff line number Diff line change
@@ -1,8 +1,7 @@
use crate::app::heartbeat;
use embedded_hal::digital::v2::ToggleableOutputPin;
use crate::{app::heartbeat, Mono};
use rp_pico::hal::fugit::ExtU64;
use rtic::Mutex;
use rtic_monotonics::{rp2040::Timer, Monotonic};
use rtic_monotonics::Monotonic;

#[derive(defmt::Format, Copy, Clone)]
pub enum Color {
Expand Down Expand Up @@ -64,10 +63,10 @@ pub async fn heartbeat(mut cx: heartbeat::Context<'_>) {
const SCALE: u8 = 4;

// 10hz loop
let mut next_iteration_instant = Timer::now();
let mut next_iteration_instant = Mono::now();
loop {
next_iteration_instant += 100.millis();
Timer::delay_until(next_iteration_instant).await;
Mono::delay_until(next_iteration_instant).await;

let state = cx.shared.led_status.lock(|s| *s);

Expand Down
14 changes: 7 additions & 7 deletions slamrs-robot-rtic/firmware/src/tasks/motors.rs
Original file line number Diff line number Diff line change
@@ -1,9 +1,9 @@
use crate::app::motor_control_loop;
use defmt::{info, warn};
use crate::{app::motor_control_loop, Mono};
use defmt::warn;
use fixed::{types::extra::U16, FixedI32};
use rp_pico::hal::fugit::ExtU32;
use rtic::Mutex;
use rtic_monotonics::{rp2040::Timer, Monotonic};
use rtic_monotonics::Monotonic;

/// The fixed point type used for the PI controller
pub type F32 = FixedI32<U16>;
Expand All @@ -19,14 +19,14 @@ pub async fn motor_control_loop(mut cx: motor_control_loop::Context<'_>) {
let mut pi_right = PiController::new();
let mut pi_left = PiController::new();

let mut next_iteration_instant = Timer::now();
let mut next_iteration_instant = Mono::now();
loop {
next_iteration_instant += CONTROL_LOOP_PERIOD_MS.millis();
if next_iteration_instant < Timer::now() {
if next_iteration_instant < Mono::now() {
warn!("Motor control loop is running behind");
next_iteration_instant = Timer::now();
next_iteration_instant = Mono::now();
}
Timer::delay_until(next_iteration_instant).await;
Mono::delay_until(next_iteration_instant).await;

// do the actual control loop logic with a PI controller

Expand Down
10 changes: 6 additions & 4 deletions slamrs-robot-rtic/firmware/src/tasks/neato.rs
Original file line number Diff line number Diff line change
@@ -1,14 +1,14 @@
use crate::{
app::{neato_motor_control, uart0_neato},
motor::MotorDirection,
Mono,
};
use core::sync::atomic::{AtomicBool, AtomicU16, Ordering};
use cortex_m::peripheral::dwt;
use defmt::{info, warn};
use defmt::info;
use library::slamrs_message::{RobotMessage, ScanFrame};
use rp_pico::hal::fugit::ExtU64;
use rtic::Mutex;
use rtic_monotonics::rp2040::Timer;
use rtic_monotonics::Monotonic;

/// Atomic variables to control the on/off state of the motor and the last measured RPM
pub static MOTOR_ON: AtomicBool = AtomicBool::new(false);
Expand All @@ -25,7 +25,7 @@ pub async fn neato_motor_control(mut cx: neato_motor_control::Context<'_>) {

let mut pwm_current: i32 = 0;
loop {
Timer::delay(200.millis()).await;
Mono::delay(200.millis()).await;

let rpm_target = if MOTOR_ON.load(Ordering::Relaxed) {
300
Expand Down Expand Up @@ -61,6 +61,7 @@ pub async fn neato_motor_control(mut cx: neato_motor_control::Context<'_>) {
// );
}
}

pub fn uart0_neato(cx: uart0_neato::Context<'_>) {
cx.local.parser.consume(cx.local.uart0_rx_neato, |data| {
// some exponential smoothing on the raw (*64) RPM value
Expand All @@ -73,6 +74,7 @@ pub fn uart0_neato(cx: uart0_neato::Context<'_>) {
info!("neato rpm: {:?}", rpm);
// TODO: should we add a data validation check?
if rpm < 250 && rpm > 350 {
// THIS WILL NEVER BE TRUE LOL
return;
}

Expand Down
2 changes: 1 addition & 1 deletion slamrs-robot-rtic/firmware/src/tasks/usb.rs
Original file line number Diff line number Diff line change
@@ -1,5 +1,5 @@
use crate::{app::usb_irq, app::usb_sender, util::channel_send};
use defmt::{info, warn};
use defmt::warn;
use library::event::Event;
use rtic::mutex_prelude::*;
use usb_device::prelude::*;
Expand Down
3 changes: 1 addition & 2 deletions slamrs-robot-rtic/firmware/src/ws2812b.rs
Original file line number Diff line number Diff line change
@@ -1,4 +1,3 @@
use defmt::info;
use rp_pico::{
hal::{
fugit::HertzU32,
Expand Down Expand Up @@ -62,7 +61,7 @@ impl WS2812B {
let (div_int, div_frac) = pio_calculate_clkdiv_from_float(div);

// configure the state machine
let (mut sm, _rx, tx) = rp_pico::hal::pio::PIOBuilder::from_program(installed)
let (mut sm, _rx, tx) = rp_pico::hal::pio::PIOBuilder::from_installed_program(installed)
.side_set_pin_base(pin.id().num)
.clock_divisor_fixed_point(div_int, div_frac)
.out_shift_direction(ShiftDirection::Left)
Expand Down
Loading

0 comments on commit 5397c28

Please sign in to comment.