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

//! http://cache.nxp.com/documents/data_sheet/PCA9685.pdf?pspll=1

extern crate tessel;

use std::io;
use std::thread;
use std::time::Duration;
use std::ops::Range;

#[repr(u8)]
#[allow(dead_code)]
#[derive(Copy, Clone)]
enum Command {
    MODE1 = 0x0,
    LED0_ON_L = 0x06,
    LED0_ON_H = 0x07,
    LED0_OFF_L = 0x08,
    LED0_OFF_H = 0x09,
    PRESCALE = 0xFE,
}

const MAX: u16 = 4096;
//const MODE1 = 0x0;
//const PRE_SCALE = 0xFE;

const I2C_ID: u8 = 0x73;

#[allow(dead_code)]
pub struct ServoArray<'a> {
    i2c: tessel::I2cPort<'a>,
    addr2: tessel::Pin<'a>,
    addr3: tessel::Pin<'a>,
    output_enable: tessel::Pin<'a>,
    range: Range<f64>,
    i2c_id: u8,
}

impl<'a> ServoArray<'a> {
    pub fn new<'b>(port: tessel::Port, addr2: bool, addr3: bool) -> ServoArray<'b> {
        let (i2c, gpio) = port.i2c();
        let (addr2, addr3, output_enable) = gpio.pin_select((5, 6, 7));

        ServoArray {
            i2c: i2c,
            addr2: addr2,
            addr3: addr3,
            output_enable: output_enable,
            range: 0.0..1.0,
            i2c_id: I2C_ID, // TODO
        }
    }

    /// Reads sequential buffers.
    //fn read(&mut self, values: &[Command], buf: &mut [u8]) -> io::Result<()> {
    //    let a: Vec<u8> = values.iter().map(|x| *x as u8).collect();
    //    try!(self.i2c.transfer(I2C_ID, &a, buf));
    //    Ok(())
    //}

    //fn write(&mut self, values: &[Command]) -> io::Result<()> {
    //    let mut a: Vec<u8> = values.iter().map(|x| *x as u8).collect();
    //    a.push(value);
    //    self.i2c.send(I2C_ID, &a);
    //    Ok(())
    //}

    pub fn connect(&mut self) -> io::Result<()> {
        // Enable the outputs.
        self.output_enable.output(false);

        //TODO
        self.addr2.output(false);
        self.addr3.output(false);

        //let mut buf = [0; 6];
        //println!("hi");
        //try!(self.read(&[Command::ReadId3, Command::ReadId4], &mut buf));
        //println!("hey");
        //println!("hi {:?}", buf);
        //if buf[0] != 0x14 {
        //    return Err(io::Error::new(io::ErrorKind::InvalidData, "Invalid connection code."))
        //}

        self.set_module_frequency(50);

        Ok(())
    }

    pub fn set_module_frequency(&mut self, frequency: u64) {
        let prescale: u8 = (((25000000 / (MAX as u64)) / frequency) - 1) as u8;

        let mut buf = [0; 1];
        self.i2c.transfer(self.i2c_id, &[Command::MODE1 as u8], &mut buf);
        let mode = buf[0];

        self.i2c.send(self.i2c_id, &[Command::MODE1 as u8, mode | 0x10]);
        self.i2c.send(self.i2c_id, &[Command::PRESCALE as u8, prescale]);
        self.i2c.send(self.i2c_id, &[Command::MODE1 as u8, mode]);
        self.i2c.send(self.i2c_id, &[Command::MODE1 as u8, 0xA1]);
    }

    /// Set duty cycle for entry 1 to 16.
    pub fn set_duty_cycle(&mut self, i: usize, value: f64) {
        let offset = ((i - 1) * 4) as u8;
        let reg = (((MAX - 1) as f64) * f64::max(f64::min(value, 1.0), 0.0)) as u16;
        println!("0 0 {:?} {:?}", (reg & 0xFF) as u8, ((reg >> 8) & 0xFF) as u8);
        self.i2c.send(self.i2c_id, &[Command::LED0_ON_L as u8 + offset, 0]);
        self.i2c.send(self.i2c_id, &[Command::LED0_ON_H as u8 + offset, 0]);
        self.i2c.send(self.i2c_id, &[Command::LED0_OFF_L as u8 + offset, (reg & 0xFF) as u8]);
        self.i2c.send(self.i2c_id, &[Command::LED0_OFF_H as u8 + offset, ((reg >> 8) & 0xFF) as u8]);
    }
}