ank/cruisecontrol
1
Fork 0
Code Issues Activity

sync working dir

Browse source
This commit is contained in:
Andy Killorin 2024-11-21 19:48:43 -05:00
parent 8a10953cc4
commit 06b955875c
Signed by: ank
GPG key ID: 23F9463ECB67FE8C
10 changed files with 404 additions and 3631 deletions
Show stats Download patch file Download diff file Expand all files Collapse all files

587
controller/src/main.rs
View file

@ -15,6 +15,8 @@ use core::str::from_utf8;
use bt_hci::cmd::info;
use cyw43_pio::PioSpi;
use embassy_rp::i2c::{Async, I2c};
use embassy_rp::pwm::{self, Pwm};
use fixed::FixedU16;
use log::*;
//use embassy_rp::i2c::InterruptHandler;
use embassy_executor::Spawner;
@ -71,6 +73,16 @@ async fn main(spawner: Spawner) {
let driver = Driver::new(p.USB, Irqs);
spawner.spawn(logger_task(driver)).unwrap();
let mut flipper = Output::new(p.PIN_22, Level::Low);
let mut c: pwm::Config = Default::default();
c.divider = 255.into();
c.top = 8715;
c.compare_b = 7210;
c.compare_a = 2240;
let mut pwm = Pwm::new_output_ab(p.PWM_SLICE1, p.PIN_18, p.PIN_19, c.clone());
flipper.set_high();
let sda = p.PIN_26;
let scl = p.PIN_27;
let config = embassy_rp::i2c::Config::default();
@ -128,291 +140,302 @@ async fn main(spawner: Spawner) {
//embassy_time::Timer::after_millis(7000).await;
async fn write_to_device<'a, T,const N: usize>(bus: &mut embassy_rp::i2c::I2c<'a, T,Async>, addr: u16, data: [[u8;2];N])
where T: embassy_rp::i2c::Instance {
for transaction in data {
let _ = bus.write_async(addr, transaction).await;
}
}
async fn write_flag<'a, T>(bus: &mut embassy_rp::i2c::I2c<'a, T,Async>, addr: u16, reg: u8, bit: u8, value: bool)
where T: embassy_rp::i2c::Instance {
let mut initial: [u8;1] = [0];
let _ = bus.write_read_async(addr, [reg], &mut initial).await;
let mask = 1 << bit;
if value {
initial[0] |= mask;
} else {
initial[0] &= !mask;
}
let _ = bus.write_async(addr, [reg, initial[0]]).await;
}
let id = 0x29;
Timer::after_millis(50).await; // sensor boot
write_to_device(&mut bus, id, [
[0x88, 0x00],
[0x80, 0x01],
[0xFF, 0x01],
[0x00, 0x00],
]).await;
let mut stop: [u8;1] = [0];
let _ = bus.write_read_async(id, [0x91], &mut stop);
write_to_device(&mut bus, id, [
[0x00, 0x01],
[0xFF, 0x00],
[0x80, 0x00],
]).await;
// disable SIGNAL_RATE_MSRC (bit 1) and SIGNAL_RATE_PRE_RANGE (bit 4) limit checks
write_flag(&mut bus, id, 0x60, 1, true).await;
write_flag(&mut bus, id, 0x60, 4, true).await;
let mega_counts_per_second = 0.25;
let mega_counts_per_second: u16 = (mega_counts_per_second * (1<<7) as f64) as u16;
let _ = bus.write_async(id, [FINAL_RANGE_CONFIG_MIN_COUNT_RATE_RTN_LIMIT as u8,
(mega_counts_per_second >> 8) as u8,
(mega_counts_per_second & 255) as u8
]).await;
// get spad info
let _ = bus.write_async(id, [SYSTEM_SEQUENCE_CONFIG as u8, 0xFF]).await;
write_to_device(&mut bus, id , [
[0x80, 0x01],
[0xff, 0x01],
[0x00, 0x00],
[0xff, 0x06],
]).await;
write_flag(&mut bus, id, 0x83, 3, true).await;
write_to_device(&mut bus, id , [
[0xff, 0x07],
[0x81, 0x01],
[0x80, 0x01],
[0x94, 0x6b],
[0x83, 0x00],
]).await;
debug!("starting spad wait");
loop {
let mut wait: [u8;1] = [0];
let _ = bus.write_read_async(id, [0x83], &mut wait).await;
if wait[0] != 0 {
break;
}
Timer::after_micros(5).await;
}
debug!("ended spad wait");
let _ = bus.write_async(id, [0x83, 0x01]).await;
let mut value: [u8;1] = [0];
let _ = bus.write_read_async(id, [0x92], &mut value).await;
write_to_device(&mut bus, id, [
[0x81, 0x00],
[0xff, 0x06],
]).await;
write_flag(&mut bus, id, 0x83, 3, false).await;
write_to_device(&mut bus, id, [
[0xff, 0x01],
[0x00, 0x01],
[0xff, 0x00],
[0x80, 0x00],
]).await;
let count = value[0] & 0x7f;
let is_aperture = value[0] & 0b10000000;
let is_aperture = is_aperture != 0;
// TODO: vl53l0x.py post line 200
let mut spad_map: [u8;6]=[0;6];
let _ = bus.write_read_async(id, [GLOBAL_CONFIG_SPAD_ENABLES_REF_0 as u8], &mut spad_map);
write_to_device(&mut bus, id, [
[0xff, 0x01],
[DYNAMIC_SPAD_REF_EN_START_OFFSET as u8, 0x00],
[DYNAMIC_SPAD_NUM_REQUESTED_REF_SPAD as u8, 0x2c],
[0xff, 0x00],
[DYNAMIC_SPAD_REF_EN_START_OFFSET as u8, 0xb4],
]).await;
let mut spads_enabled = 0;
for i in 0..48 {
if i < 12 && is_aperture || spads_enabled >= count {
spad_map[i/8] &= !(1<< (i>>2));
} else if (spad_map[i/8] & (1<< (i>>2))) != 0 {
spads_enabled += 1;
}
}
let mut spad_write: [u8;7] = [0;7];
spad_write[0] = GLOBAL_CONFIG_SPAD_ENABLES_REF_0 as u8;
spad_write[1..].clone_from_slice(&spad_map);
let _ = bus.write_async(id, spad_write).await;
write_to_device(&mut bus, id, [
[0xFF, 0x01],
[0x00, 0x00],
[0xFF, 0x00],
[0x09, 0x00],
[0x10, 0x00],
[0x11, 0x00],
[0x24, 0x01],
[0x25, 0xFF],
[0x75, 0x00],
[0xFF, 0x01],
[0x4E, 0x2C],
[0x48, 0x00],
[0x30, 0x20],
[0xFF, 0x00],
[0x30, 0x09],
[0x54, 0x00],
[0x31, 0x04],
[0x32, 0x03],
[0x40, 0x83],
[0x46, 0x25],
[0x60, 0x00],
[0x27, 0x00],
[0x50, 0x06],
[0x51, 0x00],
[0x52, 0x96],
[0x56, 0x08],
[0x57, 0x30],
[0x61, 0x00],
[0x62, 0x00],
[0x64, 0x00],
[0x65, 0x00],
[0x66, 0xA0],
[0xFF, 0x01],
[0x22, 0x32],
[0x47, 0x14],
[0x49, 0xFF],
[0x4A, 0x00],
[0xFF, 0x00],
[0x7A, 0x0A],
[0x7B, 0x00],
[0x78, 0x21],
[0xFF, 0x01],
[0x23, 0x34],
[0x42, 0x00],
[0x44, 0xFF],
[0x45, 0x26],
[0x46, 0x05],
[0x40, 0x40],
[0x0E, 0x06],
[0x20, 0x1A],
[0x43, 0x40],
[0xFF, 0x00],
[0x34, 0x03],
[0x35, 0x44],
[0xFF, 0x01],
[0x31, 0x04],
[0x4B, 0x09],
[0x4C, 0x05],
[0x4D, 0x04],
[0xFF, 0x00],
[0x44, 0x00],
[0x45, 0x20],
[0x47, 0x08],
[0x48, 0x28],
[0x67, 0x00],
[0x70, 0x04],
[0x71, 0x01],
[0x72, 0xFE],
[0x76, 0x00],
[0x77, 0x00],
[0xFF, 0x01],
[0x0D, 0x01],
[0xFF, 0x00],
[0x80, 0x01],
[0x01, 0xF8],
[0xFF, 0x01],
[0x8E, 0x01],
[0x00, 0x01],
[0xFF, 0x00],
[0x80, 0x00],
]).await;
//write_flag(&mut bus, id, GPIO_HV_MUX_ACTIVE_HIGH as u8, 4, false).await;
//let _ = bus.write_async(id, [SYSTEM_SEQUENCE_CONFIG as u8, 0x01]).await;
//calibrate(&mut bus, id, 0x40).await;
//let _ = bus.write_async(id, [SYSTEM_SEQUENCE_CONFIG as u8, 0x02]).await;
//calibrate(&mut bus, id, 0x00).await;
//let _ = bus.write_async(id, [SYSTEM_SEQUENCE_CONFIG as u8, 0xe8]).await;
async fn calibrate<'a, T>(bus: &mut embassy_rp::i2c::I2c<'a, T,Async>, addr: u16, data: u8)
where T: embassy_rp::i2c::Instance {
let _ = bus.write_async(addr, [SYSRANGE_START as u8 ,data | 0x01]).await;
debug!("started calib wait");
loop {
let mut wait: [u8;1] = [0];
let _ = bus.write_read_async(addr, [0x13], &mut wait).await;
if wait[0] & 0x07 != 0 {
break;
}
Timer::after_micros(5).await;
}
debug!("ended calib wait");
write_to_device(bus, addr, [
[SYSTEM_INTERRUPT_CLEAR as u8, 0x01],
[SYSRANGE_START as u8, 0x00],
]).await;
}
//TODO VL53L0X.cpp L 236-280
// start continuous mode
write_to_device(&mut bus, id, [
[0x80, 0x01],
[0xFF, 0x01],
[0x00, 0x00],
[0x91, stop[0]],
[0x00, 0x01],
[0xFF, 0x00],
[0x80, 0x00],
//[0x04, 0x00], // measurement delay
[0x00, 0x02], // back to back shots
]).await;
//let _ = bus.write_async(0x88u16, [0x00]).await;
//let _ = bus.write_async(0x80u16, [0x01]).await;
//let _ = bus.write_async(0xFFu16, [0x01]).await;
//let _ = bus.write_async(0x00u16, [0x00]).await;
//let _ = bus.write_async(0x00u16, [0x01]).await;
//let _ = bus.write_async(0xFFu16, [0x00]).await;
//let _ = bus.write_async(0x80u16, [0x00]).await;
//loop {
// debug!("starting wait part 1");
// loop {
// let mut wait: [u8;1] = [0];
// let _ = bus.write_read_async(id, [SYSRANGE_START as u8], &mut wait).await;
// debug!("{wait:?}");
// if wait[0] & 0x01 != 0 {
// break;
// }
// Timer::after_micros(5).await;
//async fn write_to_device<'a, T,const N: usize>(bus: &mut embassy_rp::i2c::I2c<'a, T,Async>, addr: u16, data: [[u8;2];N])
//where T: embassy_rp::i2c::Instance {
// for transaction in data {
// let _ = bus.write_async(addr, transaction).await;
// }
// debug!("starting wait part 2");
//}
//async fn write_flag<'a, T>(bus: &mut embassy_rp::i2c::I2c<'a, T,Async>, addr: u16, reg: u8, bit: u8, value: bool)
//where T: embassy_rp::i2c::Instance {
// let mut initial: [u8;1] = [0];
// let _ = bus.write_read_async(addr, [reg], &mut initial).await;
// let mask = 1 << bit;
// if value {
// initial[0] |= mask;
// } else {
// initial[0] &= !mask;
// }
// let _ = bus.write_async(addr, [reg, initial[0]]).await;
//}
//let id = 0x29;
//Timer::after_millis(50).await; // sensor boot
//write_to_device(&mut bus, id, [
// [0x88, 0x00],
// [0x80, 0x01],
// [0xFF, 0x01],
// [0x00, 0x00],
//]).await;
//let mut stop: [u8;1] = [0];
//let _ = bus.write_read_async(id, [0x91], &mut stop);
//write_to_device(&mut bus, id, [
// [0x00, 0x01],
// [0xFF, 0x00],
// [0x80, 0x00],
//]).await;
//
//// disable SIGNAL_RATE_MSRC (bit 1) and SIGNAL_RATE_PRE_RANGE (bit 4) limit checks
//write_flag(&mut bus, id, 0x60, 1, true).await;
//write_flag(&mut bus, id, 0x60, 4, true).await;
//let mega_counts_per_second = 0.25;
//let mega_counts_per_second: u16 = (mega_counts_per_second * (1<<7) as f64) as u16;
//let _ = bus.write_async(id, [FINAL_RANGE_CONFIG_MIN_COUNT_RATE_RTN_LIMIT as u8,
// (mega_counts_per_second >> 8) as u8,
// (mega_counts_per_second & 255) as u8
//]).await;
//// get spad info
//let _ = bus.write_async(id, [SYSTEM_SEQUENCE_CONFIG as u8, 0xFF]).await;
//write_to_device(&mut bus, id , [
// [0x80, 0x01],
// [0xff, 0x01],
// [0x00, 0x00],
// [0xff, 0x06],
//]).await;
//write_flag(&mut bus, id, 0x83, 3, true).await;
//write_to_device(&mut bus, id , [
// [0xff, 0x07],
// [0x81, 0x01],
// [0x80, 0x01],
// [0x94, 0x6b],
// [0x83, 0x00],
//]).await;
//debug!("starting spad wait");
//loop {
// let mut wait: [u8;1] = [0];
// let _ = bus.write_read_async(id, [0x83], &mut wait).await;
// if wait[0] != 0 {
// break;
// }
// Timer::after_micros(5).await;
//}
//debug!("ended spad wait");
//let _ = bus.write_async(id, [0x83, 0x01]).await;
//let mut value: [u8;1] = [0];
//let _ = bus.write_read_async(id, [0x92], &mut value).await;
//write_to_device(&mut bus, id, [
// [0x81, 0x00],
// [0xff, 0x06],
//]).await;
//write_flag(&mut bus, id, 0x83, 3, false).await;
//write_to_device(&mut bus, id, [
// [0xff, 0x01],
// [0x00, 0x01],
// [0xff, 0x00],
// [0x80, 0x00],
//]).await;
//let count = value[0] & 0x7f;
//let is_aperture = value[0] & 0b10000000;
//let is_aperture = is_aperture != 0;
//// TODO: vl53l0x.py post line 200
//let mut spad_map: [u8;6]=[0;6];
//let _ = bus.write_read_async(id, [GLOBAL_CONFIG_SPAD_ENABLES_REF_0 as u8], &mut spad_map);
//write_to_device(&mut bus, id, [
// [0xff, 0x01],
// [DYNAMIC_SPAD_REF_EN_START_OFFSET as u8, 0x00],
// [DYNAMIC_SPAD_NUM_REQUESTED_REF_SPAD as u8, 0x2c],
// [0xff, 0x00],
// [DYNAMIC_SPAD_REF_EN_START_OFFSET as u8, 0xb4],
//]).await;
//let mut spads_enabled = 0;
//for i in 0..48 {
// if i < 12 && is_aperture || spads_enabled >= count {
// spad_map[i/8] &= !(1<< (i>>2));
// } else if (spad_map[i/8] & (1<< (i>>2))) != 0 {
// spads_enabled += 1;
// }
//}
//let mut spad_write: [u8;7] = [0;7];
//spad_write[0] = GLOBAL_CONFIG_SPAD_ENABLES_REF_0 as u8;
//spad_write[1..].clone_from_slice(&spad_map);
//let _ = bus.write_async(id, spad_write).await;
//
//write_to_device(&mut bus, id, [
// [0xFF, 0x01],
// [0x00, 0x00],
// [0xFF, 0x00],
// [0x09, 0x00],
// [0x10, 0x00],
// [0x11, 0x00],
// [0x24, 0x01],
// [0x25, 0xFF],
// [0x75, 0x00],
// [0xFF, 0x01],
// [0x4E, 0x2C],
// [0x48, 0x00],
// [0x30, 0x20],
// [0xFF, 0x00],
// [0x30, 0x09],
// [0x54, 0x00],
// [0x31, 0x04],
// [0x32, 0x03],
// [0x40, 0x83],
// [0x46, 0x25],
// [0x60, 0x00],
// [0x27, 0x00],
// [0x50, 0x06],
// [0x51, 0x00],
// [0x52, 0x96],
// [0x56, 0x08],
// [0x57, 0x30],
// [0x61, 0x00],
// [0x62, 0x00],
// [0x64, 0x00],
// [0x65, 0x00],
// [0x66, 0xA0],
// [0xFF, 0x01],
// [0x22, 0x32],
// [0x47, 0x14],
// [0x49, 0xFF],
// [0x4A, 0x00],
// [0xFF, 0x00],
// [0x7A, 0x0A],
// [0x7B, 0x00],
// [0x78, 0x21],
// [0xFF, 0x01],
// [0x23, 0x34],
// [0x42, 0x00],
// [0x44, 0xFF],
// [0x45, 0x26],
// [0x46, 0x05],
// [0x40, 0x40],
// [0x0E, 0x06],
// [0x20, 0x1A],
// [0x43, 0x40],
// [0xFF, 0x00],
// [0x34, 0x03],
// [0x35, 0x44],
// [0xFF, 0x01],
// [0x31, 0x04],
// [0x4B, 0x09],
// [0x4C, 0x05],
// [0x4D, 0x04],
// [0xFF, 0x00],
// [0x44, 0x00],
// [0x45, 0x20],
// [0x47, 0x08],
// [0x48, 0x28],
// [0x67, 0x00],
// [0x70, 0x04],
// [0x71, 0x01],
// [0x72, 0xFE],
// [0x76, 0x00],
// [0x77, 0x00],
// [0xFF, 0x01],
// [0x0D, 0x01],
// [0xFF, 0x00],
// [0x80, 0x01],
// [0x01, 0xF8],
// [0xFF, 0x01],
// [0x8E, 0x01],
// [0x00, 0x01],
// [0xFF, 0x00],
// [0x80, 0x00],
// [SYSTEM_INTERRUPT_CONFIG_GPIO as u8, 0x04],
//]).await;
////write_flag(&mut bus, id, GPIO_HV_MUX_ACTIVE_HIGH as u8, 4, false).await;
////let _ = bus.write_async(id, [SYSTEM_SEQUENCE_CONFIG as u8, 0x01]).await;
////calibrate(&mut bus, id, 0x40).await;
////let _ = bus.write_async(id, [SYSTEM_SEQUENCE_CONFIG as u8, 0x02]).await;
////calibrate(&mut bus, id, 0x00).await;
////let _ = bus.write_async(id, [SYSTEM_SEQUENCE_CONFIG as u8, 0xe8]).await;
//async fn calibrate<'a, T>(bus: &mut embassy_rp::i2c::I2c<'a, T,Async>, addr: u16, data: u8)
//where T: embassy_rp::i2c::Instance {
// let _ = bus.write_async(addr, [SYSRANGE_START as u8 ,data | 0x01]).await;
// debug!("started calib wait");
// loop {
// let mut wait: [u8;1] = [0];
// let _ = bus.write_read_async(id, [RESULT_INTERRUPT_STATUS as u8], &mut wait).await;
// let _ = bus.write_read_async(addr, [0x13], &mut wait).await;
// if wait[0] & 0x07 != 0 {
// break;
// }
// Timer::after_micros(5).await;
// }
// debug!("ending wait part 2");
// let mut output: [u8; 2] = [0;2];
// let _ = bus.write_read_async(id, [0x14], &mut output).await;
// let _ = bus.write_async(id, [0x0B, 0x01]).await;
// info!("{:?}",output);
// Timer::after_millis(20).await;
// debug!("ended calib wait");
// write_to_device(bus, addr, [
// [SYSTEM_INTERRUPT_CLEAR as u8, 0x01],
// [SYSRANGE_START as u8, 0x00],
// ]).await;
//}
////TODO VL53L0X.cpp L 236-280
//// start continuous mode
//write_to_device(&mut bus, id, [
// [0x80, 0x01],
// [0xFF, 0x01],
// [0x00, 0x00],
// [0x91, stop[0]],
// [0x00, 0x01],
// [0xFF, 0x00],
// [0x80, 0x00],
// //[0x04, 0x00], // measurement delay
// [0x00, 0x02], // back to back shots
//]).await;
////let _ = bus.write_async(0x88u16, [0x00]).await;
////let _ = bus.write_async(0x80u16, [0x01]).await;
////let _ = bus.write_async(0xFFu16, [0x01]).await;
////let _ = bus.write_async(0x00u16, [0x00]).await;
////let _ = bus.write_async(0x00u16, [0x01]).await;
////let _ = bus.write_async(0xFFu16, [0x00]).await;
////let _ = bus.write_async(0x80u16, [0x00]).await;
//
////loop {
//// debug!("starting wait part 1");
//// loop {
//// let mut wait: [u8;1] = [0];
//// let _ = bus.write_read_async(id, [SYSRANGE_START as u8], &mut wait).await;
//// debug!("{wait:?}");
//// if wait[0] & 0x01 != 0 {
//// break;
//// }
//// Timer::after_micros(5).await;
//// }
//// debug!("starting wait part 2");
//// loop {
//// let mut wait: [u8;1] = [0];
//// let _ = bus.write_read_async(id, [RESULT_INTERRUPT_STATUS as u8], &mut wait).await;
//// if wait[0] & 0x07 != 0 {
//// break;
//// }
//// Timer::after_micros(5).await;
//// }
//// debug!("ending wait part 2");
//// let mut output: [u8; 2] = [0;2];
//// let _ = bus.write_read_async(id, [0x14], &mut output).await;
//// let _ = bus.write_async(id, [0x0B, 0x01]).await;
//// info!("out: {:?}",output);
//// Timer::after_millis(20).await;
////}
loop {
let mut socket = TcpSocket::new(stack, &mut rx_buffer, &mut tx_buffer);
socket.set_timeout(Some(Duration::from_secs(10)));
socket.set_timeout(Some(Duration::from_secs(3)));
let mut c: pwm::Config = Default::default();
c.divider = 255.into();
c.top = 8715;
c.compare_b = 0;
c.compare_a = 0;
pwm.set_config(&c);
flipper.set_low();
control.gpio_set(0, false).await;
info!("Listening on TCP:1234...");
@ -469,8 +492,24 @@ async fn main(spawner: Spawner) {
info!("recd {:#02x}", response[0]);
let _ = hex::encode_to_slice(response, &mut buf);
n = 4;
},
'D' => {
let left: [u8;2] = hex::FromHex::from_hex(segs.next().unwrap()).unwrap();
let right: [u8;2] = hex::FromHex::from_hex(segs.next().unwrap()).unwrap();
info!("left {:?}, right {left:?}", right);
let left = u16::from_be_bytes(left);
c.compare_a = left;
let right = u16::from_be_bytes(right);
c.compare_b = right;
info!("left {:?}, right {left:?}", right);
pwm.set_config(&c);
},
'F' => {
flipper.set_high();
},
'L' => {
flipper.set_low();
},
_ => {}

91
controller/src/vl53l0.rs Normal file
View file

@ -0,0 +1,91 @@
use core::ops::Deref;
#[repr(u8)]
pub enum RegAddr
{
SYSRANGE_START = 0x00,
SYSTEM_THRESH_HIGH = 0x0C,
SYSTEM_THRESH_LOW = 0x0E,
SYSTEM_SEQUENCE_CONFIG = 0x01,
SYSTEM_RANGE_CONFIG = 0x09,
SYSTEM_INTERMEASUREMENT_PERIOD = 0x04,
SYSTEM_INTERRUPT_CONFIG_GPIO = 0x0A,
GPIO_HV_MUX_ACTIVE_HIGH = 0x84,
SYSTEM_INTERRUPT_CLEAR = 0x0B,
RESULT_INTERRUPT_STATUS = 0x13,
RESULT_RANGE_STATUS = 0x14,
RESULT_CORE_AMBIENT_WINDOW_EVENTS_RTN = 0xBC,
//RESULT_CORE_RANGING_TOTAL_EVENTS_RTN = 0xC0,
RESULT_CORE_AMBIENT_WINDOW_EVENTS_REF = 0xD0,
RESULT_CORE_RANGING_TOTAL_EVENTS_REF = 0xD4,
RESULT_PEAK_SIGNAL_RATE_REF = 0xB6,
ALGO_PART_TO_PART_RANGE_OFFSET_MM = 0x28,
I2C_SLAVE_DEVICE_ADDRESS = 0x8A,
MSRC_CONFIG_CONTROL = 0x60,
PRE_RANGE_CONFIG_MIN_SNR = 0x27,
PRE_RANGE_CONFIG_VALID_PHASE_LOW = 0x56,
PRE_RANGE_CONFIG_VALID_PHASE_HIGH = 0x57,
PRE_RANGE_MIN_COUNT_RATE_RTN_LIMIT = 0x64,
FINAL_RANGE_CONFIG_MIN_SNR = 0x67,
FINAL_RANGE_CONFIG_VALID_PHASE_LOW = 0x47,
FINAL_RANGE_CONFIG_VALID_PHASE_HIGH = 0x48,
FINAL_RANGE_CONFIG_MIN_COUNT_RATE_RTN_LIMIT = 0x44,
PRE_RANGE_CONFIG_SIGMA_THRESH_HI = 0x61,
PRE_RANGE_CONFIG_SIGMA_THRESH_LO = 0x62,
PRE_RANGE_CONFIG_VCSEL_PERIOD = 0x50,
PRE_RANGE_CONFIG_TIMEOUT_MACROP_HI = 0x51,
PRE_RANGE_CONFIG_TIMEOUT_MACROP_LO = 0x52,
SYSTEM_HISTOGRAM_BIN = 0x81,
HISTOGRAM_CONFIG_INITIAL_PHASE_SELECT = 0x33,
HISTOGRAM_CONFIG_READOUT_CTRL = 0x55,
FINAL_RANGE_CONFIG_VCSEL_PERIOD = 0x70,
FINAL_RANGE_CONFIG_TIMEOUT_MACROP_HI = 0x71,
FINAL_RANGE_CONFIG_TIMEOUT_MACROP_LO = 0x72,
CROSSTALK_COMPENSATION_PEAK_RATE_MCPS = 0x20,
MSRC_CONFIG_TIMEOUT_MACROP = 0x46,
SOFT_RESET_GO2_SOFT_RESET_N = 0xBF,
IDENTIFICATION_MODEL_ID = 0xC0,
IDENTIFICATION_REVISION_ID = 0xC2,
OSC_CALIBRATE_VAL = 0xF8,
GLOBAL_CONFIG_VCSEL_WIDTH = 0x32,
GLOBAL_CONFIG_SPAD_ENABLES_REF_0 = 0xB0,
GLOBAL_CONFIG_SPAD_ENABLES_REF_1 = 0xB1,
GLOBAL_CONFIG_SPAD_ENABLES_REF_2 = 0xB2,
GLOBAL_CONFIG_SPAD_ENABLES_REF_3 = 0xB3,
GLOBAL_CONFIG_SPAD_ENABLES_REF_4 = 0xB4,
GLOBAL_CONFIG_SPAD_ENABLES_REF_5 = 0xB5,
//GLOBAL_CONFIG_REF_EN_START_SELECT = 0xB6,
DYNAMIC_SPAD_NUM_REQUESTED_REF_SPAD = 0x4E,
DYNAMIC_SPAD_REF_EN_START_OFFSET = 0x4F,
POWER_MANAGEMENT_GO1_POWER_FORCE = 0x80,
VHV_CONFIG_PAD_SCL_SDA__EXTSUP_HV = 0x89,
ALGO_PHASECAL_LIM = 0x30,
//ALGO_PHASECAL_CONFIG_TIMEOUT = 0x30,
}
struct Vl53l0x {
}

158
python-controller/MPU6050.py
View file

@ -1,158 +0,0 @@
"""
A lightweight MicroPython implementation for interfacing with an MPU-6050 via I2C.
Author: Tim Hanewich - https://github.com/TimHanewich
Version: 1.0
Get updates to this code file here: https://github.com/TimHanewich/MicroPython-Collection/blob/master/MPU6050/MPU6050.py
License: MIT License
Copyright 2023 Tim Hanewich
Permission is hereby granted, free of charge, to any person obtaining a copy of this software and associated documentation files (the “Software”), to deal in the Software without restriction, including without limitation the rights to use, copy, modify, merge, publish, distribute, sublicense, and/or sell copies of the Software, and to permit persons to whom the Software is furnished to do so, subject to the following conditions:
The above copyright notice and this permission notice shall be included in all copies or substantial portions of the Software.
THE SOFTWARE IS PROVIDED “AS IS”, WITHOUT WARRANTY OF ANY KIND, EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.
"""
import machine
class MPU6050:
"""Class for reading gyro rates and acceleration data from an MPU-6050 module via I2C."""
def __init__(self, i2c:machine.I2C, address:int = 0x68):
"""
Creates a new MPU6050 class for reading gyro rates and acceleration data.
:param i2c: A setup I2C module of the machine module.
:param address: The I2C address of the MPU-6050 you are using (0x68 is the default).
"""
self.address = address
self.i2c = i2c
def wake(self) -> None:
"""Wake up the MPU-6050."""
self.i2c.writeto_mem(self.address, 0x6B, bytes([0x01]))
def sleep(self) -> None:
"""Places MPU-6050 in sleep mode (low power consumption). Stops the internal reading of new data. Any calls to get gyro or accel data while in sleep mode will remain unchanged - the data is not being updated internally within the MPU-6050!"""
self.i2c.writeto_mem(self.address, 0x6B, bytes([0x40]))
def who_am_i(self) -> int:
"""Returns the address of the MPU-6050 (ensure it is working)."""
return self.i2c.readfrom_mem(self.address, 0x75, 1)[0]
def read_temperature(self) -> float:
"""Reads the temperature, in celsius, of the onboard temperature sensor of the MPU-6050."""
data = self.i2c.readfrom_mem(self.address, 0x41, 2)
raw_temp:float = self._translate_pair(data[0], data[1])
temp:float = (raw_temp / 340.0) + 36.53
return temp
def read_gyro_range(self) -> int:
"""Reads the gyroscope range setting."""
return self._hex_to_index(self.i2c.readfrom_mem(self.address, 0x1B, 1)[0])
def write_gyro_range(self, range:int) -> None:
"""Sets the gyroscope range setting."""
self.i2c.writeto_mem(self.address, 0x1B, bytes([self._index_to_hex(range)]))
def read_gyro_data(self) -> tuple[float, float, float]:
"""Read the gyroscope data, in a (x, y, z) tuple."""
# set the modified based on the gyro range (need to divide to calculate)
gr:int = self.read_gyro_range()
modifier:float = None
if gr == 0:
modifier = 131.0
elif gr == 1:
modifier = 65.5
elif gr == 2:
modifier = 32.8
elif gr == 3:
modifier = 16.4
# read data
data = self.i2c.readfrom_mem(self.address, 0x43, 6) # read 6 bytes (gyro data)
x:float = (self._translate_pair(data[0], data[1])) / modifier
y:float = (self._translate_pair(data[2], data[3])) / modifier
z:float = (self._translate_pair(data[4], data[5])) / modifier
return (x, y, z)
def read_accel_range(self) -> int:
"""Reads the accelerometer range setting."""
return self._hex_to_index(self.i2c.readfrom_mem(self.address, 0x1C, 1)[0])
def write_accel_range(self, range:int) -> None:
"""Sets the gyro accelerometer setting."""
self.i2c.writeto_mem(self.address, 0x1C, bytes([self._index_to_hex(range)]))
def read_accel_data(self) -> tuple[float, float, float]:
"""Read the accelerometer data, in a (x, y, z) tuple."""
# set the modified based on the gyro range (need to divide to calculate)
ar:int = self.read_accel_range()
modifier:float = None
if ar == 0:
modifier = 16384.0
elif ar == 1:
modifier = 8192.0
elif ar == 2:
modifier = 4096.0
elif ar == 3:
modifier = 2048.0
# read data
data = self.i2c.readfrom_mem(self.address, 0x3B, 6) # read 6 bytes (accel data)
x:float = (self._translate_pair(data[0], data[1])) / modifier
y:float = (self._translate_pair(data[2], data[3])) / modifier
z:float = (self._translate_pair(data[4], data[5])) / modifier
return (x, y, z)
def read_lpf_range(self) -> int:
return self.i2c.readfrom_mem(self.address, 0x1A, 1)[0]
def write_lpf_range(self, range:int) -> None:
"""
Sets low pass filter range.
:param range: Low pass range setting, 0-6. 0 = minimum filter, 6 = maximum filter.
"""
# check range
if range < 0 or range > 6:
raise Exception("Range '" + str(range) + "' is not a valid low pass filter setting.")
self.i2c.writeto_mem(self.address, 0x1A, bytes([range]))
#### UTILITY FUNCTIONS BELOW ####
def _translate_pair(self, high:int, low:int) -> int:
"""Converts a byte pair to a usable value. Borrowed from https://github.com/m-rtijn/mpu6050/blob/0626053a5e1182f4951b78b8326691a9223a5f7d/mpu6050/mpu6050.py#L76C39-L76C39."""
value = (high << 8) + low
if value >= 0x8000:
value = -((65535 - value) + 1)
return value
def _hex_to_index(self, range:int) -> int:
"""Converts a hexadecimal range setting to an integer (index), 0-3. This is used for both the gyroscope and accelerometer ranges."""
if range== 0x00:
return 0
elif range == 0x08:
return 1
elif range == 0x10:
return 2
elif range == 0x18:
return 3
else:
raise Exception("Found unknown gyro range setting '" + str(range) + "'")
def _index_to_hex(self, index:int) -> int:
"""Converts an index integer (0-3) to a hexadecimal range setting. This is used for both the gyroscope and accelerometer ranges."""
if index == 0:
return 0x00
elif index == 1:
return 0x08
elif index == 2:
return 0x10
elif index == 3:
return 0x18
else:
raise Exception("Range index '" + index + "' invalid. Must be 0-3.")

648
python-controller/VL53L0X.py
View file

@ -1,648 +0,0 @@
from micropython import const
import ustruct
import utime
from machine import Timer
import time
_IO_TIMEOUT = 1000
_SYSRANGE_START = const(0x00)
_EXTSUP_HV = const(0x89)
_MSRC_CONFIG = const(0x60)
_FINAL_RATE_RTN_LIMIT = const(0x44)
_SYSTEM_SEQUENCE = const(0x01)
_SPAD_REF_START = const(0x4f)
_SPAD_ENABLES = const(0xb0)
_REF_EN_START_SELECT = const(0xb6)
_SPAD_NUM_REQUESTED = const(0x4e)
_INTERRUPT_GPIO = const(0x0a)
_INTERRUPT_CLEAR = const(0x0b)
_GPIO_MUX_ACTIVE_HIGH = const(0x84)
_RESULT_INTERRUPT_STATUS = const(0x13)
_RESULT_RANGE_STATUS = const(0x14)
_OSC_CALIBRATE = const(0xf8)
_MEASURE_PERIOD = const(0x04)
SYSRANGE_START = 0x00
SYSTEM_THRESH_HIGH = 0x0C
SYSTEM_THRESH_LOW = 0x0E
SYSTEM_SEQUENCE_CONFIG = 0x01
SYSTEM_RANGE_CONFIG = 0x09
SYSTEM_INTERMEASUREMENT_PERIOD = 0x04
SYSTEM_INTERRUPT_CONFIG_GPIO = 0x0A
GPIO_HV_MUX_ACTIVE_HIGH = 0x84
SYSTEM_INTERRUPT_CLEAR = 0x0B
RESULT_INTERRUPT_STATUS = 0x13
RESULT_RANGE_STATUS = 0x14
RESULT_CORE_AMBIENT_WINDOW_EVENTS_RTN = 0xBC
RESULT_CORE_RANGING_TOTAL_EVENTS_RTN = 0xC0
RESULT_CORE_AMBIENT_WINDOW_EVENTS_REF = 0xD0
RESULT_CORE_RANGING_TOTAL_EVENTS_REF = 0xD4
RESULT_PEAK_SIGNAL_RATE_REF = 0xB6
ALGO_PART_TO_PART_RANGE_OFFSET_MM = 0x28
I2C_SLAVE_DEVICE_ADDRESS = 0x8A
MSRC_CONFIG_CONTROL = 0x60
PRE_RANGE_CONFIG_MIN_SNR = 0x27
PRE_RANGE_CONFIG_VALID_PHASE_LOW = 0x56
PRE_RANGE_CONFIG_VALID_PHASE_HIGH = 0x57
PRE_RANGE_MIN_COUNT_RATE_RTN_LIMIT = 0x64
FINAL_RANGE_CONFIG_MIN_SNR = 0x67
FINAL_RANGE_CONFIG_VALID_PHASE_LOW = 0x47
FINAL_RANGE_CONFIG_VALID_PHASE_HIGH = 0x48
FINAL_RANGE_CONFIG_MIN_COUNT_RATE_RTN_LIMIT = 0x44
PRE_RANGE_CONFIG_SIGMA_THRESH_HI = 0x61
PRE_RANGE_CONFIG_SIGMA_THRESH_LO = 0x62
PRE_RANGE_CONFIG_VCSEL_PERIOD = 0x50
PRE_RANGE_CONFIG_TIMEOUT_MACROP_HI = 0x51
PRE_RANGE_CONFIG_TIMEOUT_MACROP_LO = 0x52
SYSTEM_HISTOGRAM_BIN = 0x81
HISTOGRAM_CONFIG_INITIAL_PHASE_SELECT = 0x33
HISTOGRAM_CONFIG_READOUT_CTRL = 0x55
FINAL_RANGE_CONFIG_VCSEL_PERIOD = 0x70
FINAL_RANGE_CONFIG_TIMEOUT_MACROP_HI = 0x71
FINAL_RANGE_CONFIG_TIMEOUT_MACROP_LO = 0x72
CROSSTALK_COMPENSATION_PEAK_RATE_MCPS = 0x20
MSRC_CONFIG_TIMEOUT_MACROP = 0x46
SOFT_RESET_GO2_SOFT_RESET_N = 0xBF
IDENTIFICATION_MODEL_ID = 0xC0
IDENTIFICATION_REVISION_ID = 0xC2
OSC_CALIBRATE_VAL = 0xF8
GLOBAL_CONFIG_VCSEL_WIDTH = 0x32
GLOBAL_CONFIG_SPAD_ENABLES_REF_0 = 0xB0
GLOBAL_CONFIG_SPAD_ENABLES_REF_1 = 0xB1
GLOBAL_CONFIG_SPAD_ENABLES_REF_2 = 0xB2
GLOBAL_CONFIG_SPAD_ENABLES_REF_3 = 0xB3
GLOBAL_CONFIG_SPAD_ENABLES_REF_4 = 0xB4
GLOBAL_CONFIG_SPAD_ENABLES_REF_5 = 0xB5
GLOBAL_CONFIG_REF_EN_START_SELECT = 0xB6
DYNAMIC_SPAD_NUM_REQUESTED_REF_SPAD = 0x4E
DYNAMIC_SPAD_REF_EN_START_OFFSET = 0x4F
POWER_MANAGEMENT_GO1_POWER_FORCE = 0x80
VHV_CONFIG_PAD_SCL_SDA__EXTSUP_HV = 0x89
ALGO_PHASECAL_LIM = 0x30
ALGO_PHASECAL_CONFIG_TIMEOUT = 0x30
class TimeoutError(RuntimeError):
pass
class VL53L0X:
def __init__(self, i2c, address=0x29):
self.i2c = i2c
self.address = address
self.init()
self._started = False
self.measurement_timing_budget_us = 0
self.set_measurement_timing_budget(self.measurement_timing_budget_us)
self.enables = {"tcc": 0,
"dss": 0,
"msrc": 0,
"pre_range": 0,
"final_range": 0}
self.timeouts = {"pre_range_vcsel_period_pclks": 0,
"msrc_dss_tcc_mclks": 0,
"msrc_dss_tcc_us": 0,
"pre_range_mclks": 0,
"pre_range_us": 0,
"final_range_vcsel_period_pclks": 0,
"final_range_mclks": 0,
"final_range_us": 0
}
self.vcsel_period_type = ["VcselPeriodPreRange", "VcselPeriodFinalRange"]
def _registers(self, register, values=None, struct='B'):
if values is None:
size = ustruct.calcsize(struct)
data = self.i2c.readfrom_mem(self.address, register, size)
values = ustruct.unpack(struct, data)
return values
data = ustruct.pack(struct, *values)
self.i2c.writeto_mem(self.address, register, data)
def _register(self, register, value=None, struct='B'):
if value is None:
return self._registers(register, struct=struct)[0]
self._registers(register, (value,), struct=struct)
def _flag(self, register=0x00, bit=0, value=None):
data = self._register(register)
mask = 1 << bit
if value is None:
return bool(data & mask)
elif value:
data |= mask
else:
data &= ~mask
self._register(register, data)
def _config(self, *config):
for register, value in config:
self._register(register, value)
def init(self, power2v8=True):
self._flag(_EXTSUP_HV, 0, power2v8)
# I2C standard mode
self._config(
(0x88, 0x00),
(0x80, 0x01),
(0xff, 0x01),
(0x00, 0x00),
)
self._stop_variable = self._register(0x91)
self._config(
(0x00, 0x01),
(0xff, 0x00),
(0x80, 0x00),
)
# disable signal_rate_msrc and signal_rate_pre_range limit checks
self._flag(_MSRC_CONFIG, 1, True)
self._flag(_MSRC_CONFIG, 4, True)
# rate_limit = 0.25
self._register(_FINAL_RATE_RTN_LIMIT, int(0.1 * (1 << 7)),
struct='>H')
self._register(_SYSTEM_SEQUENCE, 0xff)
spad_count, is_aperture = self._spad_info()
spad_map = bytearray(self._registers(_SPAD_ENABLES, struct='6B'))
# set reference spads
self._config(
(0xff, 0x01),
(_SPAD_REF_START, 0x00),
(_SPAD_NUM_REQUESTED, 0x2c),
(0xff, 0x00),
(_REF_EN_START_SELECT, 0xb4),
)
spads_enabled = 0
for i in range(48):
if i < 12 and is_aperture or spads_enabled >= spad_count:
spad_map[i // 8] &= ~(1 << (i >> 2))
elif spad_map[i // 8] & (1 << (i >> 2)):
spads_enabled += 1
self._registers(_SPAD_ENABLES, spad_map, struct='6B')
self._config(
(0xff, 0x01),
(0x00, 0x00),
(0xff, 0x00),
(0x09, 0x00),
(0x10, 0x00),
(0x11, 0x00),
(0x24, 0x01),
(0x25, 0xFF),
(0x75, 0x00),
(0xFF, 0x01),
(0x4E, 0x2C),
(0x48, 0x00),
(0x30, 0x20),
(0xFF, 0x00),
(0x30, 0x09),
(0x54, 0x00),
(0x31, 0x04),
(0x32, 0x03),
(0x40, 0x83),
(0x46, 0x25),
(0x60, 0x00),
(0x27, 0x00),
(0x50, 0x06),
(0x51, 0x00),
(0x52, 0x96),
(0x56, 0x08),
(0x57, 0x30),
(0x61, 0x00),
(0x62, 0x00),
(0x64, 0x00),
(0x65, 0x00),
(0x66, 0xA0),
(0xFF, 0x01),
(0x22, 0x32),
(0x47, 0x14),
(0x49, 0xFF),
(0x4A, 0x00),
(0xFF, 0x00),
(0x7A, 0x0A),
(0x7B, 0x00),
(0x78, 0x21),
(0xFF, 0x01),
(0x23, 0x34),
(0x42, 0x00),
(0x44, 0xFF),
(0x45, 0x26),
(0x46, 0x05),
(0x40, 0x40),
(0x0E, 0x06),
(0x20, 0x1A),
(0x43, 0x40),
(0xFF, 0x00),
(0x34, 0x03),
(0x35, 0x44),
(0xFF, 0x01),
(0x31, 0x04),
(0x4B, 0x09),
(0x4C, 0x05),
(0x4D, 0x04),
(0xFF, 0x00),
(0x44, 0x00),
(0x45, 0x20),
(0x47, 0x08),
(0x48, 0x28),
(0x67, 0x00),
(0x70, 0x04),
(0x71, 0x01),
(0x72, 0xFE),
(0x76, 0x00),
(0x77, 0x00),
(0xFF, 0x01),
(0x0D, 0x01),
(0xFF, 0x00),
(0x80, 0x01),
(0x01, 0xF8),
(0xFF, 0x01),
(0x8E, 0x01),
(0x00, 0x01),
(0xFF, 0x00),
(0x80, 0x00),
)
self._register(_INTERRUPT_GPIO, 0x04)
self._flag(_GPIO_MUX_ACTIVE_HIGH, 4, False)
self._register(_INTERRUPT_CLEAR, 0x01)
# XXX Need to implement this.
# budget = self._timing_budget()
# self._register(_SYSTEM_SEQUENCE, 0xe8)
# self._timing_budget(budget)
self._register(_SYSTEM_SEQUENCE, 0x01)
self._calibrate(0x40)
self._register(_SYSTEM_SEQUENCE, 0x02)
self._calibrate(0x00)
self._register(_SYSTEM_SEQUENCE, 0xe8)
def _spad_info(self):
self._config(
(0x80, 0x01),
(0xff, 0x01),
(0x00, 0x00),
(0xff, 0x06),
)
self._flag(0x83, 3, True)
self._config(
(0xff, 0x07),
(0x81, 0x01),
(0x80, 0x01),
(0x94, 0x6b),
(0x83, 0x00),
)
for timeout in range(_IO_TIMEOUT):
if self._register(0x83):
break
utime.sleep_ms(1)
else:
raise TimeoutError()
self._config(
(0x83, 0x01),
)
value = self._register(0x92)
self._config(
(0x81, 0x00),
(0xff, 0x06),
)
self._flag(0x83, 3, False)
self._config(
(0xff, 0x01),
(0x00, 0x01),
(0xff, 0x00),
(0x80, 0x00),
)
count = value & 0x7f
is_aperture = bool(value & 0b10000000)
return count, is_aperture
def _calibrate(self, vhv_init_byte):
self._register(_SYSRANGE_START, 0x01 | vhv_init_byte)
for timeout in range(_IO_TIMEOUT):
if self._register(_RESULT_INTERRUPT_STATUS) & 0x07:
break
utime.sleep_ms(1)
else:
raise TimeoutError()
self._register(_INTERRUPT_CLEAR, 0x01)
self._register(_SYSRANGE_START, 0x00)
def start(self, period=0):
self._config(
(0x80, 0x01),
(0xFF, 0x01),
(0x00, 0x00),
(0x91, self._stop_variable),
(0x00, 0x01),
(0xFF, 0x00),
(0x80, 0x00),
)
if period:
oscilator = self._register(_OSC_CALIBRATE, struct='>H')
if oscilator:
period *= oscilator
self._register(_MEASURE_PERIOD, period, struct='>H')
self._register(_SYSRANGE_START, 0x04)
else:
self._register(_SYSRANGE_START, 0x02)
self._started = True
def stop(self):
self._register(_SYSRANGE_START, 0x01)
self._config(
(0xFF, 0x01),
(0x00, 0x00),
(0x91, self._stop_variable),
(0x00, 0x01),
(0xFF, 0x00),
)
self._started = False
def read(self):
if not self._started:
self._config(
(0x80, 0x01),
(0xFF, 0x01),
(0x00, 0x00),
(0x91, self._stop_variable),
(0x00, 0x01),
(0xFF, 0x00),
(0x80, 0x00),
(_SYSRANGE_START, 0x01),
)
for timeout in range(_IO_TIMEOUT):
if not self._register(_SYSRANGE_START) & 0x01:
break
utime.sleep_ms(1)
else:
raise TimeoutError()
for timeout in range(_IO_TIMEOUT):
if self._register(_RESULT_INTERRUPT_STATUS) & 0x07:
break
utime.sleep_ms(1)
else:
raise TimeoutError()
value = self._register(_RESULT_RANGE_STATUS + 10, struct='>H')
self._register(_INTERRUPT_CLEAR, 0x01)
return value
def set_signal_rate_limit(self, limit_Mcps):
if limit_Mcps < 0 or limit_Mcps > 511.99:
return False
self._register(0x44, limit_Mcps * (1 << 7))
return True
def decode_Vcsel_period(self, reg_val):
return (((reg_val) + 1) << 1)
def encode_Vcsel_period(self, period_pclks):
return (((period_pclks) >> 1) - 1)
def set_Vcsel_pulse_period(self, type, period_pclks):
vcsel_period_reg = self.encode_Vcsel_period(period_pclks)
self.get_sequence_step_enables()
self.get_sequence_step_timeouts()
if type == self.vcsel_period_type[0]:
if period_pclks == 12:
self._register(PRE_RANGE_CONFIG_VALID_PHASE_HIGH, 0x18)
elif period_pclks == 14:
self._register(PRE_RANGE_CONFIG_VALID_PHASE_HIGH, 0x30)
elif period_pclks == 16:
self._register(PRE_RANGE_CONFIG_VALID_PHASE_HIGH, 0x40)
elif period_pclks == 18:
self._register(PRE_RANGE_CONFIG_VALID_PHASE_HIGH, 0x50)
else:
return False
self._register(PRE_RANGE_CONFIG_VALID_PHASE_LOW, 0x08)
self._register(PRE_RANGE_CONFIG_VCSEL_PERIOD, vcsel_period_reg)
new_pre_range_timeout_mclks = self.timeout_microseconds_to_Mclks(self.timeouts["pre_range_us"],
period_pclks)
self._register(PRE_RANGE_CONFIG_TIMEOUT_MACROP_HI, self.encode_timeout(new_pre_range_timeout_mclks))
new_msrc_timeout_mclks = self.timeout_microseconds_to_Mclks(self.timeouts["msrc_dss_tcc_us"],
period_pclks)
self._register(MSRC_CONFIG_TIMEOUT_MACROP, 255 if new_msrc_timeout_mclks > 256 else (new_msrc_timeout_mclks - 1))
elif type == self.vcsel_period_type[1]:
if period_pclks == 8:
self._register(FINAL_RANGE_CONFIG_VALID_PHASE_HIGH, 0x10)
self._register(FINAL_RANGE_CONFIG_VALID_PHASE_LOW, 0x08)
self._register(GLOBAL_CONFIG_VCSEL_WIDTH, 0x02)
self._(ALGO_PHASECAL_CONFIG_TIMEOUT, 0x0C)
self._register(0xFF, 0x01)
self._register(ALGO_PHASECAL_LIM, 0x30)
self._register(0xFF, 0x00)
elif period_pclks == 10:
self._register(FINAL_RANGE_CONFIG_VALID_PHASE_HIGH, 0x28)
self._register(FINAL_RANGE_CONFIG_VALID_PHASE_LOW, 0x08)
self._register(GLOBAL_CONFIG_VCSEL_WIDTH, 0x03)
self._register(ALGO_PHASECAL_CONFIG_TIMEOUT, 0x09)
self._register(0xFF, 0x01)
self._register(ALGO_PHASECAL_LIM, 0x20)
self._register(0xFF, 0x00)
elif period_pclks == 12:
self._register(FINAL_RANGE_CONFIG_VALID_PHASE_HIGH, 0x38)
self._register(FINAL_RANGE_CONFIG_VALID_PHASE_LOW, 0x08)
self._register(GLOBAL_CONFIG_VCSEL_WIDTH, 0x03)
self._register(ALGO_PHASECAL_CONFIG_TIMEOUT, 0x08)
self._register(0xFF, 0x01)
self._register(ALGO_PHASECAL_LIM, 0x20)
self._register(0xFF, 0x00)
elif period_pclks == 14:
self._register(FINAL_RANGE_CONFIG_VALID_PHASE_HIGH, 0x48)
self._register(FINAL_RANGE_CONFIG_VALID_PHASE_LOW, 0x08)
self._register(GLOBAL_CONFIG_VCSEL_WIDTH, 0x03)
self._register(ALGO_PHASECAL_CONFIG_TIMEOUT, 0x07)
self._register(0xFF, 0x01)
self._register(ALGO_PHASECAL_LIM, 0x20)
self._register(0xFF, 0x00)
else:
return False
self._register(FINAL_RANGE_CONFIG_VCSEL_PERIOD, vcsel_period_reg)
new_final_range_timeout_mclks = self.timeout_microseconds_to_Mclks(self.timeouts["final_range_us"], period_pclks)
if self.enables["pre_range"]:
new_final_range_timeout_mclks += 1
self._register(FINAL_RANGE_CONFIG_TIMEOUT_MACROP_HI, self.encode_timeout(new_final_range_timeout_mclks))
else:
return False
self.set_measurement_timing_budget(self.measurement_timing_budget_us)
sequence_config = self._register(SYSTEM_SEQUENCE_CONFIG)
self._register(SYSTEM_SEQUENCE_CONFIG, 0x02)
self.perform_single_ref_calibration(0x0)
self._register(SYSTEM_SEQUENCE_CONFIG, sequence_config)
return True
def get_sequence_step_enables(self):
sequence_config = self._register(0x01)
self.enables["tcc"] = (sequence_config >> 4) & 0x1
self.enables["dss"] = (sequence_config >> 3) & 0x1
self.enables["msrc"] = (sequence_config >> 2) & 0x1
self.enables["pre_range"] = (sequence_config >> 6) & 0x1
self.enables["final_range"] = (sequence_config >> 7) & 0x1
def get_vcsel_pulse_period(self, type):
if type == self.vcsel_period_type[0]:
return self.decode_Vcsel_period(0x50)
elif type == self.vcsel_period_type[1]:
return self.decode_Vcsel_period(0x70)
else:
return 255
def get_sequence_step_timeouts(self):
self.timeouts["pre_range_vcsel_period_pclks"] = self.get_vcsel_pulse_period(self.vcsel_period_type[0])
self.timeouts["msrc_dss_tcc_mclks"] = int(self._register(MSRC_CONFIG_TIMEOUT_MACROP)) + 1
self.timeouts["msrc_dss_tcc_us"] = self.timeout_Mclks_to_microseconds(self.timeouts["msrc_dss_tcc_mclks"],
self.timeouts[
"pre_range_vcsel_period_pclks"])
self.timeouts["pre_range_mclks"] = self.decode_timeout(PRE_RANGE_CONFIG_TIMEOUT_MACROP_HI)
self.timeouts["pre_range_us"] = self.timeout_Mclks_to_microseconds(self.timeouts["pre_range_mclks"],
self.timeouts[
"pre_range_vcsel_period_pclks"])
self.timeouts["final_range_vcsel_period_pclks"] = self.get_vcsel_pulse_period(self.vcsel_period_type[1])
self.timeouts["final_range_mclks"] = self.decode_timeout(self._register(FINAL_RANGE_CONFIG_TIMEOUT_MACROP_HI))
if self.enables["pre_range"]:
self.timeouts["final_range_mclks"] -= self.timeouts["pre_range_mclks"]
self.timeouts["final_range_us"] = self.timeout_Mclks_to_microseconds(self.timeouts["final_range_mclks"],
self.timeouts[
"final_range_vcsel_period_pclks"])
def timeout_Mclks_to_microseconds(self, timeout_period_mclks, vcsel_period_pclks):
macro_period_ns = self.calc_macro_period(vcsel_period_pclks)
return ((timeout_period_mclks * macro_period_ns) + (macro_period_ns / 2)) / 1000
def timeout_microseconds_to_Mclks(self, timeout_period_us, vcsel_period_pclks):
macro_period_ns = self.calc_macro_period(vcsel_period_pclks)
return (((timeout_period_us * 1000) + (macro_period_ns / 2)) / macro_period_ns)
def calc_macro_period(self, vcsel_period_pclks):
return (((2304 * (vcsel_period_pclks) * 1655) + 500) / 1000)
def decode_timeout(self, reg_val):
return ((reg_val & 0x00FF) << ((reg_val & 0xFF00) >> 8)) + 1
def encode_timeout(self, timeout_mclks):
timeout_mclks = int(timeout_mclks)
ls_byte = 0
ms_byte = 0
if timeout_mclks > 0:
ls_byte = timeout_mclks - 1
while (ls_byte & 0xFFFFFF00) > 0:
ls_byte >>= 1
ms_byte += 1
return (ms_byte << 8) or (ls_byte & 0xFF)
else:
return 0
def set_measurement_timing_budget(self, budget_us):
start_overhead = 1320
end_overhead = 960
msrc_overhead = 660
tcc_overhead = 590
dss_overhead = 690
pre_range_overhead = 660
final_range_overhead = 550
min_timing_budget = 20000
if budget_us < min_timing_budget:
return False
used_budget_us = start_overhead + end_overhead
self.get_sequence_step_enables()
self.get_sequence_step_timeouts()
if self.enables["tcc"]:
used_budget_us += self.timeouts["msrc_dss_tcc_us"] + tcc_overhead
if self.enables["dss"]:
used_budget_us += 2* self.timeouts["msrc_dss_tcc_us"] + dss_overhead
if self.enables["msrc"]:
used_budget_us += self.timeouts["msrc_dss_tcc_us"] + msrc_overhead
if self.enables["pre_range"]:
used_budget_us += self.timeouts["pre_range_us"] + pre_range_overhead
if self.enables["final_range"]:
used_budget_us += final_range_overhead
if used_budget_us > budget_us:
return False
final_range_timeout_us = budget_us - used_budget_us
final_range_timeout_mclks = self.timeout_microseconds_to_Mclks(final_range_timeout_us, self.timeouts["final_range_vcsel_period_pclks"])
if self.enables["pre_range"]:
final_range_timeout_mclks += self.timeouts["pre_range_mclks"]
self._register(FINAL_RANGE_CONFIG_TIMEOUT_MACROP_HI, self.encode_timeout(final_range_timeout_mclks))
self.measurement_timing_budget_us = budget_us
return True
def perform_single_ref_calibration(self, vhv_init_byte):
chrono = Timer.Chrono()
self._register(SYSRANGE_START, 0x01|vhv_init_byte)
chrono.start()
while self._register((RESULT_INTERRUPT_STATUS & 0x07) == 0):
time_elapsed = chrono.read_ms()
if time_elapsed > _IO_TIMEOUT:
return False
self._register(SYSTEM_INTERRUPT_CLEAR, 0x01)
self._register(SYSRANGE_START, 0x00)
return True

64
python-controller/main.py
View file

@ -1,64 +0,0 @@
import time
import network
import socket
from machine import Pin
from machine import I2C
import VL53L0X
import MPU6050
ssid = "cruise"
password = "rust"
ap = network.WLAN(network.AP_IF)
ap.config(essid=ssid)
ap.active(True)
i2c_tof = I2C(1, scl=Pin(27), sda=Pin(26))
tof = False
try:
tof = VL53L0X.VL53L0X(i2c_tof, address=0x29)
except Exception as e:
tof = False
i2c_gyro = machine.I2C(0, sda=machine.Pin(20), scl=machine.Pin(21));
mpu = False
try:
mpu = MPU6050.MPU6050(i2c_gyro, address = 0x68)
mpu.wake()
except Exception as e:
mpu = False
while (ap.active() == False):
pass
print("network active")
print("ip: "+ap.ifconfig()[0])
s = socket.socket(socket.AF_INET, socket.SOCK_STREAM)
s.bind(('', 1337))
s.listen(5)
while True:
try:
cl, addr = s.accept()
request = cl.recv(1024)
print(request)
cl.send("recvd")
cl.close()
except OSError as e:
cl.close()
print("closed")
pass
while True:
if tof:
tof.start()
print(tof.read())
if mpu:
gyro = mpu.read_gyro_data()
accel = mpu.read_accel_data()
print("Gyro: " + str(gyro) + ", Accel: " + str(accel))
time.sleep(0.1)

2158
simcontroller/Cargo.lock generated
View file

File diff suppressed because it is too large Load diff

10
simcontroller/Cargo.toml
View file

@ -1,10 +0,0 @@
[package]
name = "simcontroller"
version = "0.1.0"
edition = "2021"
[dependencies]
const-tweaker = "0.3.1"
opencv = {version = "0.92.3", features = ["clang-runtime"]}
thiserror = "1.0.63"
webots = { git = "https://github.com/katharostech/webots-rust" }

4
simcontroller/Justfile
View file

@ -1,4 +0,0 @@
debug:
cargo build
cp target/debug/simcontroller ../simulation/controllers/cruise/cruise

122
simcontroller/src/camera.rs
View file

@ -1,122 +0,0 @@
// MIT from HULKs
// https://github.com/HULKs/webots-rs/blob/5c4c36012c8cc0f0fe53f67b19294605a6867649/src/camera.rs
use std::{ffi::CString, slice::from_raw_parts};
use thiserror::Error;
use webots::{robot::Device, sys::{
wb_camera_disable, wb_camera_enable, wb_camera_get_exposure, wb_camera_get_focal_distance,
wb_camera_get_focal_length, wb_camera_get_fov, wb_camera_get_height, wb_camera_get_image,
wb_camera_get_max_focal_distance, wb_camera_get_max_fov, wb_camera_get_min_focal_distance,
wb_camera_get_min_fov, wb_camera_get_near, wb_camera_get_sampling_period, wb_camera_get_width,
wb_camera_has_recognition, wb_camera_save_image, wb_camera_set_exposure,
wb_camera_set_focal_distance, wb_camera_set_fov, wb_device_get_node_type, WbDeviceTag,
WbNodeType_WB_NODE_CAMERA,
}};
#[derive(Debug, Error)]
pub enum CameraError {
#[error("failed to get image: image data is NULL")]
ImageDataIsNull,
}
pub struct Camera(WbDeviceTag);
impl Camera {
pub(crate) fn new(device: &str) -> Self {
let device = Device::new(device).tag;
assert_eq!(WbNodeType_WB_NODE_CAMERA, unsafe {
wb_device_get_node_type(device)
});
Self(device)
}
pub fn enable(&self, sampling_period: i32) {
unsafe { wb_camera_enable(self.0, sampling_period) }
}
pub fn disable(&self) {
unsafe { wb_camera_disable(self.0) }
}
pub fn get_sampling_period(&self) -> i32 {
unsafe { wb_camera_get_sampling_period(self.0) }
}
pub fn get_image(&self) -> Result<&[u8], CameraError> {
let width = self.get_width();
let height = self.get_height();
unsafe {
let image = wb_camera_get_image(self.0);
if image.is_null() {
return Err(CameraError::ImageDataIsNull);
}
Ok(from_raw_parts(image, (width * height * 4) as usize))
}
}
pub fn get_width(&self) -> i32 {
unsafe { wb_camera_get_width(self.0) }
}
pub fn get_height(&self) -> i32 {
unsafe { wb_camera_get_height(self.0) }
}
pub fn get_fov(&self) -> f64 {
unsafe { wb_camera_get_fov(self.0) }
}
pub fn get_max_fov(&self) -> f64 {
unsafe { wb_camera_get_max_fov(self.0) }
}
pub fn get_min_fov(&self) -> f64 {
unsafe { wb_camera_get_min_fov(self.0) }
}
pub fn set_fov(&self, fov: f64) {
unsafe { wb_camera_set_fov(self.0, fov) }
}
pub fn get_exposure(&self) -> f64 {
unsafe { wb_camera_get_exposure(self.0) }
}
pub fn set_exposure(&self, exposure: f64) {
unsafe { wb_camera_set_exposure(self.0, exposure) }
}
pub fn get_focal_length(&self) -> f64 {
unsafe { wb_camera_get_focal_length(self.0) }
}
pub fn get_focal_distance(&self) -> f64 {
unsafe { wb_camera_get_focal_distance(self.0) }
}
pub fn get_max_focal_distance(&self) -> f64 {
unsafe { wb_camera_get_max_focal_distance(self.0) }
}
pub fn get_min_focal_distance(&self) -> f64 {
unsafe { wb_camera_get_min_focal_distance(self.0) }
}
pub fn set_focal_distance(&self, focal_distance: f64) {
unsafe { wb_camera_set_focal_distance(self.0, focal_distance) }
}
pub fn get_near(&self) -> f64 {
unsafe { wb_camera_get_near(self.0) }
}
pub fn save_image(&self, filename: &str, quality: i32) -> i32 {
let filename = CString::new(filename).expect("CString::new failed");
unsafe { wb_camera_save_image(self.0, filename.as_ptr(), quality) }
}
pub fn has_recognition(&self) -> bool {
unsafe { wb_camera_has_recognition(self.0) != 0 }
}
}

193
simcontroller/src/main.rs
View file

@ -1,193 +0,0 @@
use camera::Camera;
use opencv::{core::{bitwise_and, bitwise_and_def, bitwise_or_def, in_range, Kernel, KeyPoint, KeyPointTraitConst, Mat, MatTraitConst, MatTraitConstManual, Ptr, Size, Size_, VecN, Vector, CV_64F, CV_8UC4}, features2d::{draw_matches_def, BFMatcher, AKAZE, ORB}, gapi::{bgr2_luv, erode_def, GMat}, highgui::{self, MouseCallback}, imgcodecs::ImreadModes, imgproc::{self, cvt_color_def, gaussian_blur, gaussian_blur_def, get_structuring_element_def, sobel_def, MorphShapes, COLOR_BGR2GRAY, COLOR_BGR2HLS}, prelude::{DescriptorMatcherTrait, DescriptorMatcherTraitConst, Feature2DTrait}, ximgproc::erode};
use webots::prelude::*;
use std::{f64::consts::TAU, time::Duration};
mod camera;
const TIME_STEP: Duration = Duration::from_millis(20);
const MAX_SPEED: f64 = TAU;
// const tweaker only supports floating point, these are mapped
#[const_tweaker::tweak]
const LOWERB_H: f32 = 18. / 180.;
#[const_tweaker::tweak]
const LOWERB_L: f32 = 23. / 255.;
#[const_tweaker::tweak]
const LOWERB_S: f32 = 36. / 255.;
#[const_tweaker::tweak]
const UPPERB_H: f32 = 92. / 180.;
#[const_tweaker::tweak]
const UPPERB_L: f32 = 151. / 255.;
#[const_tweaker::tweak]
const UPPERB_S: f32 = 169. / 255.;
struct MyRobot {
camera: Camera,
left_motor: Motor,
right_motor: Motor,
window: &'static str,
orb_detector: Ptr<ORB>,
matcher: BFMatcher,
last_frame_descriptors: Option<Mat>,
last_frame_keypoints: Option<Vector<KeyPoint>>,
last_image: Option<Mat>,
direction: f32,
rotational_velocity: f32,
}
impl Robot for MyRobot {
fn time_step(&self) -> Duration {
TIME_STEP
}
fn init() -> Self {
let camera = Camera::new("camera7");
let left_motor = Motor::new("motorl");
let right_motor = Motor::new("motorr");
left_motor.set_position(f64::INFINITY);
right_motor.set_position(f64::INFINITY);
left_motor.set_velocity(0.1 * MAX_SPEED);
right_motor.set_velocity(0.1 * MAX_SPEED);
let window = "processed";
//highgui::named_window(window, 1).unwrap();
//highgui::set_mouse_callback(window, |x,y,_,_| {dbg!(x+y);}).unwrap();
let orb_detector = ORB::create_def().unwrap();
let matcher = opencv::features2d::BFMatcher::new_def().unwrap();
//let last_frame_descriptors = opencv::core::Mat::default();
Self {
camera,
left_motor,
right_motor,
window,
orb_detector,
matcher,
last_frame_descriptors: None,
last_frame_keypoints: None,
last_image: None,
direction: 0.,
rotational_velocity: 0.,
}
}
fn step(&mut self, _time: StepTime) {
self.camera.enable(20);
if let Ok(img) = self.camera.get_image() { // BGRA
let img = opencv::core::Mat::new_rows_cols_with_bytes::<VecN<u8,4>>(240, 320, &img).unwrap();
//opencv::imgcodecs::imwrite_def("cam.png", &img).unwrap();
//highgui::imshow(self.window, &img).unwrap();
let mut blurred = opencv::core::Mat::default();
let mut edge = opencv::core::Mat::default();
gaussian_blur_def(&img, &mut blurred, Size::new(3, 3), 0.).unwrap();
//sobel_def(&blurred, &mut edge, CV_64F, 0, 2).unwrap();
cvt_color_def(&blurred, &mut edge, COLOR_BGR2HLS).unwrap();
highgui::imshow(self.window, &blurred).unwrap();
let mut mask_a = opencv::core::Mat::default();
let mut mask_b = opencv::core::Mat::default();
let mut mask = opencv::core::Mat::default();
opencv::imgcodecs::imwrite_def("blurred.png", &blurred).unwrap();
println!("Lowrerh {}", LOWERB_H);
let lowerb = dbg!([(*LOWERB_H*255.) as u8,(*LOWERB_L*255.) as u8,(*LOWERB_S*255.) as u8,255]);
let upperb = [(*UPPERB_H*255.) as u8,(*UPPERB_L*255.) as u8,(*UPPERB_S*255.) as u8,255];
in_range(&blurred, &lowerb, &upperb, &mut mask_a).unwrap(); // h low
let gmask = GMat::new(mask_a).unwrap();
let kernel= get_structuring_element_def(MorphShapes::MORPH_CROSS as i32, Size_::new(2, 2)).unwrap();
let eroded = erode_def(&gmask, &kernel).unwrap();
// 103/180
in_range(&blurred, &[0,30,4,255], &[52,209,63,255], &mut mask_b).unwrap(); // hh28
//highgui::imshow("a", &mask_a).unwrap();
//highgui::imshow("b", &mask_b).unwrap();
dbg!(blurred.row(240/2).unwrap().col(320/2).unwrap().data_bytes().unwrap());
let mut range = opencv::core::Mat::default();
//bitwise_or_def(&mask_a, &mask_a, &mut mask).unwrap();
bitwise_and(&img, &img, &mut range, &eroded.into()).unwrap();
highgui::imshow("edge", &range).unwrap();
let mask = opencv::core::Mat::default();
let mut keypoints = opencv::core::Vector::default();
let mut descriptors = opencv::core::Mat::default();
self.orb_detector.detect_and_compute_def(&blurred, &mask, &mut keypoints, &mut descriptors).unwrap();
let mut debugout = opencv::core::Mat::default();
opencv::features2d::draw_keypoints(&img, &keypoints, &mut debugout, opencv::core::VecN([0.,255.,255.,255.]), opencv::features2d::DrawMatchesFlags::DEFAULT).unwrap();
//highgui::imshow("keypoints", &debugout).unwrap();
let mut matches: Vector<opencv::core::DMatch> = Vector::new();
if let Some(mut last_frame_descriptors) = self.last_frame_descriptors.as_ref() {
if let Err(e) = self.matcher.train_match_def(&mut last_frame_descriptors, &mut descriptors, &mut matches) {
eprintln!("no features to match");
}
let matches = matches.iter().filter(|m| m.distance < 105.).collect();
draw_matches_def(self.last_image.as_ref().unwrap(), self.last_frame_keypoints.as_ref().unwrap(), &img , &keypoints, &matches, &mut debugout).unwrap();
highgui::imshow("matches", &debugout).unwrap();
let mut horiz = Vec::new();
for hit in matches {
if hit.distance > 45. {continue;}
let last = self.last_frame_keypoints.as_ref().unwrap().get(hit.query_idx as usize).unwrap();
let current = keypoints.get(hit.train_idx as usize).unwrap();
let dx = last.pt().x - current.pt().x;
horiz.push(dx);
}
if horiz.len() > 0 {
let avg = horiz.iter().sum::<f32>()/horiz.len() as f32;
let lerp = 0.1 + horiz.len() as f32 * 0.05;
let lerp = lerp.max(0.65);
self.rotational_velocity = self.rotational_velocity * (1.-lerp) + avg * lerp;
//self.direction += avg;
}
self.direction += self.rotational_velocity;
}
self.last_frame_descriptors = Some(descriptors);
self.last_frame_keypoints = Some(keypoints);
self.last_image = Some(img.clone_pointee());
}
highgui::poll_key().unwrap();
const DEG_TO_PX: f64 = 320. / 62.2;
let goal = if _time.elapsed.as_secs() % 8 < 4 {
90. * DEG_TO_PX
} else {
0. * DEG_TO_PX
};
let error = goal - self.direction as f64;
let mut error = error * 0.02;
if error.abs() < 0.5 {
error = 0.;
}
dbg!(self.direction);
dbg!(self.rotational_velocity);
let mut left_speed = error * MAX_SPEED;
let mut right_speed = -error * MAX_SPEED;
self.left_motor.set_velocity(left_speed.clamp(-10., 10.));
self.right_motor.set_velocity(right_speed.clamp(-10., 10.));
//self.left_motor.set_velocity(0.);
//self.right_motor.set_velocity(0.);
}
}
fn main() {
println!("Rust controller has started");
webots::run_robot::<MyRobot>();
}