moves the servo in response to sensor output
This commit is contained in:
Joe Ardent
parent
bcf4231c84
commit
f3f2604ff8
1 changed files with 91 additions and 83 deletions
174
src/main.rs
174
src/main.rs
|
|
@ -1,17 +1,17 @@
|
|||
#![no_std]
|
||||
#![no_main]
|
||||
|
||||
use cortex_m::prelude::_embedded_hal_Pwm;
|
||||
use core::sync::atomic::{AtomicBool, AtomicU32, Ordering::Relaxed};
|
||||
use defmt::unwrap;
|
||||
use embassy_executor::Spawner;
|
||||
use embassy_stm32::{
|
||||
adc::{Adc, SampleTime},
|
||||
gpio::{AnyPin, Level, Output, Speed},
|
||||
peripherals::{ADC1, PA0},
|
||||
gpio::OutputType::PushPull,
|
||||
peripherals::{ADC1, PA0, TIM3},
|
||||
time::Hertz,
|
||||
timer::{
|
||||
low_level::CountingMode::EdgeAlignedUp,
|
||||
simple_pwm::{PwmPin, SimplePwm},
|
||||
Channel,
|
||||
},
|
||||
Config, Peri,
|
||||
};
|
||||
|
|
@ -19,116 +19,124 @@ use embassy_time::{Duration, Timer};
|
|||
use {defmt_rtt as _, panic_probe as _};
|
||||
|
||||
static SERVO_HZ: Hertz = Hertz::hz(60);
|
||||
static SERVO_MIN_USECS: f32 = 550.0;
|
||||
static SERVO_MAX_USECS: f32 = 1900.0;
|
||||
static SERVO_MIN_DEG: f32 = -180.0;
|
||||
static SERVO_MAX_DEG: f32 = 180.0;
|
||||
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 mut config = Config::default();
|
||||
let config = config();
|
||||
|
||||
{
|
||||
use embassy_stm32::rcc::{mux, Pll, PllMul, PllPreDiv, PllRDiv, PllSource, Sysclk};
|
||||
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;
|
||||
}
|
||||
let peripherals = embassy_stm32::init(config);
|
||||
|
||||
let p = embassy_stm32::init(config);
|
||||
|
||||
let pwm_pin = PwmPin::new(p.PC7, embassy_stm32::gpio::OutputType::PushPull);
|
||||
let mut pwm = SimplePwm::new(
|
||||
p.TIM3,
|
||||
let pwm_pin = PwmPin::new(peripherals.PC7, PushPull);
|
||||
let pwm = SimplePwm::new(
|
||||
peripherals.TIM3,
|
||||
None,
|
||||
Some(pwm_pin),
|
||||
None,
|
||||
None,
|
||||
SERVO_HZ,
|
||||
embassy_stm32::timer::low_level::CountingMode::EdgeAlignedUp,
|
||||
EdgeAlignedUp,
|
||||
);
|
||||
|
||||
pwm.enable(Channel::Ch2);
|
||||
let adc1 = Adc::new(peripherals.ADC1, Default::default());
|
||||
|
||||
let pwm_max_dur = Duration::from_hz(SERVO_HZ.0 as u64);
|
||||
let pwm_max_duty = pwm.get_max_duty();
|
||||
let sensor_input = peripherals.PA0.into();
|
||||
spawner.spawn(unwrap!(read_adc1_pa0(adc1, sensor_input)));
|
||||
|
||||
let a = Adc::new(p.ADC1, Default::default());
|
||||
spawner.spawn(unwrap!(servo_loop(pwm)));
|
||||
|
||||
let channel = p.PA0.into();
|
||||
spawner.spawn(unwrap!(read_adc1_pa0(a, channel)));
|
||||
|
||||
let zero = degrees2duty(0.0, pwm_max_dur, pwm_max_duty);
|
||||
pwm.set_duty(Channel::Ch2, zero);
|
||||
Timer::after_millis(500).await;
|
||||
|
||||
for _ in 0..10 {
|
||||
let duty = degrees2duty(SERVO_MIN_DEG, pwm_max_dur, pwm_max_duty);
|
||||
pwm.set_duty(Channel::Ch2, duty);
|
||||
Timer::after_secs(1).await;
|
||||
|
||||
let duty = degrees2duty(SERVO_MAX_DEG, pwm_max_dur, pwm_max_duty);
|
||||
pwm.set_duty(Channel::Ch2, duty);
|
||||
Timer::after_secs(1).await;
|
||||
}
|
||||
|
||||
pwm.set_duty(Channel::Ch2, zero);
|
||||
Timer::after_millis(600).await;
|
||||
|
||||
treatbot_5k::exit()
|
||||
// calls the global exit after 30 seconds
|
||||
spawner.spawn(unwrap!(exit()));
|
||||
}
|
||||
|
||||
#[embassy_executor::task]
|
||||
async fn read_adc1_pa0(mut a: Adc<'static, ADC1>, mut channel: Peri<'static, PA0>) {
|
||||
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 {
|
||||
let val = a.blocking_read(&mut channel.reborrow(), SampleTime::CYCLES640_5);
|
||||
if val < 20 {
|
||||
defmt::info!("low: {}", val);
|
||||
} else {
|
||||
defmt::info!("high {}", val);
|
||||
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;
|
||||
}
|
||||
}
|
||||
|
||||
#[embassy_executor::task]
|
||||
async fn blinky(p: Peri<'static, AnyPin>) {
|
||||
let mut led = Output::new(p, Level::High, Speed::Low);
|
||||
fn config() -> Config {
|
||||
use embassy_stm32::rcc::{mux, Pll, PllMul, PllPreDiv, PllRDiv, PllSource, Sysclk};
|
||||
|
||||
led.set_low();
|
||||
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;
|
||||
|
||||
loop {
|
||||
led.set_high();
|
||||
Timer::after_millis(300).await;
|
||||
|
||||
led.set_low();
|
||||
Timer::after_millis(300).await;
|
||||
|
||||
led.set_high();
|
||||
Timer::after_millis(300).await;
|
||||
|
||||
led.set_low();
|
||||
Timer::after_millis(300).await;
|
||||
|
||||
led.set_high();
|
||||
Timer::after_millis(600).await;
|
||||
|
||||
led.set_low();
|
||||
Timer::after_millis(300).await;
|
||||
}
|
||||
config
|
||||
}
|
||||
|
|
|
|||
Loading…
Reference in a new issue