142 lines
4 KiB
Rust
142 lines
4 KiB
Rust
#![no_std]
|
|
#![no_main]
|
|
|
|
use core::sync::atomic::{AtomicBool, AtomicU32, Ordering::Relaxed};
|
|
use defmt::unwrap;
|
|
use embassy_executor::Spawner;
|
|
use embassy_stm32::{
|
|
adc::{Adc, SampleTime},
|
|
gpio::OutputType::PushPull,
|
|
peripherals::{ADC1, PA0, TIM3},
|
|
time::Hertz,
|
|
timer::{
|
|
low_level::CountingMode::EdgeAlignedUp,
|
|
simple_pwm::{PwmPin, SimplePwm},
|
|
},
|
|
Config, Peri,
|
|
};
|
|
use embassy_time::{Duration, Timer};
|
|
use {defmt_rtt as _, panic_probe as _};
|
|
|
|
static SERVO_HZ: Hertz = Hertz::hz(60);
|
|
static SERVO_MIN_USECS: f32 = 490.0;
|
|
static SERVO_MAX_USECS: f32 = 2000.0;
|
|
static SERVO_MIN_DEG: f32 = -90.0;
|
|
static SERVO_MAX_DEG: f32 = 90.0;
|
|
static SERVO_SLOPE: f32 = (SERVO_MAX_USECS - SERVO_MIN_USECS) / (SERVO_MAX_DEG - SERVO_MIN_DEG);
|
|
|
|
static SENSOR_VALUE: AtomicU32 = AtomicU32::new(4095);
|
|
static SHOULD_EXIT: AtomicBool = AtomicBool::new(false);
|
|
|
|
fn degrees2duty(degrees: f32, max_dur: Duration, max_duty: u32) -> u32 {
|
|
let degrees = degrees.clamp(SERVO_MIN_DEG, SERVO_MAX_DEG);
|
|
let micros = 0.5 + SERVO_MIN_USECS + SERVO_SLOPE * (degrees - SERVO_MIN_DEG);
|
|
((micros / max_dur.as_micros() as f32) * max_duty as f32) as u32
|
|
}
|
|
|
|
fn sensor2duty(sensor: u32, max_dur: Duration, max_duty: u32) -> u32 {
|
|
let slope = 180.0 / (4095.0 - 300.0);
|
|
let degrees = SERVO_MIN_DEG + slope * (sensor as f32 - 300.00);
|
|
defmt::debug!("degrees {}", degrees);
|
|
degrees2duty(degrees, max_dur, max_duty)
|
|
}
|
|
|
|
#[embassy_executor::main]
|
|
async fn main(spawner: Spawner) {
|
|
let config = config();
|
|
|
|
let peripherals = embassy_stm32::init(config);
|
|
|
|
let pwm_pin = PwmPin::new(peripherals.PC7, PushPull);
|
|
let pwm = SimplePwm::new(
|
|
peripherals.TIM3,
|
|
None,
|
|
Some(pwm_pin),
|
|
None,
|
|
None,
|
|
SERVO_HZ,
|
|
EdgeAlignedUp,
|
|
);
|
|
|
|
let adc1 = Adc::new(peripherals.ADC1, Default::default());
|
|
|
|
let sensor_input = peripherals.PA0.into();
|
|
spawner.spawn(unwrap!(read_adc1_pa0(adc1, sensor_input)));
|
|
|
|
spawner.spawn(unwrap!(servo_loop(pwm)));
|
|
|
|
// calls the global exit after 30 seconds
|
|
spawner.spawn(unwrap!(exit()));
|
|
}
|
|
|
|
#[embassy_executor::task]
|
|
async fn servo_loop(mut pwm: SimplePwm<'static, TIM3>) {
|
|
let mut pwm_channel = pwm.ch2();
|
|
pwm_channel.enable();
|
|
|
|
let max_duty = pwm_channel.max_duty_cycle();
|
|
let max_dur = Duration::from_hz(SERVO_HZ.0 as u64);
|
|
|
|
let zero = degrees2duty(0.0, max_dur, max_duty);
|
|
pwm_channel.set_duty_cycle(zero);
|
|
Timer::after_millis(500).await;
|
|
|
|
defmt::info!(
|
|
"max duty: {}",
|
|
degrees2duty(SERVO_MAX_DEG, max_dur, max_duty)
|
|
);
|
|
let min_duty = degrees2duty(SERVO_MIN_DEG, max_dur, max_duty);
|
|
defmt::info!("min duty: {}", min_duty);
|
|
|
|
//
|
|
loop {
|
|
if SHOULD_EXIT.load(Relaxed) {
|
|
break;
|
|
}
|
|
let sensor = SENSOR_VALUE.load(Relaxed);
|
|
let duty = sensor2duty(sensor, max_dur, max_duty);
|
|
defmt::info!("duty: {}", duty);
|
|
pwm_channel.set_duty_cycle(duty);
|
|
Timer::after_millis(100).await;
|
|
}
|
|
|
|
pwm_channel.set_duty_cycle(degrees2duty(0.0, max_dur, max_duty));
|
|
Timer::after_millis(600).await;
|
|
|
|
treatbot_5k::exit();
|
|
}
|
|
|
|
#[embassy_executor::task]
|
|
async fn exit() {
|
|
Timer::after_secs(30).await;
|
|
SHOULD_EXIT.store(true, Relaxed);
|
|
}
|
|
|
|
#[embassy_executor::task]
|
|
async fn read_adc1_pa0(mut adc1: Adc<'static, ADC1>, mut channel: Peri<'static, PA0>) {
|
|
loop {
|
|
let val = adc1.blocking_read(&mut channel.reborrow(), SampleTime::CYCLES640_5);
|
|
defmt::info!("sensor val: {}", val);
|
|
SENSOR_VALUE.store(val as u32, Relaxed);
|
|
Timer::after_millis(100).await;
|
|
}
|
|
}
|
|
|
|
fn config() -> Config {
|
|
use embassy_stm32::rcc::{mux, Pll, PllMul, PllPreDiv, PllRDiv, PllSource, Sysclk};
|
|
|
|
let mut config = Config::default();
|
|
config.rcc.pll = Some(Pll {
|
|
source: PllSource::HSI,
|
|
prediv: PllPreDiv::DIV4,
|
|
mul: PllMul::MUL85,
|
|
divp: None,
|
|
divq: None,
|
|
// Main system clock at 170 MHz
|
|
divr: Some(PllRDiv::DIV2),
|
|
});
|
|
config.rcc.mux.adc12sel = mux::Adcsel::SYS;
|
|
config.rcc.sys = Sysclk::PLL1_R;
|
|
|
|
config
|
|
}
|