AFG/rotor-control
3
0
Fork 0
Code Issues Pull requests Packages Projects Releases Wiki Activity

Added movement pins

Browse source
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) .draw(&mut display)
.unwrap(); .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) .draw(&mut display)
.unwrap(); .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) .draw(&mut display)
.unwrap(); .unwrap();
@ -73,7 +77,7 @@ pub async fn display_task(
.draw(&mut display) .draw(&mut display)
.unwrap(); .unwrap();
let mut tmp: String<16> = String::new(); tmp.clear();
write!(tmp, "AZ: {}", rotor_state.setpoint_pos.az).unwrap(); write!(tmp, "AZ: {}", rotor_state.setpoint_pos.az).unwrap();
Text::new(&tmp, Point::new(1, 30), text_large_inv) Text::new(&tmp, Point::new(1, 30), text_large_inv)
.draw(&mut display) .draw(&mut display)
@ -88,7 +92,7 @@ pub async fn display_task(
display.flush().unwrap(); display.flush().unwrap();
let result = select( let result = select(
Timer::after(Duration::from_millis(100)), Timer::after(Duration::from_millis(500)),
cmd_receiver.recv(), cmd_receiver.recv(),
) )
.await; .await;

12
src/main.rs
View file

@ -63,11 +63,21 @@ async fn main(spawner: 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(); .unwrap();
spawner spawner
.spawn(movement_task( .spawn(movement_task(
p.PA3,
p.PA4,
p.PA5,
p.PA6,
CMD_CHAN.receiver(), CMD_CHAN.receiver(),
POS_CHAN.sender(), POS_CHAN.sender(),
STATE_CHAN.sender(), STATE_CHAN.sender(),

91
src/movement.rs
View file

@ -1,22 +1,21 @@
use defmt::Format; use embassy_stm32::gpio::{Level, Output, Speed};
use core::fmt::Write;
use embassy_stm32::interrupt;
use embassy_stm32::peripherals; use embassy_stm32::peripherals;
use embassy_stm32::usb::Driver; use embassy_time::{Duration, Instant, Timer};
use embassy_usb::Builder;
use embassy_usb_serial::{CdcAcmClass, State};
use embassy_util::blocking_mutex::raw::ThreadModeRawMutex; use embassy_util::blocking_mutex::raw::ThreadModeRawMutex;
use embassy_util::channel::mpmc::{Receiver, Sender}; use embassy_util::channel::mpmc::{Receiver, Sender};
use embassy_util::{select, Either};
use futures::future::join; use futures::future::join;
use heapless::String;
use crate::usb::Gs232Cmd; use crate::usb::Gs232Cmd;
use crate::{AzElPair, RotorState}; use crate::{AzElPair, RotorState};
#[embassy_executor::task] #[embassy_executor::task]
pub async fn movement_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>, cmd_receiver: Receiver<'static, ThreadModeRawMutex, Gs232Cmd, 1>,
pos_sender: Sender<'static, ThreadModeRawMutex, AzElPair, 1>, pos_sender: Sender<'static, ThreadModeRawMutex, AzElPair, 1>,
state_sender: Sender<'static, ThreadModeRawMutex, RotorState, 1>, state_sender: Sender<'static, ThreadModeRawMutex, RotorState, 1>,
@ -27,24 +26,66 @@ pub async fn movement_task(
stopped: false, 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 { loop {
let cmd = cmd_receiver.recv().await; match select(
cmd_receiver.recv(),
match cmd { Timer::after(Duration::from_millis(100)),
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),
) )
.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::Builder;
use embassy_usb_serial::{CdcAcmClass, State}; use embassy_usb_serial::{CdcAcmClass, State};
use embassy_util::blocking_mutex::raw::ThreadModeRawMutex; 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 futures::future::join;
use heapless::String; use heapless::String;
@ -20,6 +21,7 @@ pub async fn usb_task(
dp_pin: peripherals::PA12, dp_pin: peripherals::PA12,
dm_pin: peripherals::PA11, dm_pin: peripherals::PA11,
cmd_sender: Sender<'static, ThreadModeRawMutex, Gs232Cmd, 1>, cmd_sender: Sender<'static, ThreadModeRawMutex, Gs232Cmd, 1>,
pos_receiver: Receiver<'static, ThreadModeRawMutex, AzElPair, 1>,
) { ) {
let irq = interrupt::take!(USB_LP_CAN1_RX0); let irq = interrupt::take!(USB_LP_CAN1_RX0);
let driver = Driver::new(usb, irq, dp_pin, dm_pin); let driver = Driver::new(usb, irq, dp_pin, dm_pin);
@ -53,11 +55,10 @@ pub async fn usb_task(
// Build the builder. // Build the builder.
let mut usb = builder.build(); let mut usb = builder.build();
let az_actual: u16 = 0;
let el_actual: u16 = 0;
// Do stuff with the class! // Do stuff with the class!
let usb_handler_fut = async { let usb_handler_fut = async {
let mut current_pos = AzElPair { az: 0, el: 0 };
loop { loop {
class.wait_connection().await; class.wait_connection().await;
defmt::info!("USB connected"); defmt::info!("USB connected");
@ -65,11 +66,17 @@ pub async fn usb_task(
let mut buffer: String<64> = String::new(); let mut buffer: String<64> = String::new();
loop { loop {
let n = match class.read_packet(&mut packet).await { let n = match select(class.read_packet(&mut packet), pos_receiver.recv()).await {
Ok(n) => n, Either::First(res) => match res {
Err(err) => { Ok(n) => n,
defmt::error!("Unable to read packet: {}", err); Err(err) => {
break; 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(); let mut resp: String<16> = String::new();
match cmd { match cmd {
Gs232Cmd::GetAl => { Gs232Cmd::GetAl => {
write!(&mut resp, "AZ={}\r", az_actual).unwrap(); write!(&mut resp, "AZ={}\r", current_pos.az).unwrap();
} }
Gs232Cmd::GetEz => { Gs232Cmd::GetEz => {
write!(&mut resp, "EL={}\r", el_actual).unwrap(); write!(&mut resp, "EL={}\r", current_pos.el).unwrap();
} }
Gs232Cmd::GetAlEz => { 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(_) => { Gs232Cmd::MoveTo(_) => {
cmd_sender.send(cmd).await; cmd_sender.send(cmd).await;