Added commands stuff

src/command.rs

@@ -0,0 +1,150 @@
+use std::rc::Rc;
+use std::cell::RefCell;
+
+pub trait CommandId {
+    fn command_id(&self) -> u64;
+}
+
+impl CommandId for u64 {
+    fn command_id(&self) -> u64 {
+        *self
+    }
+}
+
+#[derive(Debug, PartialEq)]
+pub enum CommandResult {
+    Continue,
+    Done,
+    Cancel,
+}
+
+pub trait Command {
+    fn init(&mut self);
+    fn step(&mut self) -> CommandResult;
+    fn finish(&mut self, result: CommandResult);
+
+    fn command_data<'a>(&'a self) -> &'a CommandData;
+
+    fn command_name(&self) -> &str {
+        &self.command_data().name
+    }
+}
+
+impl<T: Command> CommandId for T {
+    fn command_id(&self) -> u64 {
+        self.command_data().id
+    }
+}
+
+pub struct CommandData {
+    pub id: u64,
+    pub name: String,
+}
+
+pub type CommandRef = Rc<RefCell<dyn Command>>;
+
+impl CommandId for CommandRef {
+    fn command_id(&self) -> u64 {
+        self.borrow().command_data().id
+    }
+}
+
+pub struct CommandRun {
+    initialized: bool,
+    cancelled: bool,
+    command: CommandRef,
+}
+
+impl CommandId for CommandRun {
+    fn command_id(&self) -> u64 {
+        self.command.command_id()
+    }
+}
+
+impl CommandRun {
+    pub fn initialized(&self) -> bool {
+        self.initialized
+    }
+
+    pub fn cancelled(&self) -> bool {
+        self.cancelled
+    }
+
+    pub fn cancel(&mut self) -> bool {
+        if self.cancelled {
+            false
+        } else {
+            self.cancelled = true;
+            true
+        }
+    }
+}
+
+pub struct CommandScheduler {
+    running_commands: Vec<RefCell<CommandRun>>,
+}
+
+impl CommandScheduler {
+    pub fn new() -> Self {
+        CommandScheduler{ running_commands: Vec::new() }
+    }
+
+    pub fn get_command_run<T: CommandId>(&self, id: T) -> Option<&RefCell<CommandRun>> {
+        let id = id.command_id();
+        for command_run in &self.running_commands {
+            if command_run.borrow().command_id() == id {
+                return Some(command_run);
+            }
+        }
+        None
+    }
+
+    pub fn is_running<T: CommandId>(&self, id: T) -> bool {
+        self.get_command_run(id).is_some()
+    }
+
+    /**
+     * Starts running a command.
+     * 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()) {
+            false
+        } else {
+            self.running_commands.push(RefCell::new(CommandRun{ 
+                initialized: false, cancelled: false, command: command,
+            }));
+            true
+        }
+    }
+
+    pub fn cancel<T: CommandId>(&mut self, id: T) -> bool {
+        self.get_command_run(id)
+            .map_or(false, |command_run| {
+                command_run.borrow_mut().cancelled = true;
+                true
+            })
+    }
+    
+    pub fn execute(&mut self) {
+        self.running_commands.retain(|command_run| { 
+            let mut command_run = command_run.borrow_mut();
+            if !command_run.initialized && !command_run.cancelled {
+                command_run.command.borrow_mut().init();
+                command_run.initialized = true;
+            }
+            let result = 
+                if command_run.initialized {
+                    command_run.command.borrow_mut().step()
+                } else {
+                    CommandResult::Cancel
+                };
+            if result != CommandResult::Continue {
+                command_run.command.borrow_mut().finish(result);
+                false
+            } else {
+                true
+            }
+        })
+    }
+}

src/drive.rs

@@ -1,4 +1,8 @@
+use std::rc::Rc;
+use std::cell::RefCell;
+
 use motor_controller::MotorControllerBox;
+use motor_controller::MotorControllerBoxed;
 use motor_controller::MockMotorController;
 
 pub struct Drive {
@@ -6,8 +10,10 @@ pub struct Drive {
     right_motors: Vec<MotorControllerBox>,
 }
 
-fn create_motor_controllers<T: Iterator<Item = u16>>(ids: T) -> Vec<MotorControllerBox> {
-    ids.map(MockMotorController::new_id).map(|m| Box::new(m) as MotorControllerBox).collect()
+pub type DriveRef = Rc<RefCell<Drive>>;
+
+fn create_motor_controllers<T: IntoIterator<Item = u16>>(ids: T) -> Vec<MotorControllerBox> {
+    ids.into_iter().map(MockMotorController::new_box).collect()
 }
 
 impl Drive {
@@ -17,6 +23,10 @@ impl Drive {
         Drive { left_motors, right_motors }
     }
 
+    pub fn new_ref() -> DriveRef {
+        Rc::new(RefCell::new(Drive::new()))
+    }
+
     pub fn drive_powers(&mut self, left_power: f64, right_power: f64) {
         for motor in &mut self.left_motors {
             motor.set(left_power);

src/main.rs

@@ -1,11 +1,12 @@
 mod robot;
 mod drive;
 mod motor_controller;
+mod command;
 
 use robot::Robot;
 
 fn main() {
     let mut robot = Robot::new();
 
-    robot.start();
+    robot.run();
 }

src/motor_controller.rs

@@ -1,24 +1,47 @@
 pub trait MotorController {
+    fn id(&self) -> u16;
     fn set(&mut self, value: f64);
+    fn get(&self) -> f64;
+}
+
+pub trait MotorControllerId: MotorController {
+    fn new(id: u16) -> Self;
 }
 
 pub type MotorControllerBox = Box<dyn MotorController>;
 
+pub trait MotorControllerBoxed {
+    fn new_box(id: u16) -> MotorControllerBox;
+}
+
+impl<T: MotorControllerId + 'static> MotorControllerBoxed for T {
+    fn new_box(id: u16) -> MotorControllerBox {
+        Box::new(T::new(id))
+    }
+}
+
 pub struct MockMotorController {
     id: u16,
     value: f64,
 }
 
-impl MockMotorController {
-    pub fn new_id(id: u16) -> Self {
+impl MotorControllerId for MockMotorController {
+    fn new(id: u16) -> Self {
         MockMotorController{ id: id, value: 0.0 }
     }
 }
 
 impl MotorController for MockMotorController {
+    fn id(&self) -> u16 {
+        self.id
+    }
 
     fn set(&mut self, value: f64) {
         self.value = value;
         println!("MotorController id {} value={}", self.id, self.value);
     }
+
+    fn get(&self) -> f64 {
+        self.value
+    }
 }

src/robot.rs

@@ -1,21 +1,75 @@
-use drive::Drive;
+use std::time::Duration;
+use std::thread;
+use std::cell::RefCell;
+use std::rc::Rc;
+
+use drive::{Drive, DriveRef};
+use command::{Command, CommandData, CommandScheduler, CommandResult};
+
+struct DriveCommand {
+    iterations: u64,
+    data: CommandData,
+    drive: DriveRef,
+}
+
+impl DriveCommand {
+    fn new(data: CommandData, drive: DriveRef) -> Self {
+        Self { iterations: 0, data: data, drive: drive }
+    }
+}
+
+impl Command for DriveCommand {
+    fn init(&mut self) {
+        self.iterations = 0;
+        println!("DriveCommand init");
+    }
+
+    fn step(&mut self) -> CommandResult {
+        println!("DriveCommand step");
+        self.drive.borrow_mut().drive_powers(1.0, 1.0);
+        self.iterations += 1;
+        if self.iterations >= 10 { CommandResult::Done } else { CommandResult::Continue }
+    }
+
+    fn finish(&mut self, result: CommandResult) {
+        println!("DriveCommand finish interrupted: {:?}", result);
+    }
+
+    fn command_data(&self) -> &CommandData {
+        &self.data
+    }
+}
 
 pub struct Robot {
-    drive: Drive,
+    drive: DriveRef,
+    command_scheduler: CommandScheduler,
 }
 
+const ITER_PERIOD: Duration = Duration::from_nanos(1000000000 / 50);
+
 impl Robot {
     pub fn new() -> Self {
         Robot{ 
-            drive: Drive::new(),
+            drive: Drive::new_ref(),
+            command_scheduler: CommandScheduler::new(),
         }
     }
 
-    pub fn start(&mut self) {
+    pub fn run(mut self) {
         println!("Starting RobotRS");
 
-        for _ in 0..10 {
-            self.drive.drive_powers(1.0, 1.0);
+        let command_data = CommandData { id: 0, name: "DriveCommand".into() };
+        let command = Rc::new(RefCell::new(DriveCommand::new(command_data, self.drive.clone())));
+
+        self.command_scheduler.start(command);
+
+        loop {
+            self.execute();
+            thread::sleep(ITER_PERIOD);
         }
     }
+
+    fn execute(&mut self) {
+        self.command_scheduler.execute();
+    }
 }