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

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 = 255;

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 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 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 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)
    }    
}