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
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158
159
160
161
162
163
164
165
166
167
168
169
170
171
172

use std::thread;
use std::time::Duration;

use rppal::i2c::I2c;

const THUNDERBORG_I2C_ADDR: u16 = 0x15;

const COMMAND_ANALOG_MAX: u16 = 0x3FF;
const VOLTAGE_PIN_MAX: f32 = 36.3;

const SET_LED_CMD: u8 = 5;
const GET_BATT_VOLTAGE_CMD: u8 = 21;
const SET_LEFT_MOTORS_PWR_FWD_CMD: u8 = 8;
const SET_LEFT_MOTORS_PWR_REV_CMD: u8 = 9; 
const SET_RIGHT_MOTORS_PWR_FWD_CMD: u8 = 11;
const SET_RIGHT_MOTORS_PWR_REV_CMD: u8 = 12;    
const ALL_MOTORS_OFF_CMD: u8 = 14;

const MAX_MOTOR_POWER: u8 = 242;

pub struct ThunderborgDriver
{
    i2c_conn: I2c, 
}

impl ThunderborgDriver
{
    pub fn new() -> Result<Self, rppal::i2c::Error>
    {
        let mut i2c = I2c::new()?;
        i2c.set_slave_address(THUNDERBORG_I2C_ADDR)?;

        Ok(Self{i2c_conn: i2c,})
    }

    pub fn drive_fwd_for_ms(&self, num_milliseconds: u64) -> Result<(), rppal::i2c::Error>
    {
        self.set_left_motors_pwr_fwd(MAX_MOTOR_POWER)?;
        self.set_right_motors_pwr_fwd(MAX_MOTOR_POWER)?;
        let sleep_time = Duration::from_millis(num_milliseconds);
        thread::sleep(sleep_time);
        self.motors_off()
    }

    pub fn drive_rev_for_ms(&self, num_milliseconds: u64) -> Result<(), rppal::i2c::Error>
    {
        self.set_left_motors_pwr_rev(MAX_MOTOR_POWER)?;
        self.set_right_motors_pwr_rev(MAX_MOTOR_POWER)?;
        let sleep_time = Duration::from_millis(num_milliseconds);
        thread::sleep(sleep_time);
        self.motors_off()
    }

    pub fn spin_left_for_ms(&self, num_milliseconds: u64) -> Result<(), rppal::i2c::Error>
    {
        self.set_left_motors_pwr_fwd(MAX_MOTOR_POWER)?;
        self.set_right_motors_pwr_rev(MAX_MOTOR_POWER)?;
        let sleep_time = Duration::from_millis(num_milliseconds);
        thread::sleep(sleep_time);
        self.motors_off()
    }

    pub fn spin_right_for_ms(&self, num_milliseconds: u64) -> Result<(), rppal::i2c::Error>
    {
        self.set_right_motors_pwr_fwd(MAX_MOTOR_POWER)?;
        self.set_left_motors_pwr_rev(MAX_MOTOR_POWER)?;
        let sleep_time = Duration::from_millis(num_milliseconds);
        thread::sleep(sleep_time);
        self.motors_off()
    }

    pub fn set_left_motors_pwr_fwd(&self, power_input: u8) -> Result<(), rppal::i2c::Error>
    {
        let mut set_power: [u8;1] = [0];

        if power_input > MAX_MOTOR_POWER
        {
            set_power[0] = MAX_MOTOR_POWER;
        }
        else
        {
            set_power[0] = power_input;
        }
        self.i2c_conn.block_write(SET_LEFT_MOTORS_PWR_FWD_CMD, &set_power)?;

        Ok(())
    }

    pub fn set_right_motors_pwr_fwd(&self, power_input: u8) -> Result<(), rppal::i2c::Error>
    {
        let mut set_power: [u8;1] = [0];

        if power_input > MAX_MOTOR_POWER
        {
            set_power[0] = MAX_MOTOR_POWER;
        }
        else
        {
            set_power[0] = power_input;
        }
        self.i2c_conn.block_write(SET_RIGHT_MOTORS_PWR_FWD_CMD, &set_power)?;

        Ok(())
    }

    pub fn set_left_motors_pwr_rev(&self, power_input: u8) -> Result<(), rppal::i2c::Error>
    {
        let mut set_power: [u8;1] = [0];

        if power_input > MAX_MOTOR_POWER
        {
            set_power[0] = MAX_MOTOR_POWER;
        }
        else
        {
            set_power[0] = power_input;
        }
        self.i2c_conn.block_write(SET_LEFT_MOTORS_PWR_REV_CMD, &set_power)?;

        Ok(())
    }

    pub fn set_right_motors_pwr_rev(&self, power_input: u8) -> Result<(), rppal::i2c::Error>
    {
        let mut set_power: [u8;1] = [0];

        if power_input > MAX_MOTOR_POWER
        {
            set_power[0] = MAX_MOTOR_POWER;
        }
        else
        {
            set_power[0] = power_input;
        }
        self.i2c_conn.block_write(SET_RIGHT_MOTORS_PWR_REV_CMD, &set_power)?;

        Ok(())
    }

    pub fn motors_off(&self) -> Result<(), rppal::i2c::Error>
    {
        //No data for this command
        let data: [u8;1] = [0];
        self.i2c_conn.block_write(ALL_MOTORS_OFF_CMD, &data)?;

        Ok(())
    }

    pub fn set_led_color(&self, red:u8, green: u8, blue: u8) -> Result<(), rppal::i2c::Error>
    {
        let set_rgb: [u8;3] = [red, green, blue];
        self.i2c_conn.block_write(SET_LED_CMD, &set_rgb)?;

        Ok(())
    }

    pub fn get_battery_voltage(&self) -> Result<f32, rppal::i2c::Error>
    {
        let mut read_buffer: [u8;4] = [0, 0, 0, 0];
        self.i2c_conn.block_read(GET_BATT_VOLTAGE_CMD, &mut read_buffer)?;
    
        //Have to convert u8 values to u32 so that we don't open up potential for left-shift overflow
        let v1: u32 = read_buffer[1].into();
        let v2: u32 = read_buffer[2].into();
        let raw = ((v1 << 8) + v2) as f32;

        let level = raw/(COMMAND_ANALOG_MAX as f32);
        let voltage = level*VOLTAGE_PIN_MAX;
        
        Ok(voltage)
    }    
}