diff --git a/libs/engine/src/motor.rs b/libs/engine/src/motor.rs index ddaa63f..2a52668 100644 --- a/libs/engine/src/motor.rs +++ b/libs/engine/src/motor.rs @@ -32,11 +32,14 @@ pub struct Motor> { in_1: A, in_2: B, pwm: P, + // In case the motor is connected with inverted pinout, this is a temporary fix and it'll later + // be fixed in the actual hardware + inverted: bool, } impl> Motor { - pub fn new(in_1: A, in_2: B, pwm: P) -> Self { - Motor { in_1, in_2, pwm } + pub fn new(in_1: A, in_2: B, pwm: P, inverted: bool) -> Self { + Motor { in_1, in_2, pwm, inverted } } } @@ -49,16 +52,16 @@ impl> MotorController for Moto // 3. Brake: in_1 = 1, in_2 = 1 # this creates a short circuit, it is not recommended to use it for a long time // 4. Stop: speed = 0 fn set_state(&mut self, state: MotorState) { - match state { - MotorState::Backward => { + match (self.inverted, state) { + (false, MotorState::Backward) | (true, MotorState::Forward) => { let _ = self.in_1.set_high(); let _ = self.in_2.set_low(); } - MotorState::Forward => { + (false, MotorState::Forward) | (true, MotorState::Backward) => { let _ = self.in_1.set_low(); let _ = self.in_2.set_high(); } - MotorState::Brake => { + (_, MotorState::Brake) => { let _ = self.in_1.set_high(); let _ = self.in_2.set_high(); } @@ -120,7 +123,7 @@ mod tests { let pwm_pin = MockFakePwmPin::new(); // when - let mut motor = Motor::new(in_1, in_2, pwm_pin); + let mut motor = Motor::new(in_1, in_2, pwm_pin, false); motor.forward(); } @@ -138,7 +141,25 @@ mod tests { let pwm_pin = MockFakePwmPin::new(); // when - let mut motor = Motor::new(in_1, in_2, pwm_pin); + let mut motor = Motor::new(in_1, in_2, pwm_pin, false); + motor.backward(); + } + + #[test] + fn test_inverted_motor() { + // given + let mut in_1 = MockFakePin::new(); + in_1.expect_set_low().times(1).returning(|| Ok(())); + in_1.expect_set_high().times(0).returning(|| Ok(())); + + let mut in_2 = MockFakePin::new(); + in_2.expect_set_low().times(0).returning(|| Ok(())); + in_2.expect_set_high().times(1).returning(|| Ok(())); + + let pwm_pin = MockFakePwmPin::new(); + + // when + let mut motor = Motor::new(in_1, in_2, pwm_pin, true); motor.backward(); } } diff --git a/mightybuga_bsc/src/lib.rs b/mightybuga_bsc/src/lib.rs index b1370de..3f5740b 100644 --- a/mightybuga_bsc/src/lib.rs +++ b/mightybuga_bsc/src/lib.rs @@ -159,11 +159,13 @@ impl Mightybuga_BSC { gpiob.pb5.into_push_pull_output(&mut gpiob.crl), gpioa.pa12.into_push_pull_output(&mut gpioa.crh), left_motor_channel, + true, ); let motor_right = Motor::new( gpiob.pb9.into_push_pull_output(&mut gpiob.crh), gpiob.pb8.into_push_pull_output(&mut gpiob.crh), right_motor_channel, + false, ); // Engine is the struct which contains all the logics regarding the motors