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

lots of command changes

master
Alex Mikhalev 6 years ago
parent
d81b02be47
commit
0723b5c6a4
8 changed files with 544 additions and 79 deletions
Whitespace
Show all changes Ignore whitespace when comparing lines Ignore changes in amount of whitespace Ignore changes in whitespace at EOL
Unified View
Diff Options
Show Stats Download Patch File Download Diff File
  1. 19
      .vscode/launch.json
  2. 216
      Cargo.lock
  3. 2
      Cargo.toml
  4. 270
      src/command.rs
  5. 10
      src/drive.rs
  6. 15
      src/main.rs
  7. 4
      src/motor_controller.rs
  8. 85
      src/robot.rs

19
.vscode/launch.json vendored
Unescape View File

@ -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}"
}
]
}

216
Cargo.lock generated
Unescape View File

@ -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]] [[package]]
name = "robotrs" name = "robotrs"
version = "0.1.0" 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"

2
Cargo.toml
Unescape View File

@ -4,3 +4,5 @@ version = "0.1.0"
authors = ["Alex Mikhalev <alexmikhalevalex@gmail.com>"] authors = ["Alex Mikhalev <alexmikhalevalex@gmail.com>"]
[dependencies] [dependencies]
log = "0.4"
env_logger = "0.6"

270
src/command.rs
Unescape View File

