Check status in check-firmware

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