fix on firmware_01

Author: gbr1

examples/firmware_01/firmware_01.ino

@@ -135,7 +135,6 @@ void loop(){
   if (millis()-tmotor>20){
     tmotor=millis();
     alvik.updateMotors();
-    alvik.updateImu();
     msg_size = packeter.packetC2F('j', alvik.getRpmLeft(),alvik.getRpmRight());
     alvik.serial->write(packeter.msg,msg_size);
 

src/Arduino_AlvikCarrier.cpp

@@ -632,3 +632,29 @@ void Arduino_AlvikCarrier::rotate(const float angle){
     updateMotors();
 }
 
+/*
+void Arduino_AlvikCarrier::move(const float distance){
+    float initial_distance=kinematics->get();
+    float final_distance=distance+initial_distance;
+    float error=distance;
+    unsigned long t=millis();
+    while(abs(error)>2){
+        if (millis()-t>20){
+            t=millis();
+            updateMotors();
+            kinematics->inverse(motor_control_left->getRPM(),motor_control_right->getRPM());
+            kinematics->updatePose();
+            error=final_distance-kinematics->getTheta();
+            Serial.println(error);
+        }
+        if (error>0){
+            drive(40,0);
+        }else{
+            drive(-40,0);
+        }
+    }
+    drive(0,0);
+    updateMotors();
+    */
+}
+