update kinematics example

Author: gbr1

examples/kinematics/kinematics.ino

@@ -9,21 +9,24 @@
     
 */
 
-// WIP
+// This example shows how to implement kinematics commands and retrieve odometry data from Arduino_AlvikCarrier class
 
 #include "Arduino_AlvikCarrier.h"
 
 Arduino_AlvikCarrier alvik;
 
 unsigned long tmotor=0;
 unsigned long ttask=0;
+unsigned long tled=0;
 uint8_t task=0;
+bool led_value = false;
 
 void setup() {
   Serial.begin(115200);
   alvik.begin();
   ttask=millis();
   tmotor=millis();
+  tled=millis();
   task=0;
 }
 
@@ -33,19 +36,19 @@ void loop() {
     alvik.updateMotors();
     alvik.updateKinematics();
     Serial.print("\t");
-    Serial.print(alvik.kinematics->getLinearVelocity());
+    Serial.print(alvik.getLinearVelocity());
     Serial.print("\t");
-    Serial.print(alvik.kinematics->getAngularVelocity());
+    Serial.print(alvik.getAngularVelocity());
     Serial.print("\t");
-    Serial.print(alvik.kinematics->getX());
+    Serial.print(alvik.getX());
     Serial.print("\t");
-    Serial.print(alvik.kinematics->getY());
+    Serial.print(alvik.getY());
     Serial.print("\t");
-    Serial.print(alvik.kinematics->getTheta());
+    Serial.print(alvik.getTheta());
     Serial.print("\n");
   }
 
-  if (millis()-ttask>5000){
+  if ((millis()-ttask>5000)||alvik.isTargetReached()){
     ttask=millis();
     switch (task){
       case 0:
@@ -56,7 +59,7 @@ void loop() {
         alvik.drive(0,0);
         break;
       case 2:
-        alvik.rotate(-90);
+        alvik.move(100);
         break;
       case 3:
         alvik.disableKinematicsMovement();
@@ -68,4 +71,17 @@ void loop() {
       task=0;
     }
   }
+
+  if (millis()-tled>200){
+    tled=millis();
+    led_value=!led_value;
+    if (task==1){
+      alvik.setLedLeftGreen(false);
+      alvik.setLedLeftRed(led_value);
+    }
+    if (task==3){
+      alvik.setLedLeftRed(false);
+      alvik.setLedLeftGreen(led_value);
+    }
+  }
 }