diff --git a/tele0592/examples/check-firmware.rs b/tele0592/examples/check-firmware.rs index c0d264d3e9c7dd023359d0dba4a897111f350e34..c5682e7d648d78491a742e84916fae0883d11bd5 100644 --- a/tele0592/examples/check-firmware.rs +++ b/tele0592/examples/check-firmware.rs @@ -15,6 +15,11 @@ fn main() -> color_eyre::Result<()> { println!("- Check firmware version consistency"); c.check_firmware_version()?; + println!("- Check that robot is not moving and not in controlled mode"); + let status = c.get_status()?; + assert!(!status.is_moving()); + assert!(!status.is_controlled()); + println!("- Check that the encoders have been reset"); assert_eq!(c.get_encoder_ticks()?, (0, 0)); @@ -57,10 +62,19 @@ fn main() -> color_eyre::Result<()> { c.set_raw_motor_speed(Some(30), Some(-40))?; assert_eq!(c.get_raw_motor_speed()?.unwrap(), (30, -40)); + println!("- Check that the motors are moving and are not controlled"); + let status = c.get_status()?; + assert!(status.is_moving()); + assert!(!status.is_controlled()); + println!("- Check that the motor can be stopped"); c.set_raw_motor_speed(Some(0), Some(0))?; assert_eq!(c.get_raw_motor_speed()?.unwrap(), (0, 0)); + println!("- Check that the status reports not moving"); + let status = c.get_status()?; + assert!(!status.is_moving()); + println!("- Check that the motor can be put in standby mode"); c.set_raw_motor_speed(None, None)?; assert_eq!(c.get_raw_motor_speed()?, None); @@ -136,8 +150,13 @@ fn main() -> color_eyre::Result<()> { println!("- Check that controlled motor speed can be read back"); assert_eq!(c.get_motor_speed()?, Some((30, -30))); - println!("- Check that raw motor speed can be read in controlled mode"); + println!("- Check that the robot is moving and is controlled"); thread::sleep(Duration::from_millis(100)); + let status = c.get_status()?; + assert!(status.is_moving()); + assert!(status.is_controlled()); + + println!("- Check that raw motor speed can be read in controlled mode"); let (left, right) = c.get_raw_motor_speed()?.unwrap(); assert!(left > 0 && right < 0); @@ -145,6 +164,11 @@ fn main() -> color_eyre::Result<()> { thread::sleep(Duration::from_millis(250)); assert_eq!(c.get_raw_motor_speed()?, None); + println!("- Check that controlled mode has been left"); + let status = c.get_status()?; + assert!(!status.is_moving()); + assert!(!status.is_controlled()); + println!("- Check that a positive P goes into the right direction"); c.set_motor_shutdown_timeout(Duration::from_secs(1))?; c.get_encoder_ticks()?; // Reset ticks