thunderborg_driver/
lib.rs

1use std::thread;
2use std::time::Duration;
3
4use rppal::i2c::I2c;
5
6const THUNDERBORG_I2C_ADDR: u16 = 0x15;
7
8const COMMAND_ANALOG_MAX: u16 = 0x3FF;
9const VOLTAGE_PIN_MAX: f32 = 36.3;
10
11const SET_LED_CMD: u8 = 5;
12const GET_BATT_VOLTAGE_CMD: u8 = 21;
13const SET_LEFT_MOTORS_PWR_FWD_CMD: u8 = 8;
14const SET_LEFT_MOTORS_PWR_REV_CMD: u8 = 9; 
15const SET_RIGHT_MOTORS_PWR_FWD_CMD: u8 = 11;
16const SET_RIGHT_MOTORS_PWR_REV_CMD: u8 = 12;    
17const ALL_MOTORS_OFF_CMD: u8 = 14;
18
19const MAX_MOTOR_POWER: u8 = 242;
20
21pub struct ThunderborgDriver
22{
23    i2c_conn: I2c, 
24}
25
26impl ThunderborgDriver
27{
28    pub fn new() -> Result<Self, rppal::i2c::Error>
29    {
30        let mut i2c = I2c::new()?;
31        i2c.set_slave_address(THUNDERBORG_I2C_ADDR)?;
32
33        Ok(Self{i2c_conn: i2c,})
34    }
35
36    pub fn drive_fwd_for_ms(&self, num_milliseconds: u64) -> Result<(), rppal::i2c::Error>
37    {
38        self.set_left_motors_pwr_fwd(MAX_MOTOR_POWER)?;
39        self.set_right_motors_pwr_fwd(MAX_MOTOR_POWER)?;
40        let sleep_time = Duration::from_millis(num_milliseconds);
41        thread::sleep(sleep_time);
42        self.motors_off()
43    }
44
45    pub fn drive_rev_for_ms(&self, num_milliseconds: u64) -> Result<(), rppal::i2c::Error>
46    {
47        self.set_left_motors_pwr_rev(MAX_MOTOR_POWER)?;
48        self.set_right_motors_pwr_rev(MAX_MOTOR_POWER)?;
49        let sleep_time = Duration::from_millis(num_milliseconds);
50        thread::sleep(sleep_time);
51        self.motors_off()
52    }
53
54    pub fn spin_left_for_ms(&self, num_milliseconds: u64) -> Result<(), rppal::i2c::Error>
55    {
56        self.set_left_motors_pwr_fwd(MAX_MOTOR_POWER)?;
57        self.set_right_motors_pwr_rev(MAX_MOTOR_POWER)?;
58        let sleep_time = Duration::from_millis(num_milliseconds);
59        thread::sleep(sleep_time);
60        self.motors_off()
61    }
62
63    pub fn spin_right_for_ms(&self, num_milliseconds: u64) -> Result<(), rppal::i2c::Error>
64    {
65        self.set_right_motors_pwr_fwd(MAX_MOTOR_POWER)?;
66        self.set_left_motors_pwr_rev(MAX_MOTOR_POWER)?;
67        let sleep_time = Duration::from_millis(num_milliseconds);
68        thread::sleep(sleep_time);
69        self.motors_off()
70    }
71
72    pub fn set_left_motors_pwr_fwd(&self, power_input: u8) -> Result<(), rppal::i2c::Error>
73    {
74        let mut set_power: [u8;1] = [0];
75
76        if power_input > MAX_MOTOR_POWER
77        {
78            set_power[0] = MAX_MOTOR_POWER;
79        }
80        else
81        {
82            set_power[0] = power_input;
83        }
84        self.i2c_conn.block_write(SET_LEFT_MOTORS_PWR_FWD_CMD, &set_power)?;
85
86        Ok(())
87    }
88
89    pub fn set_right_motors_pwr_fwd(&self, power_input: u8) -> Result<(), rppal::i2c::Error>
90    {
91        let mut set_power: [u8;1] = [0];
92
93        if power_input > MAX_MOTOR_POWER
94        {
95            set_power[0] = MAX_MOTOR_POWER;
96        }
97        else
98        {
99            set_power[0] = power_input;
100        }
101        self.i2c_conn.block_write(SET_RIGHT_MOTORS_PWR_FWD_CMD, &set_power)?;
102
103        Ok(())
104    }
105
106    pub fn set_left_motors_pwr_rev(&self, power_input: u8) -> Result<(), rppal::i2c::Error>
107    {
108        let mut set_power: [u8;1] = [0];
109
110        if power_input > MAX_MOTOR_POWER
111        {
112            set_power[0] = MAX_MOTOR_POWER;
113        }
114        else
115        {
116            set_power[0] = power_input;
117        }
118        self.i2c_conn.block_write(SET_LEFT_MOTORS_PWR_REV_CMD, &set_power)?;
119
120        Ok(())
121    }
122
123    pub fn set_right_motors_pwr_rev(&self, power_input: u8) -> Result<(), rppal::i2c::Error>
124    {
125        let mut set_power: [u8;1] = [0];
126
127        if power_input > MAX_MOTOR_POWER
128        {
129            set_power[0] = MAX_MOTOR_POWER;
130        }
131        else
132        {
133            set_power[0] = power_input;
134        }
135        self.i2c_conn.block_write(SET_RIGHT_MOTORS_PWR_REV_CMD, &set_power)?;
136
137        Ok(())
138    }
139
140    pub fn motors_off(&self) -> Result<(), rppal::i2c::Error>
141    {
142        //No data for this command
143        let data: [u8;1] = [0];
144        self.i2c_conn.block_write(ALL_MOTORS_OFF_CMD, &data)?;
145
146        Ok(())
147    }
148
149    pub fn set_led_color(&self, red:u8, green: u8, blue: u8) -> Result<(), rppal::i2c::Error>
150    {
151        let set_rgb: [u8;3] = [red, green, blue];
152        self.i2c_conn.block_write(SET_LED_CMD, &set_rgb)?;
153
154        Ok(())
155    }
156
157    pub fn get_battery_voltage(&self) -> Result<f32, rppal::i2c::Error>
158    {
159        let mut read_buffer: [u8;4] = [0, 0, 0, 0];
160        self.i2c_conn.block_read(GET_BATT_VOLTAGE_CMD, &mut read_buffer)?;
161    
162        //Have to convert u8 values to u32 so that we don't open up potential for left-shift overflow
163        let v1: u32 = read_buffer[1].into();
164        let v2: u32 = read_buffer[2].into();
165        let raw = ((v1 << 8) + v2) as f32;
166
167        let level = raw/(COMMAND_ANALOG_MAX as f32);
168        let voltage = level*VOLTAGE_PIN_MAX;
169        
170        Ok(voltage)
171    }    
172}