@ -1,5 +1,5 @@
use std::rc::Rc;
use std::cell::RefCell; use std::cell::RefCell;
use std::rc::Rc;
pub trait CommandId { pub trait CommandId {
fn command_id(&self) -> u64; fn command_id(&self) -> u64;
@ -11,7 +11,7 @@ impl CommandId for u64 {
} }
} }
#[derive(Debug, PartialEq)] #[derive(Debug, PartialEq, Clone, Copy)]
pub enum CommandResult { pub enum CommandResult {
Continue, Continue,
Done, Done,
@ -22,23 +22,94 @@ pub trait Command {
fn init(&mut self); fn init(&mut self);
fn step(&mut self) -> CommandResult; fn step(&mut self) -> CommandResult;
fn finish(&mut self, result: 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 { fn step(&mut $step_self) -> $crate::command::CommandResult {
&self.command_data().name 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<T: Command> CommandId for T { pub struct CommandData {
fn command_id(&self) -> u64 { id: u64,
self.command_data().id 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
} }
} }
pub struct CommandData { impl CommandId for CommandData {
pub id: u64, fn command_id(&self) -> u64 {
pub name: String, self.id
}
} }
pub type CommandRef = Rc<RefCell<dyn Command>>; pub type CommandRef = Rc<RefCell<dyn Command>>;
@ -49,6 +120,112 @@ impl CommandId for CommandRef {
} }
} }
pub struct CommandGroup {
commands: Vec<CommandRef>,
command_data: CommandData,
current_idx: Option<usize>,
has_initialized: bool,
has_cancelled: bool,
}
impl CommandGroup {
pub fn new<TCommands>(data: CommandData, commands: TCommands) -> Self
where
TCommands: IntoIterator<Item = CommandRef>,
{
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 { pub struct CommandRun {
initialized: bool, initialized: bool,
cancelled: bool, cancelled: bool,
@ -81,18 +258,30 @@ impl CommandRun {
} }
pub struct CommandScheduler { pub struct CommandScheduler {
running_commands: Vec<RefCell<CommandRun>>, running_commands: Vec<CommandRun>,
} }
impl CommandScheduler { impl CommandScheduler {
pub fn new() -> Self { pub fn new() -> Self {
CommandScheduler{ running_commands: Vec::new() } CommandScheduler {
running_commands: Vec::new(),
}
} }
pub fn get_command_run<T: CommandId>(&self, id: T) -> Option<&RefCell<CommandRun>> { pub fn get_command_run<T: CommandId>(&self, id: T) -> Option<&CommandRun> {
let id = id.command_id(); let id = id.command_id();
for command_run in &self.running_commands { 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<T: CommandId>(&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); return Some(command_run);
} }
} }
@ -111,40 +300,47 @@ impl CommandScheduler {
if self.is_running(command.command_id()) { if self.is_running(command.command_id()) {
false false
} else { } else {
self.running_commands.push(RefCell::new(CommandRun{ self.running_commands.push(CommandRun {
initialized: false, cancelled: false, command: command, initialized: false,
})); cancelled: false,
command,
});
true true
} }
} }
pub fn cancel<T: CommandId>(&mut self, id: T) -> bool { pub fn cancel<T: CommandId>(&mut self, id: T) -> bool {
self.get_command_run(id) self.get_command_run_mut(id).map_or(false, |command_run| {
.map_or(false, |command_run| { command_run.cancelled = true;
command_run.borrow_mut().cancelled = true; true
true })
})
} }
pub fn execute(&mut self) { pub fn execute(&mut self) {
self.running_commands.retain(|command_run| { self.running_commands.drain_filter(|command_run| {
let mut command_run = command_run.borrow_mut(); let mut command = command_run.command.borrow_mut();
if !command_run.initialized && !command_run.cancelled { 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; command_run.initialized = true;
} }
let result = let result = if command_run.initialized {
if command_run.initialized { command.step()
command_run.command.borrow_mut().step()
} else {
CommandResult::Cancel
};
if result != CommandResult::Continue {
command_run.command.borrow_mut().finish(result);
false
} else { } else {
CommandResult::Cancel
};
if result != CommandResult::Continue {
command.finish(result);
true true
} else {
false
} }
}) });
} }
} }

10
src/drive.rs
Unescape View File

@ -1,9 +1,9 @@
use std::rc::Rc;
use std::cell::RefCell; use std::cell::RefCell;
use std::rc::Rc;
use motor_controller::MockMotorController;
use motor_controller::MotorControllerBox; use motor_controller::MotorControllerBox;
use motor_controller::MotorControllerBoxed; use motor_controller::MotorControllerBoxed;
use motor_controller::MockMotorController;
pub struct Drive { pub struct Drive {
left_motors: Vec<MotorControllerBox>, left_motors: Vec<MotorControllerBox>,
@ -20,7 +20,10 @@ impl Drive {
pub fn new() -> Self { pub fn new() -> Self {
let left_motors = create_motor_controllers(0u16..4); let left_motors = create_motor_controllers(0u16..4);
let right_motors = create_motor_controllers(4u16..8); let right_motors = create_motor_controllers(4u16..8);
Drive { left_motors, right_motors } Drive {
left_motors,
right_motors,
}
} }
pub fn new_ref() -> DriveRef { pub fn new_ref() -> DriveRef {
@ -36,4 +39,3 @@ impl Drive {
} }
} }
} }

15
src/main.rs
Unescape View File

@ -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 drive;
mod motor_controller; mod motor_controller;
mod command; mod robot;
use robot::Robot; use robot::Robot;
fn main() { fn main() {
let mut robot = Robot::new(); env_logger::init();
let robot = Robot::new();
robot.run(); robot.run();
} }

4
src/motor_controller.rs
Unescape View File

@ -27,7 +27,7 @@ pub struct MockMotorController {
impl MotorControllerId for MockMotorController { impl MotorControllerId for MockMotorController {
fn new(id: u16) -> Self { 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) { fn set(&mut self, value: f64) {
self.value = value; self.value = value;
println!("MotorController id {} value={}", self.id, self.value); debug!("MotorController id {} value={}", self.id, self.value);
} }
fn get(&self) -> f64 { fn get(&self) -> f64 {

85
src/robot.rs
Unescape View File

@ -1,42 +1,54 @@
use std::time::Duration;
use std::thread;
use std::cell::RefCell; use std::cell::RefCell;
use std::rc::Rc; use std::rc::Rc;
use std::thread;
use std::time::Duration;
use command::{CommandData, CommandGroup, CommandRef, CommandScheduler};
use drive::{Drive, DriveRef}; use drive::{Drive, DriveRef};
use command::{Command, CommandData, CommandScheduler, CommandResult};
struct DriveCommand { mod cmds {
iterations: u64, use command::{CommandData, CommandResult};
data: CommandData, use drive::DriveRef;
drive: DriveRef,
}
impl DriveCommand { pub struct DriveCommand {
fn new(data: CommandData, drive: DriveRef) -> Self { command_data: CommandData,
Self { iterations: 0, data: data, drive: drive } iterations: u64,
drive: DriveRef,
} }
}
impl Command for DriveCommand { impl DriveCommand {
fn init(&mut self) { pub fn new(command_data: CommandData, drive: DriveRef) -> Self {
self.iterations = 0; Self {
println!("DriveCommand init"); command_data,
iterations: 0,
drive,
}
}
} }
fn step(&mut self) -> CommandResult { command_impl! { DriveCommand.command_data;
println!("DriveCommand step"); self => {
self.drive.borrow_mut().drive_powers(1.0, 1.0); self.iterations = 0;
self.iterations += 1;
if self.iterations >= 10 { CommandResult::Done } else { CommandResult::Continue }
}
fn finish(&mut self, result: CommandResult) { }; self =>{
println!("DriveCommand finish interrupted: {:?}", result); 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 { command_impl! {
&self.data LogCmd.command_data; self => { info!("{}", "LogCmd") }
} }
} }
@ -45,23 +57,32 @@ pub struct Robot {
command_scheduler: CommandScheduler, 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 { impl Robot {
pub fn new() -> Self { pub fn new() -> Self {
Robot{ Robot {
drive: Drive::new_ref(), drive: Drive::new_ref(),
command_scheduler: CommandScheduler::new(), command_scheduler: CommandScheduler::new(),
} }
} }
pub fn run(mut self) { pub fn run(mut self) {
println!("Starting RobotRS"); warn!("Starting RobotRS");
let command_data = CommandData { id: 0, name: "DriveCommand".into() }; let command = cmds::DriveCommand::new(CommandData::new(0, "DriveCommand"), self.drive.clone());
let command = Rc::new(RefCell::new(DriveCommand::new(command_data, 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 { loop {
self.execute(); self.execute();

Write Preview
Loading…
Cancel
Save