amikhalev/robotrs
1
0
Fork 0
Code Issues Pull Requests Releases Wiki Activity

GenericCommand

Browse Source
This commit is contained in:
Alex Mikhalev 2019-01-17 12:00:25 -08:00
parent 0723b5c6a4
commit e3dbd8bdee
2 changed files with 138 additions and 92 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

159
src/command.rs
View File

@ -11,6 +11,52 @@ impl CommandId for u64 {
}
}
pub trait WithCommandData {
fn command_data(&self) -> &CommandData;
}
pub trait CommandInfo: CommandId {
fn name(&self) -> &str;
}
pub struct CommandData {
command_id: u64,
name: String,
}
impl CommandData {
pub fn new<TName: Into<String>>(command_id: u64, name: TName) -> Self {
CommandData {
command_id,
name: name.into(),
}
}
}
impl CommandId for CommandData {
fn command_id(&self) -> u64 {
self.command_id
}
}
impl CommandInfo for CommandData {
fn name(&self) -> &str {
&self.name
}
}
impl<T: WithCommandData> CommandId for T {
fn command_id(&self) -> u64 {
self.command_data().command_id
}
}
impl<T: WithCommandData> CommandInfo for T {
fn name(&self) -> &str {
&self.command_data().name
}
}
#[derive(Debug, PartialEq, Clone, Copy)]
pub enum CommandResult {
Continue,
@ -18,108 +64,106 @@ pub enum CommandResult {
Cancel,
}
pub trait Command {
pub trait Command: WithCommandData {
fn init(&mut self);
fn step(&mut self) -> CommandResult;
fn finish(&mut self, result: CommandResult);
fn command_data(&self) -> &CommandData;
}
pub struct GenericCommand<TConfig, TState> {
command_data: CommandData,
pub config: TConfig,
pub state: TState,
}
impl<TConfig, TState> GenericCommand<TConfig, TState>
where
TConfig: 'static,
GenericCommand<TConfig, TState>: Command,
TState: Default + 'static,
{
pub fn new(command_data: CommandData, config: TConfig) -> Self {
Self {
command_data,
config,
state: TState::default(),
}
}
pub fn new_ref(command_data: CommandData, config: TConfig) -> CommandRef {
Rc::new(RefCell::new(Self::new(command_data, config)))
}
}
impl<TConfig, TState> WithCommandData for GenericCommand<TConfig, TState> {
fn command_data(&self) -> &CommandData {
&self.command_data
}
}
macro_rules! command_impl {
(
$command_st:ident . $command_data:ident;
$command:ident<$tconfig:ty,$tstate:ty>;
$init_self:ident => $init:block;
$step_self:ident => $step:block;
$finish_self:ident, $finish_result:ident => $finish:block
) => {
impl $crate::command::Command for $command_st {
pub type $command = GenericCommand<$tconfig, $tstate>;
impl $crate::command::Command for $command {
fn init(&mut $init_self) {
use $crate::command::CommandInfo;
debug!(
"{}({}) init",
stringify!($command_st),
$init_self.command_data().name()
$init_self.name()
);
$init;
}
fn step(&mut $step_self) -> $crate::command::CommandResult {
use $crate::command::CommandInfo;
debug!(
"{}({}) step",
stringify!($command_st),
$step_self.command_data().name()
$step_self.name()
);
$step
}
fn finish(&mut $finish_self, $finish_result: $crate::command::CommandResult) {
use $crate::command::CommandInfo;
debug!(
"{}({}) finish({:?}",
stringify!($command_st),
$finish_self.command_data().name(),
$finish_self.name(),
$finish_result,
);
$finish;
}
fn command_data(&self) -> &$crate::command::CommandData {
&self.command_data
}
}
};
(
$command_st:ident . $command_data:ident;
$command:ident<$tconfig:ty,$tstate:ty>;
$init_self:ident => $init:block;
$step_self:ident => $step:block;
$finish_self:ident => $finish:block
) => {
command_impl! { $command_st.$command_data;
command_impl! { $command<$tconfig,$tstate>;
$init_self => $init; $step_self => $step; $finish_self, res => {} }
};
(
$command_st:ident . $command_data:ident;
$command:ident<$tconfig:ty>;
$init_self:ident => $init:block
) => {
command_impl! { $command_st.$command_data;
command_impl! { $command<$tconfig,()>;
$init_self => $init; self => { $crate::command::CommandResult::Done }; self => {} }
};
}
pub struct CommandData {
id: u64,
name: String,
}
impl CommandData {
pub fn new<TName: Into<String>>(id: u64, name: TName) -> Self {
CommandData {
id,
name: name.into(),
}
}
pub fn id(&self) -> u64 {
self.id
}
pub fn name(&self) -> &str {
&self.name
}
}
impl CommandId for CommandData {
fn command_id(&self) -> u64 {
self.id
}
}
pub type CommandRef = Rc<RefCell<dyn Command>>;
impl CommandId for CommandRef {
fn command_id(&self) -> u64 {
self.borrow().command_data().id
}
}
pub struct CommandGroup {
commands: Vec<CommandRef>,
command_data: CommandData,
@ -143,6 +187,12 @@ impl CommandGroup {
}
}
impl WithCommandData for CommandGroup {
fn command_data(&self) -> &CommandData {
&self.command_data
}
}
impl Command for CommandGroup {
fn init(&mut self) {
if self.current_idx.is_some() {
@ -220,10 +270,6 @@ impl Command for CommandGroup {
}
self.current_idx = None;
}
fn command_data(&self) -> &CommandData {
&self.command_data
}
}
pub struct CommandRun {
@ -234,7 +280,7 @@ pub struct CommandRun {
impl CommandId for CommandRun {
fn command_id(&self) -> u64 {
self.command.command_id()
self.command.borrow().command_data().command_id()
}
}
@ -297,7 +343,7 @@ impl CommandScheduler {
* Returns true if the command was started, false if it was not started because it was already running
*/
pub fn start(&mut self, command: CommandRef) -> bool {
if self.is_running(command.command_id()) {
if self.is_running(command.borrow().command_data().command_id()) {
false
} else {
self.running_commands.push(CommandRun {
@ -322,9 +368,10 @@ impl CommandScheduler {
if !command_run.initialized && !command_run.cancelled {
{
let command_data = command.command_data();
info!(
debug!(
"initialzing command {} (id {})",
command_data.name, command_data.id
command_data.name(),
command_data.command_id()
);
}
command.init();

71
src/robot.rs
View File

@ -7,48 +7,46 @@ use command::{CommandData, CommandGroup, CommandRef, CommandScheduler};
use drive::{Drive, DriveRef};
mod cmds {
use command::{CommandData, CommandResult};
use command::{Command, CommandResult, GenericCommand};
use drive::DriveRef;
use self::CommandResult::*;
pub struct DriveCommand {
command_data: CommandData,
iterations: u64,
drive: DriveRef,
pub struct DriveCommandConfig {
pub drive: DriveRef,
}
impl DriveCommand {
pub fn new(command_data: CommandData, drive: DriveRef) -> Self {
Self {
command_data,
iterations: 0,
drive,
}
pub struct DriveCommandState {
iterations: u64,
}
impl Default for DriveCommandState {
fn default() -> Self {
DriveCommandState { iterations: 0 }
}
}
command_impl! { DriveCommand.command_data;
self => {
self.iterations = 0;
pub type DriveCommand = GenericCommand<DriveCommandConfig, DriveCommandState>;
}; self =>{
self.drive.borrow_mut().drive_powers(1.0, 1.0);
self.iterations += 1;
if self.iterations >= 10 {
CommandResult::Done
impl Command for DriveCommand {
fn init(&mut self) {
self.state = DriveCommandState::default();
}
fn step(&mut self) -> CommandResult {
self.config.drive.borrow_mut().drive_powers(1.0, 1.0);
self.state.iterations += 1;
if self.state.iterations >= 10 {
Done
} else {
CommandResult::Continue
Continue
}
}; self, res => {
self.drive.borrow_mut().drive_powers(0.0,0.0);
}
}
pub struct LogCmd {
pub command_data: crate::command::CommandData,
}
fn finish(&mut self, res: CommandResult) {
self.config.drive.borrow_mut().drive_powers(0.0, 0.0);
}
}
command_impl! {
LogCmd.command_data; self => { info!("{}", "LogCmd") }
LogCmd<()>; self => { info!("{}", "LogCmd") }
}
}
@ -70,15 +68,16 @@ impl Robot {
pub fn run(mut self) {
warn!("Starting RobotRS");
let command = cmds::DriveCommand::new(CommandData::new(0, "DriveCommand"), self.drive.clone());
let command_ref: CommandRef = Rc::new(RefCell::new(command));
let log_command = cmds::LogCmd {
command_data: CommandData::new(2, "LogCommand"),
};
let log_command_ref: CommandRef = Rc::new(RefCell::new(log_command));
let command = cmds::DriveCommand::new_ref(
CommandData::new(0, "DriveCommand"),
cmds::DriveCommandConfig {
drive: self.drive.clone(),
},
);
let log_command = cmds::LogCmd::new_ref(CommandData::new(2, "LogCommand"), ());
let command_group = CommandGroup::new(
CommandData::new(1, "Group"),
vec![command_ref, log_command_ref],
vec![command, log_command],
);
let command_ref: CommandRef = Rc::new(RefCell::new(command_group));