thunderborg_driver/
lib.rs1use 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 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 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}