rustypot 1.4.2

Package to communicate with Dynamixel motors.
Documentation 
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81

use std::f64::consts::PI;
use std::time::SystemTime;
use std::{error::Error, thread, time::Duration};

use rustypot::servo::dynamixel::mx;
use rustypot::DynamixelProtocolHandler;

use clap::Parser;

use signal_hook::flag;

use std::sync::atomic::{AtomicBool, Ordering};
use std::sync::Arc;

#[derive(Parser, Debug)]
#[command(author, version, about, long_about = None)]
struct Args {
    /// tty
    #[arg(short, long)]
    serialport: String,
    /// baud
    #[arg(short, long, default_value_t = 1_000_000)]
    baudrate: u32,

    /// id
    #[arg(short, long)]
    id: u8,

    ///sinus amplitude (f64)
    #[arg(short, long, default_value_t = 10.0)]
    amplitude: f64,

    ///sinus frequency (f64)
    #[arg(short, long, default_value_t = 1.0)]
    frequency: f64,
}

fn main() -> Result<(), Box<dyn Error>> {
    let args = Args::parse();
    let serialportname: String = args.serialport;
    let baudrate: u32 = args.baudrate;
    let id: u8 = args.id;
    let amplitude: f64 = args.amplitude;
    let frequency: f64 = args.frequency;

    //print all the argument values
    println!("serialport: {}", serialportname);
    println!("baudrate: {}", baudrate);
    println!("id: {}", id);
    println!("amplitude: {}", amplitude);
    println!("frequency: {}", frequency);
    let term = Arc::new(AtomicBool::new(false));

    flag::register(signal_hook::consts::SIGINT, Arc::clone(&term))?;

    let mut serial_port = serialport::new(serialportname, baudrate)
        .timeout(Duration::from_millis(10))
        .open()?;
    println!("serial port opened");

    let io = DynamixelProtocolHandler::v1();

    let x = mx::read_present_position(&io, serial_port.as_mut(), id)?;
    println!("present pos: {}", x);

    mx::write_torque_enable(&io, serial_port.as_mut(), id, 1)?;

    let now = SystemTime::now();
    while !term.load(Ordering::Relaxed) {
        let t = now.elapsed().unwrap().as_secs_f64();
        let target = amplitude * (2.0 * PI * frequency * t).sin().to_radians();
        println!("target: {}", target);
        mx::write_goal_position(&io, serial_port.as_mut(), id, target)?;

        thread::sleep(Duration::from_millis(10));
    }
    // mx::write_torque_enable(&io, serial_port.as_mut(), id, false)?;
    mx::write_torque_enable(&io, serial_port.as_mut(), id, 0)?;

    Ok(())
}