92 lines
3.2 KiB
Rust
92 lines
3.2 KiB
Rust
use embassy_stm32::gpio::{Level, Output, Speed};
|
|
use embassy_stm32::peripherals;
|
|
use embassy_time::{Duration, Instant, Timer};
|
|
use embassy_util::blocking_mutex::raw::ThreadModeRawMutex;
|
|
use embassy_util::channel::mpmc::{Receiver, Sender};
|
|
use embassy_util::{select, Either};
|
|
|
|
use futures::future::join;
|
|
|
|
use crate::usb::Gs232Cmd;
|
|
use crate::{AzElPair, RotorState};
|
|
|
|
#[embassy_executor::task]
|
|
pub async fn movement_task(
|
|
cw_pin: peripherals::PA3,
|
|
ccw_pin: peripherals::PA4,
|
|
up_pin: peripherals::PA5,
|
|
down_pin: peripherals::PA6,
|
|
cmd_receiver: Receiver<'static, ThreadModeRawMutex, Gs232Cmd, 1>,
|
|
pos_sender: Sender<'static, ThreadModeRawMutex, AzElPair, 1>,
|
|
state_sender: Sender<'static, ThreadModeRawMutex, RotorState, 1>,
|
|
) {
|
|
let mut rotor_state = RotorState {
|
|
actual_pos: AzElPair { az: 0, el: 0 },
|
|
setpoint_pos: AzElPair { az: 0, el: 0 },
|
|
stopped: false,
|
|
};
|
|
|
|
let mut cw_pin = Output::new(cw_pin, Level::Low, Speed::Low);
|
|
let mut ccw_pin = Output::new(ccw_pin, Level::Low, Speed::Low);
|
|
let mut up_pin = Output::new(up_pin, Level::Low, Speed::Low);
|
|
let mut down_pin = Output::new(down_pin, Level::Low, Speed::Low);
|
|
|
|
loop {
|
|
match select(
|
|
cmd_receiver.recv(),
|
|
Timer::after(Duration::from_millis(100)),
|
|
)
|
|
.await
|
|
{
|
|
Either::First(cmd) => match cmd {
|
|
Gs232Cmd::MoveTo(pair) => {
|
|
rotor_state.setpoint_pos = pair;
|
|
rotor_state.stopped = false;
|
|
}
|
|
Gs232Cmd::Stop => {
|
|
rotor_state.stopped = true;
|
|
}
|
|
_ => {}
|
|
},
|
|
Either::Second(_) => {
|
|
if !rotor_state.stopped && rotor_state.actual_pos.az < rotor_state.setpoint_pos.az {
|
|
rotor_state.actual_pos.az += 1;
|
|
cw_pin.set_high();
|
|
ccw_pin.set_low();
|
|
} else if !rotor_state.stopped
|
|
&& rotor_state.actual_pos.az > rotor_state.setpoint_pos.az
|
|
{
|
|
rotor_state.actual_pos.az -= 1;
|
|
cw_pin.set_low();
|
|
ccw_pin.set_high();
|
|
} else {
|
|
cw_pin.set_low();
|
|
ccw_pin.set_low();
|
|
}
|
|
|
|
if !rotor_state.stopped && rotor_state.actual_pos.el < rotor_state.setpoint_pos.el {
|
|
rotor_state.actual_pos.el += 1;
|
|
up_pin.set_high();
|
|
down_pin.set_low();
|
|
} else if !rotor_state.stopped
|
|
&& rotor_state.actual_pos.el > rotor_state.setpoint_pos.el
|
|
{
|
|
rotor_state.actual_pos.el -= 1;
|
|
up_pin.set_low();
|
|
down_pin.set_high();
|
|
} else {
|
|
up_pin.set_low();
|
|
down_pin.set_low();
|
|
}
|
|
|
|
join(
|
|
pos_sender.send(rotor_state.actual_pos),
|
|
state_sender.send(rotor_state),
|
|
)
|
|
.await;
|
|
//state_sender.send(rotor_state).await;
|
|
}
|
|
};
|
|
}
|
|
}
|