GenericCommand

src/command.rs

@@ -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();

src/robot.rs

@@ -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,
+    pub struct DriveCommandConfig {
+        pub drive: DriveRef,
+    }
+
+    pub struct DriveCommandState {
         iterations: u64,
-        drive: DriveRef,
     }
 
-    impl DriveCommand {
-        pub fn new(command_data: CommandData, drive: DriveRef) -> Self {
-            Self {
-                command_data,
-                iterations: 0,
-                drive,
-            }
+    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));