mod: rot control on yaw TEST

Author: eigen-value

src/Arduino_AlvikCarrier.cpp

@@ -842,7 +842,8 @@ void Arduino_AlvikCarrier::updateKinematics(){
     kinematics->updatePose();
     if (kinematics_movement!=MOVEMENT_DISABLED){
         if (kinematics_movement==MOVEMENT_ROTATE){
-            rotate_pid->update(kinematics->getTheta());
+            //rotate_pid->update(kinematics->getTheta());
+            rotate_pid->update(getYaw());
             drive(0, round(rotate_pid->getControlOutput()/10.0)*10);
             if (abs(rotate_pid->getError())<ROTATE_THRESHOLD){
                 kinematics_achieved=true;
@@ -929,7 +930,8 @@ void Arduino_AlvikCarrier::rotate(const float angle){
     disableKinematicsMovement();
     kinematics_achieved=false;
     rotate_pid->reset();
-    rotate_pid->setReference(kinematics->getTheta()+angle);
+    //rotate_pid->setReference(kinematics->getTheta()+angle);
+    rotate_pid->setReference(kinematics->normalizeAngle(getYaw() + angle));
     kinematics_movement=MOVEMENT_ROTATE;
 }
 

src/robotics/kinematics.h

@@ -153,6 +153,16 @@ class Kinematics{
         }
         */
 
+        float normalizeAngle(float angle) {
+            angle = angle - 360.0f * floor(angle / 360.0f);
+            
+            if (angle < 0) {
+                angle += 360.0f;
+            }
+
+            return angle;
+        }
+
         void updatePose(){
             delta_theta=angular_velocity*control_period;
             sin_theta = sin(theta);