sebastian
/
rotor-control-stm32
1
0
Fork 0
Code Issues Pull Requests Projects Releases Wiki Activity

Added movement pins

This commit is contained in:
Sebastian 2022-08-21 22:19:34 +02:00
parent 90768d9bf0
commit 275dea77e9
4 changed files with 105 additions and 42 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

12
src/display.rs
View File

@ -60,11 +60,15 @@ pub async fn display_task(
.draw(&mut display)
.unwrap();
Text::new("AZ: 23", Point::new(1, 17), text_large)
let mut tmp: String<16> = String::new();
write!(tmp, "AZ: {}", rotor_state.actual_pos.az).unwrap();
Text::new(&tmp, Point::new(1, 17), text_large)
.draw(&mut display)
.unwrap();
Text::new("EL: 42", Point::new(64, 17), text_large)
tmp.clear();
write!(tmp, "EL: {}", rotor_state.actual_pos.el).unwrap();
Text::new(&tmp, Point::new(64, 17), text_large)
.draw(&mut display)
.unwrap();
@ -73,7 +77,7 @@ pub async fn display_task(
.draw(&mut display)
.unwrap();
let mut tmp: String<16> = String::new();
tmp.clear();
write!(tmp, "AZ: {}", rotor_state.setpoint_pos.az).unwrap();
Text::new(&tmp, Point::new(1, 30), text_large_inv)
.draw(&mut display)
@ -88,7 +92,7 @@ pub async fn display_task(
display.flush().unwrap();
let result = select(
Timer::after(Duration::from_millis(100)),
Timer::after(Duration::from_millis(500)),
cmd_receiver.recv(),
)
.await;

12
src/main.rs
View File

@ -63,11 +63,21 @@ async fn main(spawner: Spawner) {
}
spawner
.spawn(usb_task(p.USB, p.PA12, p.PA11, CMD_CHAN.sender()))
.spawn(usb_task(
p.USB,
p.PA12,
p.PA11,
CMD_CHAN.sender(),
POS_CHAN.receiver(),
))
.unwrap();
spawner
.spawn(movement_task(
p.PA3,
p.PA4,
p.PA5,
p.PA6,
CMD_CHAN.receiver(),
POS_CHAN.sender(),
STATE_CHAN.sender(),

91
src/movement.rs
View File

@ -1,22 +1,21 @@
use defmt::Format;
use core::fmt::Write;
use embassy_stm32::interrupt;
use embassy_stm32::gpio::{Level, Output, Speed};
use embassy_stm32::peripherals;
use embassy_stm32::usb::Driver;
use embassy_usb::Builder;
use embassy_usb_serial::{CdcAcmClass, State};
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 heapless::String;
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>,
@ -27,24 +26,66 @@ pub async fn movement_task(
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 {
let cmd = cmd_receiver.recv().await;
match cmd {
Gs232Cmd::MoveTo(pair) => {
rotor_state.setpoint_pos = pair;
rotor_state.stopped = false;
}
Gs232Cmd::Stop => {
rotor_state.stopped = true;
}
_ => {}
};
join(
pos_sender.send(rotor_state.actual_pos),
state_sender.send(rotor_state),
match select(
cmd_receiver.recv(),
Timer::after(Duration::from_millis(100)),
)
.await;
.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;
}
};
}
}

32
src/usb.rs
View File

@ -7,7 +7,8 @@ use embassy_stm32::usb::Driver;
use embassy_usb::Builder;
use embassy_usb_serial::{CdcAcmClass, State};
use embassy_util::blocking_mutex::raw::ThreadModeRawMutex;
use embassy_util::channel::mpmc::Sender;
use embassy_util::channel::mpmc::{Receiver, Sender};
use embassy_util::{select, Either};
use futures::future::join;
use heapless::String;
@ -20,6 +21,7 @@ pub async fn usb_task(
dp_pin: peripherals::PA12,
dm_pin: peripherals::PA11,
cmd_sender: Sender<'static, ThreadModeRawMutex, Gs232Cmd, 1>,
pos_receiver: Receiver<'static, ThreadModeRawMutex, AzElPair, 1>,
) {
let irq = interrupt::take!(USB_LP_CAN1_RX0);
let driver = Driver::new(usb, irq, dp_pin, dm_pin);
@ -53,11 +55,10 @@ pub async fn usb_task(
// Build the builder.
let mut usb = builder.build();
let az_actual: u16 = 0;
let el_actual: u16 = 0;
// Do stuff with the class!
let usb_handler_fut = async {
let mut current_pos = AzElPair { az: 0, el: 0 };
loop {
class.wait_connection().await;
defmt::info!("USB connected");
@ -65,11 +66,17 @@ pub async fn usb_task(
let mut buffer: String<64> = String::new();
loop {
let n = match class.read_packet(&mut packet).await {
Ok(n) => n,
Err(err) => {
defmt::error!("Unable to read packet: {}", err);
break;
let n = match select(class.read_packet(&mut packet), pos_receiver.recv()).await {
Either::First(res) => match res {
Ok(n) => n,
Err(err) => {
defmt::error!("Unable to read packet: {}", err);
break;
}
},
Either::Second(pair) => {
current_pos = pair;
continue;
}
};
@ -94,13 +101,14 @@ pub async fn usb_task(
let mut resp: String<16> = String::new();
match cmd {
Gs232Cmd::GetAl => {
write!(&mut resp, "AZ={}\r", az_actual).unwrap();
write!(&mut resp, "AZ={}\r", current_pos.az).unwrap();
}
Gs232Cmd::GetEz => {
write!(&mut resp, "EL={}\r", el_actual).unwrap();
write!(&mut resp, "EL={}\r", current_pos.el).unwrap();
}
Gs232Cmd::GetAlEz => {
write!(&mut resp, "AZ={} EL={}\r", az_actual, el_actual).unwrap();
write!(&mut resp, "AZ={} EL={}\r", current_pos.az, current_pos.el)
.unwrap();
}
Gs232Cmd::MoveTo(_) => {
cmd_sender.send(cmd).await;