 
 
   // Open communication with Saberteeth
-  Serial3.begin(9600);
+  // Serial3.begin(9600);
+  
+  stopAllMotors(false);
   //unitTest();
 
 


  direction = getArticulationDirection(REAR_RIGHT_DRIVE_MOTOR_ID, wheelStatus.rr_articulation_angle, wheelTarget.rr_articulation_angle); //RR
  articulateWheel(REAR_RIGHT_DRIVE_MOTOR_ID, direction);

}

/**

    //   return;
    // }

    print("current status: start_driving");
    //Set stop drive time
    driveStopTime = millis() + wheelTarget.drive_duration*1000;
    
    print("after millis(), drive all motors");

    //Send drive start command
    driveAllMotors();
