diff --git a/.vscode/launch.json b/.vscode/launch.json new file mode 100644 index 0000000..42495c2 --- /dev/null +++ b/.vscode/launch.json @@ -0,0 +1,19 @@ +{ + // Use IntelliSense to learn about possible attributes. + // Hover to view descriptions of existing attributes. + // For more information, visit: https://go.microsoft.com/fwlink/?linkid=830387 + "version": "0.2.0", + "configurations": [ + { + "type": "lldb", + "request": "launch", + "name": "Launch robotrs", + "program": "${workspaceFolder}/target/debug/robotrs", + "env": { + "RUST_BACKTRACE": "1" + }, + "args": [], + "cwd": "${workspaceFolder}" + } + ] +} \ No newline at end of file diff --git a/Cargo.lock b/Cargo.lock index dc90fae..769e4c5 100644 --- a/Cargo.lock +++ b/Cargo.lock @@ -1,4 +1,220 @@ +[[package]] +name = "aho-corasick" +version = "0.6.9" +source = "registry+https://github.com/rust-lang/crates.io-index" +dependencies = [ + "memchr 2.1.2 (registry+https://github.com/rust-lang/crates.io-index)", +] + +[[package]] +name = "atty" +version = "0.2.11" +source = "registry+https://github.com/rust-lang/crates.io-index" +dependencies = [ + "libc 0.2.47 (registry+https://github.com/rust-lang/crates.io-index)", + "termion 1.5.1 (registry+https://github.com/rust-lang/crates.io-index)", + "winapi 0.3.6 (registry+https://github.com/rust-lang/crates.io-index)", +] + +[[package]] +name = "cfg-if" +version = "0.1.6" +source = "registry+https://github.com/rust-lang/crates.io-index" + +[[package]] +name = "env_logger" +version = "0.6.0" +source = "registry+https://github.com/rust-lang/crates.io-index" +dependencies = [ + "atty 0.2.11 (registry+https://github.com/rust-lang/crates.io-index)", + "humantime 1.2.0 (registry+https://github.com/rust-lang/crates.io-index)", + "log 0.4.6 (registry+https://github.com/rust-lang/crates.io-index)", + "regex 1.1.0 (registry+https://github.com/rust-lang/crates.io-index)", + "termcolor 1.0.4 (registry+https://github.com/rust-lang/crates.io-index)", +] + +[[package]] +name = "humantime" +version = "1.2.0" +source = "registry+https://github.com/rust-lang/crates.io-index" +dependencies = [ + "quick-error 1.2.2 (registry+https://github.com/rust-lang/crates.io-index)", +] + +[[package]] +name = "lazy_static" +version = "1.2.0" +source = "registry+https://github.com/rust-lang/crates.io-index" + +[[package]] +name = "libc" +version = "0.2.47" +source = "registry+https://github.com/rust-lang/crates.io-index" + +[[package]] +name = "log" +version = "0.4.6" +source = "registry+https://github.com/rust-lang/crates.io-index" +dependencies = [ + "cfg-if 0.1.6 (registry+https://github.com/rust-lang/crates.io-index)", +] + +[[package]] +name = "memchr" +version = "2.1.2" +source = "registry+https://github.com/rust-lang/crates.io-index" +dependencies = [ + "cfg-if 0.1.6 (registry+https://github.com/rust-lang/crates.io-index)", + "libc 0.2.47 (registry+https://github.com/rust-lang/crates.io-index)", + "version_check 0.1.5 (registry+https://github.com/rust-lang/crates.io-index)", +] + +[[package]] +name = "quick-error" +version = "1.2.2" +source = "registry+https://github.com/rust-lang/crates.io-index" + +[[package]] +name = "redox_syscall" +version = "0.1.50" +source = "registry+https://github.com/rust-lang/crates.io-index" + +[[package]] +name = "redox_termios" +version = "0.1.1" +source = "registry+https://github.com/rust-lang/crates.io-index" +dependencies = [ + "redox_syscall 0.1.50 (registry+https://github.com/rust-lang/crates.io-index)", +] + +[[package]] +name = "regex" +version = "1.1.0" +source = "registry+https://github.com/rust-lang/crates.io-index" +dependencies = [ + "aho-corasick 0.6.9 (registry+https://github.com/rust-lang/crates.io-index)", + "memchr 2.1.2 (registry+https://github.com/rust-lang/crates.io-index)", + "regex-syntax 0.6.4 (registry+https://github.com/rust-lang/crates.io-index)", + "thread_local 0.3.6 (registry+https://github.com/rust-lang/crates.io-index)", + "utf8-ranges 1.0.2 (registry+https://github.com/rust-lang/crates.io-index)", +] + +[[package]] +name = "regex-syntax" +version = "0.6.4" +source = "registry+https://github.com/rust-lang/crates.io-index" +dependencies = [ + "ucd-util 0.1.3 (registry+https://github.com/rust-lang/crates.io-index)", +] + [[package]] name = "robotrs" version = "0.1.0" +dependencies = [ + "env_logger 0.6.0 (registry+https://github.com/rust-lang/crates.io-index)", + "log 0.4.6 (registry+https://github.com/rust-lang/crates.io-index)", +] + +[[package]] +name = "termcolor" +version = "1.0.4" +source = "registry+https://github.com/rust-lang/crates.io-index" +dependencies = [ + "wincolor 1.0.1 (registry+https://github.com/rust-lang/crates.io-index)", +] + +[[package]] +name = "termion" +version = "1.5.1" +source = "registry+https://github.com/rust-lang/crates.io-index" +dependencies = [ + "libc 0.2.47 (registry+https://github.com/rust-lang/crates.io-index)", + "redox_syscall 0.1.50 (registry+https://github.com/rust-lang/crates.io-index)", + "redox_termios 0.1.1 (registry+https://github.com/rust-lang/crates.io-index)", +] + +[[package]] +name = "thread_local" +version = "0.3.6" +source = "registry+https://github.com/rust-lang/crates.io-index" +dependencies = [ + "lazy_static 1.2.0 (registry+https://github.com/rust-lang/crates.io-index)", +] + +[[package]] +name = "ucd-util" +version = "0.1.3" +source = "registry+https://github.com/rust-lang/crates.io-index" + +[[package]] +name = "utf8-ranges" +version = "1.0.2" +source = "registry+https://github.com/rust-lang/crates.io-index" + +[[package]] +name = "version_check" +version = "0.1.5" +source = "registry+https://github.com/rust-lang/crates.io-index" + +[[package]] +name = "winapi" +version = "0.3.6" +source = "registry+https://github.com/rust-lang/crates.io-index" +dependencies = [ + "winapi-i686-pc-windows-gnu 0.4.0 (registry+https://github.com/rust-lang/crates.io-index)", + "winapi-x86_64-pc-windows-gnu 0.4.0 (registry+https://github.com/rust-lang/crates.io-index)", +] + +[[package]] +name = "winapi-i686-pc-windows-gnu" +version = "0.4.0" +source = "registry+https://github.com/rust-lang/crates.io-index" + +[[package]] +name = "winapi-util" +version = "0.1.1" +source = "registry+https://github.com/rust-lang/crates.io-index" +dependencies = [ + "winapi 0.3.6 (registry+https://github.com/rust-lang/crates.io-index)", +] + +[[package]] +name = "winapi-x86_64-pc-windows-gnu" +version = "0.4.0" +source = "registry+https://github.com/rust-lang/crates.io-index" + +[[package]] +name = "wincolor" +version = "1.0.1" +source = "registry+https://github.com/rust-lang/crates.io-index" +dependencies = [ + "winapi 0.3.6 (registry+https://github.com/rust-lang/crates.io-index)", + "winapi-util 0.1.1 (registry+https://github.com/rust-lang/crates.io-index)", +] +[metadata] +"checksum aho-corasick 0.6.9 (registry+https://github.com/rust-lang/crates.io-index)" = "1e9a933f4e58658d7b12defcf96dc5c720f20832deebe3e0a19efd3b6aaeeb9e" +"checksum atty 0.2.11 (registry+https://github.com/rust-lang/crates.io-index)" = "9a7d5b8723950951411ee34d271d99dddcc2035a16ab25310ea2c8cfd4369652" +"checksum cfg-if 0.1.6 (registry+https://github.com/rust-lang/crates.io-index)" = "082bb9b28e00d3c9d39cc03e64ce4cea0f1bb9b3fde493f0cbc008472d22bdf4" +"checksum env_logger 0.6.0 (registry+https://github.com/rust-lang/crates.io-index)" = "afb070faf94c85d17d50ca44f6ad076bce18ae92f0037d350947240a36e9d42e" +"checksum humantime 1.2.0 (registry+https://github.com/rust-lang/crates.io-index)" = "3ca7e5f2e110db35f93b837c81797f3714500b81d517bf20c431b16d3ca4f114" +"checksum lazy_static 1.2.0 (registry+https://github.com/rust-lang/crates.io-index)" = "a374c89b9db55895453a74c1e38861d9deec0b01b405a82516e9d5de4820dea1" +"checksum libc 0.2.47 (registry+https://github.com/rust-lang/crates.io-index)" = "48450664a984b25d5b479554c29cc04e3150c97aa4c01da5604a2d4ed9151476" +"checksum log 0.4.6 (registry+https://github.com/rust-lang/crates.io-index)" = "c84ec4b527950aa83a329754b01dbe3f58361d1c5efacd1f6d68c494d08a17c6" +"checksum memchr 2.1.2 (registry+https://github.com/rust-lang/crates.io-index)" = "db4c41318937f6e76648f42826b1d9ade5c09cafb5aef7e351240a70f39206e9" +"checksum quick-error 1.2.2 (registry+https://github.com/rust-lang/crates.io-index)" = "9274b940887ce9addde99c4eee6b5c44cc494b182b97e73dc8ffdcb3397fd3f0" +"checksum redox_syscall 0.1.50 (registry+https://github.com/rust-lang/crates.io-index)" = "52ee9a534dc1301776eff45b4fa92d2c39b1d8c3d3357e6eb593e0d795506fc2" +"checksum redox_termios 0.1.1 (registry+https://github.com/rust-lang/crates.io-index)" = "7e891cfe48e9100a70a3b6eb652fef28920c117d366339687bd5576160db0f76" +"checksum regex 1.1.0 (registry+https://github.com/rust-lang/crates.io-index)" = "37e7cbbd370869ce2e8dff25c7018702d10b21a20ef7135316f8daecd6c25b7f" +"checksum regex-syntax 0.6.4 (registry+https://github.com/rust-lang/crates.io-index)" = "4e47a2ed29da7a9e1960e1639e7a982e6edc6d49be308a3b02daf511504a16d1" +"checksum termcolor 1.0.4 (registry+https://github.com/rust-lang/crates.io-index)" = "4096add70612622289f2fdcdbd5086dc81c1e2675e6ae58d6c4f62a16c6d7f2f" +"checksum termion 1.5.1 (registry+https://github.com/rust-lang/crates.io-index)" = "689a3bdfaab439fd92bc87df5c4c78417d3cbe537487274e9b0b2dce76e92096" +"checksum thread_local 0.3.6 (registry+https://github.com/rust-lang/crates.io-index)" = "c6b53e329000edc2b34dbe8545fd20e55a333362d0a321909685a19bd28c3f1b" +"checksum ucd-util 0.1.3 (registry+https://github.com/rust-lang/crates.io-index)" = "535c204ee4d8434478593480b8f86ab45ec9aae0e83c568ca81abf0fd0e88f86" +"checksum utf8-ranges 1.0.2 (registry+https://github.com/rust-lang/crates.io-index)" = "796f7e48bef87609f7ade7e06495a87d5cd06c7866e6a5cbfceffc558a243737" +"checksum version_check 0.1.5 (registry+https://github.com/rust-lang/crates.io-index)" = "914b1a6776c4c929a602fafd8bc742e06365d4bcbe48c30f9cca5824f70dc9dd" +"checksum winapi 0.3.6 (registry+https://github.com/rust-lang/crates.io-index)" = "92c1eb33641e276cfa214a0522acad57be5c56b10cb348b3c5117db75f3ac4b0" +"checksum winapi-i686-pc-windows-gnu 0.4.0 (registry+https://github.com/rust-lang/crates.io-index)" = "ac3b87c63620426dd9b991e5ce0329eff545bccbbb34f3be09ff6fb6ab51b7b6" +"checksum winapi-util 0.1.1 (registry+https://github.com/rust-lang/crates.io-index)" = "afc5508759c5bf4285e61feb862b6083c8480aec864fa17a81fdec6f69b461ab" +"checksum winapi-x86_64-pc-windows-gnu 0.4.0 (registry+https://github.com/rust-lang/crates.io-index)" = "712e227841d057c1ee1cd2fb22fa7e5a5461ae8e48fa2ca79ec42cfc1931183f" +"checksum wincolor 1.0.1 (registry+https://github.com/rust-lang/crates.io-index)" = "561ed901ae465d6185fa7864d63fbd5720d0ef718366c9a4dc83cf6170d7e9ba" diff --git a/Cargo.toml b/Cargo.toml index a8d8558..a7a7d85 100644 --- a/Cargo.toml +++ b/Cargo.toml @@ -4,3 +4,5 @@ version = "0.1.0" authors = ["Alex Mikhalev "] [dependencies] +log = "0.4" +env_logger = "0.6" diff --git a/src/command.rs b/src/command.rs index 3331a37..7bc8de3 100644 --- a/src/command.rs +++ b/src/command.rs @@ -1,5 +1,5 @@ -use std::rc::Rc; use std::cell::RefCell; +use std::rc::Rc; pub trait CommandId { fn command_id(&self) -> u64; @@ -11,7 +11,7 @@ impl CommandId for u64 { } } -#[derive(Debug, PartialEq)] +#[derive(Debug, PartialEq, Clone, Copy)] pub enum CommandResult { Continue, Done, @@ -22,23 +22,94 @@ pub trait Command { fn init(&mut self); fn step(&mut self) -> CommandResult; fn finish(&mut self, result: CommandResult); + fn command_data(&self) -> &CommandData; +} - fn command_data<'a>(&'a self) -> &'a CommandData; +macro_rules! command_impl { + ( + $command_st:ident . $command_data:ident; + $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 { + fn init(&mut $init_self) { + debug!( + "{}({}) init", + stringify!($command_st), + $init_self.command_data().name() + ); + $init; + } - fn command_name(&self) -> &str { - &self.command_data().name - } + fn step(&mut $step_self) -> $crate::command::CommandResult { + debug!( + "{}({}) step", + stringify!($command_st), + $step_self.command_data().name() + ); + $step + } + + fn finish(&mut $finish_self, $finish_result: $crate::command::CommandResult) { + debug!( + "{}({}) finish({:?}", + stringify!($command_st), + $finish_self.command_data().name(), + $finish_result, + ); + $finish; + } + + fn command_data(&self) -> &$crate::command::CommandData { + &self.command_data + } + } + }; + ( + $command_st:ident . $command_data:ident; + $init_self:ident => $init:block; + $step_self:ident => $step:block; + $finish_self:ident => $finish:block + ) => { + command_impl! { $command_st.$command_data; + $init_self => $init; $step_self => $step; $finish_self, res => {} } + }; + ( + $command_st:ident . $command_data:ident; + $init_self:ident => $init:block + ) => { + command_impl! { $command_st.$command_data; + $init_self => $init; self => { $crate::command::CommandResult::Done }; self => {} } + }; } -impl CommandId for T { - fn command_id(&self) -> u64 { - self.command_data().id +pub struct CommandData { + id: u64, + name: String, +} + +impl CommandData { + pub fn new>(id: u64, name: TName) -> Self { + CommandData { + id, + name: name.into(), + } + } + + pub fn id(&self) -> u64 { + self.id + } + + pub fn name(&self) -> &str { + &self.name } } -pub struct CommandData { - pub id: u64, - pub name: String, +impl CommandId for CommandData { + fn command_id(&self) -> u64 { + self.id + } } pub type CommandRef = Rc>; @@ -49,6 +120,112 @@ impl CommandId for CommandRef { } } +pub struct CommandGroup { + commands: Vec, + command_data: CommandData, + current_idx: Option, + has_initialized: bool, + has_cancelled: bool, +} + +impl CommandGroup { + pub fn new(data: CommandData, commands: TCommands) -> Self + where + TCommands: IntoIterator, + { + CommandGroup { + commands: commands.into_iter().collect(), + command_data: data, + current_idx: None, + has_initialized: false, + has_cancelled: false, + } + } +} + +impl Command for CommandGroup { + fn init(&mut self) { + if self.current_idx.is_some() { + error!("CommandGroup init called twice"); + } + self.current_idx = Some(0); + self.has_initialized = false; + self.has_cancelled = false; + debug!("CommandGroup {} init", self.command_data().name()); + } + + fn step(&mut self) -> CommandResult { + use self::CommandResult::*; + if self.has_cancelled { + debug!("CommandGroup step called after cancelled"); + return CommandResult::Cancel; + } + let mut current_idx = if let Some(c) = self.current_idx { + c + } else { + error!("CommandGroup step called before init"); + self.has_cancelled = true; + return CommandResult::Cancel; + }; + let mut result: CommandResult; + loop { + if current_idx >= self.commands.len() { + debug!("CommandGroup out of commands"); + return CommandResult::Done; + } + let mut command = self.commands[current_idx].borrow_mut(); + if !self.has_initialized { + debug!( + "CommandGroup init {0} ({1})", + current_idx, + command.command_data().name() + ); + command.init(); + self.has_initialized = true; + } + result = command.step(); + if result != Continue { + debug!( + "CommandGroup {2:?} {0} ({1}) ", + current_idx, + command.command_data().name(), + result + ); + command.finish(result) + } + match result { + Done => { + current_idx += 1; + self.has_initialized = false; + } + Cancel => { + self.has_cancelled = true; + break; + } + _ => break, + } + } + self.current_idx = Some(current_idx); + CommandResult::Continue + } + + fn finish(&mut self, result: CommandResult) { + if result == CommandResult::Cancel { + if let Some(command) = self + .current_idx + .and_then(|current_idx| self.commands.get(current_idx)) + { + command.borrow_mut().finish(result); + } + } + self.current_idx = None; + } + + fn command_data(&self) -> &CommandData { + &self.command_data + } +} + pub struct CommandRun { initialized: bool, cancelled: bool, @@ -81,18 +258,30 @@ impl CommandRun { } pub struct CommandScheduler { - running_commands: Vec>, + running_commands: Vec, } impl CommandScheduler { pub fn new() -> Self { - CommandScheduler{ running_commands: Vec::new() } + CommandScheduler { + running_commands: Vec::new(), + } } - pub fn get_command_run(&self, id: T) -> Option<&RefCell> { + pub fn get_command_run(&self, id: T) -> Option<&CommandRun> { let id = id.command_id(); for command_run in &self.running_commands { - if command_run.borrow().command_id() == id { + if command_run.command_id() == id { + return Some(command_run); + } + } + None + } + + fn get_command_run_mut(&mut self, id: T) -> Option<&mut CommandRun> { + let id = id.command_id(); + for command_run in &mut self.running_commands { + if command_run.command_id() == id { return Some(command_run); } } @@ -111,40 +300,47 @@ impl CommandScheduler { if self.is_running(command.command_id()) { false } else { - self.running_commands.push(RefCell::new(CommandRun{ - initialized: false, cancelled: false, command: command, - })); + self.running_commands.push(CommandRun { + initialized: false, + cancelled: false, + 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 - }) + self.get_command_run_mut(id).map_or(false, |command_run| { + command_run.cancelled = true; + true + }) } - + pub fn execute(&mut self) { - self.running_commands.retain(|command_run| { - let mut command_run = command_run.borrow_mut(); + self.running_commands.drain_filter(|command_run| { + let mut command = command_run.command.borrow_mut(); if !command_run.initialized && !command_run.cancelled { - command_run.command.borrow_mut().init(); + { + let command_data = command.command_data(); + info!( + "initialzing command {} (id {})", + command_data.name, command_data.id + ); + } + command.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 + let result = if command_run.initialized { + command.step() } else { + CommandResult::Cancel + }; + if result != CommandResult::Continue { + command.finish(result); true + } else { + false } - }) + }); } } diff --git a/src/drive.rs b/src/drive.rs index 2fab80c..6331831 100644 --- a/src/drive.rs +++ b/src/drive.rs @@ -1,9 +1,9 @@ -use std::rc::Rc; use std::cell::RefCell; +use std::rc::Rc; +use motor_controller::MockMotorController; use motor_controller::MotorControllerBox; use motor_controller::MotorControllerBoxed; -use motor_controller::MockMotorController; pub struct Drive { left_motors: Vec, @@ -20,7 +20,10 @@ impl Drive { pub fn new() -> Self { let left_motors = create_motor_controllers(0u16..4); let right_motors = create_motor_controllers(4u16..8); - Drive { left_motors, right_motors } + Drive { + left_motors, + right_motors, + } } pub fn new_ref() -> DriveRef { @@ -36,4 +39,3 @@ impl Drive { } } } - diff --git a/src/main.rs b/src/main.rs index abf0aec..9ea85ef 100644 --- a/src/main.rs +++ b/src/main.rs @@ -1,12 +1,21 @@ -mod robot; +#![feature(drain_filter)] + +#[macro_use] +extern crate log; +extern crate env_logger; + +#[macro_use] +mod command; mod drive; mod motor_controller; -mod command; +mod robot; use robot::Robot; fn main() { - let mut robot = Robot::new(); + env_logger::init(); + + let robot = Robot::new(); robot.run(); } diff --git a/src/motor_controller.rs b/src/motor_controller.rs index 15e3da5..0b73185 100644 --- a/src/motor_controller.rs +++ b/src/motor_controller.rs @@ -27,7 +27,7 @@ pub struct MockMotorController { impl MotorControllerId for MockMotorController { fn new(id: u16) -> Self { - MockMotorController{ id: id, value: 0.0 } + MockMotorController { id, value: 0.0 } } } @@ -38,7 +38,7 @@ impl MotorController for MockMotorController { fn set(&mut self, value: f64) { self.value = value; - println!("MotorController id {} value={}", self.id, self.value); + debug!("MotorController id {} value={}", self.id, self.value); } fn get(&self) -> f64 { diff --git a/src/robot.rs b/src/robot.rs index f0faf4e..07efac7 100644 --- a/src/robot.rs +++ b/src/robot.rs @@ -1,42 +1,54 @@ -use std::time::Duration; -use std::thread; use std::cell::RefCell; use std::rc::Rc; +use std::thread; +use std::time::Duration; +use command::{CommandData, CommandGroup, CommandRef, CommandScheduler}; use drive::{Drive, DriveRef}; -use command::{Command, CommandData, CommandScheduler, CommandResult}; -struct DriveCommand { - iterations: u64, - data: CommandData, - drive: DriveRef, -} +mod cmds { + use command::{CommandData, CommandResult}; + use drive::DriveRef; -impl DriveCommand { - fn new(data: CommandData, drive: DriveRef) -> Self { - Self { iterations: 0, data: data, drive: drive } + pub struct DriveCommand { + command_data: CommandData, + iterations: u64, + drive: DriveRef, } -} -impl Command for DriveCommand { - fn init(&mut self) { - self.iterations = 0; - println!("DriveCommand init"); + impl DriveCommand { + pub fn new(command_data: CommandData, drive: DriveRef) -> Self { + Self { + command_data, + iterations: 0, + drive, + } + } } - 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 } - } + command_impl! { DriveCommand.command_data; + self => { + self.iterations = 0; - fn finish(&mut self, result: CommandResult) { - println!("DriveCommand finish interrupted: {:?}", result); + }; self =>{ + self.drive.borrow_mut().drive_powers(1.0, 1.0); + self.iterations += 1; + if self.iterations >= 10 { + CommandResult::Done + } else { + CommandResult::Continue + } + + }; self, res => { + self.drive.borrow_mut().drive_powers(0.0,0.0); + } + } + pub struct LogCmd { + pub command_data: crate::command::CommandData, } - fn command_data(&self) -> &CommandData { - &self.data + command_impl! { + LogCmd.command_data; self => { info!("{}", "LogCmd") } } } @@ -45,23 +57,32 @@ pub struct Robot { command_scheduler: CommandScheduler, } -const ITER_PERIOD: Duration = Duration::from_nanos(1000000000 / 50); +const ITER_PERIOD: Duration = Duration::from_nanos(1_000_000_000 / 50); impl Robot { pub fn new() -> Self { - Robot{ + Robot { drive: Drive::new_ref(), command_scheduler: CommandScheduler::new(), } } pub fn run(mut self) { - println!("Starting RobotRS"); + warn!("Starting RobotRS"); - let command_data = CommandData { id: 0, name: "DriveCommand".into() }; - let command = Rc::new(RefCell::new(DriveCommand::new(command_data, self.drive.clone()))); + 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_group = CommandGroup::new( + CommandData::new(1, "Group"), + vec![command_ref, log_command_ref], + ); + let command_ref: CommandRef = Rc::new(RefCell::new(command_group)); - self.command_scheduler.start(command); + self.command_scheduler.start(command_ref); loop { self.execute();