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
 82
 83
 84
 85
 86
 87
 88
 89
 90
 91
 92
 93
 94
 95
 96
 97
 98
 99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123

use std::io::{BufRead, BufReader, BufWriter, Error, Read, Write};
use std::io;
use std::thread::sleep;
use std::time::{Duration, Instant};
use crate::enumerations::AxisState;

#[cfg(test)]
mod tests;

/// Represents a connection with an ODrive motor controller.
pub struct ODrive<T> where T: Read + Write {
    remote: T
}

impl<T> ODrive<T> where T: Read + Write {
    /// Constructs a new ODrive connection using the `ReadWriteCloningDecorator`.
    /// This method of construction will have consequences in overhead.
    /// It should only be used when it is not possible to clone the type `T`.
    pub fn new(serial: T) -> Self {
        Self { remote: serial }
    }
}

impl<T> Write for ODrive<T> where T: Write + Read {
    fn write(&mut self, buf: &[u8]) -> Result<usize, Error> {
        self.remote.write(buf)
    }

    fn flush(&mut self) -> Result<(), Error> {
        self.remote.flush()
    }
}

impl<T> Read for ODrive<T> where T: Read + Write {
    fn read(&mut self, buf: &mut [u8]) -> Result<usize, Error> {
        self.remote.read(buf)
    }
}

impl<T> ODrive<T> where T: Read + Write {
    pub fn set_position(&mut self, motor_number: u8, position: f32, velocity_feed_forward: Option<f32>, current_feed_forward: Option<f32>) -> io::Result<()> {
        assert!(motor_number < 2);
        let velocity_feed_forward = velocity_feed_forward.unwrap_or_default();
        let current_feed_forward = current_feed_forward.unwrap_or_default();
        writeln!(self.remote, "p {} {} {} {}", motor_number, position, velocity_feed_forward, current_feed_forward)?;
        self.flush()
    }

    pub fn set_velocity(&mut self, motor_number: u8, position: f32, current_feed_forward: Option<f32>) -> io::Result<()> {
        assert!(motor_number < 2);
        let current_feed_forward = current_feed_forward.unwrap_or_default();
        writeln!(self.remote, "v {} {} {}", motor_number, position, current_feed_forward)?;
        self.flush()
    }

    pub fn set_current(&mut self, motor_number: u8, current: f32) -> io::Result<()> {
        assert!(motor_number < 2);
        writeln!(self.remote, "c {} {}", motor_number, current)?;
        self.flush()
    }

    pub fn trapezoidal_move(&mut self, motor_number: u8, position: f32) -> io::Result<()> {
        assert!(motor_number < 2);
        writeln!(self.remote, "t {} {}", motor_number, position)?;
        self.flush()
    }

    pub fn get_velocity(&mut self, motor_number: u8) -> io::Result<f32> {
        assert!(motor_number < 2);
        writeln!(self.remote, "r axis{} .encoder.vel_estimate", motor_number)?;
        self.flush()?;
        self.read_float()
    }

    pub fn read_float(&mut self) -> io::Result<f32> {
        let read_str = self.read_string()?;
        println!("{}", read_str);
        Ok(read_str.parse().unwrap())
    }

    pub fn read_int(&mut self) -> io::Result<i32> {
        Ok(self.read_string()?.parse().unwrap_or_default())
    }

    pub fn run_state(&mut self, axis: u8, requested_state: AxisState, wait: bool) -> io::Result<bool> {
        let mut timeout_ctr = 100;
        writeln!(self.remote, "w axis{}.requested_state {}", axis, requested_state as u8)?;
        self.flush()?;
        if wait {
            while {
                sleep(Duration::from_millis(100));
                writeln!(self.remote, "r axis{}.current_state", axis)?;
                self.flush()?;
                timeout_ctr -= 1;
                self.read_int().unwrap_or_default() != AxisState::Idle as i32 && timeout_ctr > 0
            } {}
        }

        Ok(timeout_ctr > 0)
    }

    pub fn read_string(&mut self) -> io::Result<String> {
        let mut string = String::with_capacity(20);
        let duration = Instant::now();
        loop {
            let mut buffer = [0; 1];
            while self.remote.read(&mut buffer).unwrap_or_default() == 0 {
                if duration.elapsed().as_millis() >= 1_000 {
                    return Ok(string);
                }
            }
            let ch = buffer[0];
            if ch as char == '\n' {
                break;
            }

            string.push(ch as char);
        }

//        println!("{}", string);
        Ok(string.trim().to_owned())
    }
}