extern crate cursive;
extern crate drs_0x01;
extern crate serialport;
#[macro_use]
extern crate clap;
mod cli;
mod ui;
use cursive::CursiveExt;
use cli::{parse_args, Message};
use ui::init;
use drs_0x01::addr::{ReadableRamAddr, WritableEEPAddr};
use drs_0x01::builder::HerkulexMessage;
use drs_0x01::reader::{ACKReader, Command};
use drs_0x01::{Rotation, Servo};
use serialport::{open_with_settings, SerialPort, SerialPortSettings};
use std::io;
use std::io::Read;
use std::{thread, time};
fn show_send_message<S: Into<String>>(what: S, message: &HerkulexMessage) {
println!("{}", what.into());
println!("Sending : {:#x?}", message);
}
fn main() {
include_str!("../Cargo.toml");
let app = parse_args();
let matches = app.get_matches();
if matches.is_present("gui") {
let mut id = None;
if let Some(val) = matches.value_of("id") {
id = Some(val.parse::<u8>().unwrap());
}
let mut ui = init(id);
ui.run();
} else if matches.is_present("completions") {
let sub_matches = matches.subcommand_matches("completions").unwrap();
let shell = sub_matches.value_of("SHELL").unwrap();
parse_args().gen_completions_to(
"herkulex_communicator",
shell.parse().unwrap(),
&mut io::stdout(),
);
} else if matches.is_present("message") {
let matches = matches.subcommand_matches("message").unwrap();
let mut connection = {
let mut r = SerialPortSettings::default();
r.baud_rate = 115_200;
open_with_settings(matches.value_of("connection").unwrap(), &r).unwrap()
};
let id = matches.value_of("id").unwrap().parse::<u8>().unwrap();
let message = value_t_or_exit!(matches, "command", Message);
match message {
Message::SetPosition => {
let pos = matches
.value_of("desired_position")
.unwrap()
.parse::<u16>()
.unwrap();
let message = Servo::new(id).set_position(pos);
show_send_message(
format!(
"Asking the servomotor {} to got to the absolute position :{}.",
id, pos
),
&message,
);
send_message(&mut connection, message);
}
Message::SetSpeed => {
let speed = matches
.value_of("desired_speed")
.unwrap()
.parse::<u16>()
.unwrap();
let message = Servo::new(id).set_speed(speed, Rotation::Clockwise);
show_send_message(
format!(
"Asking the servomotor {} to set its speed to : {}.",
id, speed
),
&message,
);
send_message(&mut connection, message);
}
Message::Reboot => {
let message = Servo::new(id).reboot();
show_send_message(format!("Asking the servomotor {} to reboot.", id), &message);
send_message(&mut connection, message);
}
Message::EnableTorque => {
let message = Servo::new(id).enable_torque();
show_send_message(
format!("Asking the servomotor {} to enable its torque.", id),
&message,
);
send_message(&mut connection, message);
}
Message::DisableTorque => {
let message = Servo::new(id).disable_torque();
show_send_message(
format!("Asking the servomotor {} to disable its torque.", id),
&message,
);
send_message(&mut connection, message);
}
Message::ClearErrors => {
let message = Servo::new(id).clear_errors();
show_send_message(
format!("Asking the servomotor {} to clear its error register.", id),
&message,
);
send_message(&mut connection, message);
}
Message::SetId => {
let new_id = matches.value_of("set_id").unwrap().parse::<u8>().unwrap();
let message = Servo::new(id).eep_write(WritableEEPAddr::ID(new_id));
show_send_message(
format!(
"Asking the servomotor {} to change his id to : {}.",
id, new_id
),
&message,
);
send_message(&mut connection, message);
}
Message::ForceSetId => {
let new_id = matches.value_of("set_id").unwrap().parse::<u8>().unwrap();
for i in 0..=253 {
println!(
"Asking the servomotor {} to change his id to : {}.",
i, new_id
);
let message = Servo::new(i).eep_write(WritableEEPAddr::ID(new_id));
send_message(&mut connection, message);
}
}
Message::GetId => {
let message = Servo::new(id).ram_request(ReadableRamAddr::ID);
send_message(&mut connection, message);
let mut reader = ACKReader::new();
for b in connection.bytes() {
if let Ok(b) = b {
reader.parse(&[b]);
if let Some(p) = reader.pop_ack_packet() {
if let Command::RamRead { data } = p.cmd {
if let ReadableRamAddr::ID = data.addr {
println!("The ID of the servomotor is {}", data.data[0]);
}
}
}
} else {
break;
}
}
}
Message::GetPosition => {
let message = Servo::new(id).ram_request(ReadableRamAddr::AbsolutePosition);
send_message(&mut connection, message);
let mut reader = ACKReader::new();
for b in connection.bytes() {
if let Ok(b) = b {
reader.parse(&[b]);
if let Some(p) = reader.pop_ack_packet() {
if let Command::RamRead { data } = p.cmd {
if let ReadableRamAddr::AbsolutePosition = data.addr {
if data.data_len == 2 {
println!(
"The absolute position of the servomotor is {}",
(data.data[0] as u32 + ((data.data[1] as u32) << 8))
);
}
}
}
}
} else {
break;
}
}
}
Message::GetTorque => {
let message = Servo::new(id).ram_request(ReadableRamAddr::PWM);
send_message(&mut connection, message);
let mut reader = ACKReader::new();
for b in connection.bytes() {
if let Ok(b) = b {
reader.parse(&[b]);
if let Some(p) = reader.pop_ack_packet() {
if let Command::RamRead { data } = p.cmd {
if let ReadableRamAddr::PWM = data.addr {
if data.data_len == 2 {
println!(
"The absolute position of the servomotor is {}",
(data.data[0] as u32 + ((data.data[1] as u32) << 8))
);
}
}
}
}
} else {
break;
}
}
}
}
}
}
fn send_message(connection: &mut Box<dyn SerialPort>, message: HerkulexMessage) {
match connection.write(&message) {
Ok(_) => {}
Err(e) => panic!("Failed to send message : {}", e),
}
connection.flush().expect("Failed to flush");
let ten_millis = time::Duration::from_millis(1);
thread::sleep(ten_millis);
}