From d81b02be47d975497885c23d7c3abefc75bc0168 Mon Sep 17 00:00:00 2001 From: Alex Mikhalev Date: Mon, 17 Sep 2018 03:21:34 -0600 Subject: [PATCH] Added commands stuff --- src/command.rs | 150 ++++++++++++++++++++++++++++++++++++++++ src/drive.rs | 14 +++- src/main.rs | 3 +- src/motor_controller.rs | 27 +++++++- src/robot.rs | 66 ++++++++++++++++-- 5 files changed, 249 insertions(+), 11 deletions(-) create mode 100644 src/command.rs diff --git a/src/command.rs b/src/command.rs new file mode 100644 index 0000000..3331a37 --- /dev/null +++ b/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 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>; + +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>, +} + +impl CommandScheduler { + pub fn new() -> Self { + CommandScheduler{ running_commands: Vec::new() } + } + + pub fn get_command_run(&self, id: T) -> Option<&RefCell> { + 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(&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(&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 + } + }) + } +} diff --git a/src/drive.rs b/src/drive.rs index fad0fbe..2fab80c 100644 --- a/src/drive.rs +++ b/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, } -fn create_motor_controllers>(ids: T) -> Vec { - ids.map(MockMotorController::new_id).map(|m| Box::new(m) as MotorControllerBox).collect() +pub type DriveRef = Rc>; + +fn create_motor_controllers>(ids: T) -> Vec { + 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); diff --git a/src/main.rs b/src/main.rs index 1d4a7a0..abf0aec 100644 --- a/src/main.rs +++ b/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(); } diff --git a/src/motor_controller.rs b/src/motor_controller.rs index 25f0549..15e3da5 100644 --- a/src/motor_controller.rs +++ b/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; +pub trait MotorControllerBoxed { + fn new_box(id: u16) -> MotorControllerBox; +} + +impl 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 + } } diff --git a/src/robot.rs b/src/robot.rs index 1d99824..f0faf4e 100644 --- a/src/robot.rs +++ b/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(); + } }