fix

Author: gbr1

README.md

@@ -1,2 +1,2 @@
-# Arduino_Robot_Firmware
+# Arduino_Alvik_Firmware
 firmware

library.properties

@@ -6,5 +6,6 @@ sentence=Library and firmware for Arduino Alvik
 paragraph=This library is used to build the firmware used by Arduino Alvik, examples show you how to customize the firmware.
 category=Robotics
 url=https://github.com/gbr1/Arduino_Alvik_Firmware
+architectures=stm32
 includes=Arduino_Alvik_Firmware.h
 depends=Arduino_APDS9960, Arduino_MAX17332, STM32duino_LSM6DSO, STM32duino_MotionFX, STM32duino_VL53L7CX, AT42QT, ucPack

src/Arduino_Alvik_Firmware.cpp

@@ -620,19 +620,20 @@ void Arduino_Alvik_Firmware::rotate(const float angle){
     float initial_angle=kinematics->getTheta();
     float error=angle-initial_angle;
     unsigned long t=millis();
-    while(abs(error)>0){
+    while(abs(error)>2){
         if (millis()-t>20){
             t=millis();
             updateMotors();
+            kinematics->updatePose(motor_control_left->getAngle(),motor_control_right->getAngle());
+            error=angle-kinematics->getTheta();
+            Serial.println(error);
         }
         if (error>0){
-            drive(0,90);
+            drive(0,40);
         }else{
-            drive(0,-90);
+            drive(0,-40);
         }
-        kinematics->updatePose(motor_control_left->getTravel(),motor_control_right->getTravel());
-        error=kinematics->getTheta()-initial_angle;
-        Serial.println(error);
+
     }
     drive(0,0);
     updateMotors();

src/motor_control/motor_control.h

@@ -42,6 +42,7 @@ class MotorControl{
         float kd;
 
         float travel;
+        float angle;
         float reference;
 
         float trip;
@@ -88,6 +89,7 @@ class MotorControl{
             controller_period = _controller_period;
 
             travel=0.0;
+            angle=0.0;
             reference = 0.0;
 
             trip=0.0;
@@ -224,7 +226,8 @@ class MotorControl{
 
             measure = encoder->getCount();
             encoder->reset();
-            travel += measure*conversion_factor_travel;
+            angle = measure*conversion_factor_travel;
+            travel += angle;
             measure = measure*conversion_factor;
 
             /* experimental
@@ -307,6 +310,10 @@ class MotorControl{
             return travel;
         }
 
+        float getAngle(){
+            return angle;
+        }
+
 
 };