example: position

Author: gbr1

examples/position/position.ino

@@ -0,0 +1,74 @@
+/*
+    This file is part of the Arduino_AlvikCarrier library.
+
+    Copyright (c) 2023 Arduino SA
+
+    This Source Code Form is subject to the terms of the Mozilla Public
+    License, v. 2.0. If a copy of the MPL was not distributed with this
+    file, You can obtain one at http://mozilla.org/MPL/2.0/.
+    
+*/
+
+// WIP
+
+#include "Arduino_AlvikCarrier.h"
+
+Arduino_AlvikCarrier alvik;
+
+PidController pid(1.2,0,1.0,20,50);
+
+
+unsigned long tmotor=0;
+unsigned long ttask=0;
+uint8_t task=0;
+float reference;
+float position;
+float error;
+
+void setup() {
+  Serial.begin(115200);
+  alvik.begin();
+  ttask=millis();
+  tmotor=millis();
+  task=0;
+  reference=0;
+  position=0;
+  error=0;
+}
+
+void loop() {
+  if (millis()-tmotor>20){
+    tmotor=millis();
+    alvik.updateMotors();
+    position=alvik.motor_control_left->getTravel()*360.0;
+    pid.update(position);
+    alvik.motor_control_left->setRPM(pid.getControlOutput());
+    Serial.print(reference);
+    Serial.print("\t");
+    Serial.println(position);
+  }
+
+  if (millis()-ttask>5000){
+    ttask=millis();
+    switch (task){
+      case 0:
+        reference=90;
+        break;
+      case 1:
+        reference=0;
+        break;
+      case 2:
+        reference=-90;
+        break;
+      case 3:
+        reference=0;
+        break;
+    }
+    pid.setReference(reference);
+    task++;
+    if (task>3){
+      task=0;
+    }
+  }
+  
+}

src/Arduino_AlvikCarrier.cpp

@@ -66,6 +66,7 @@ Arduino_AlvikCarrier::Arduino_AlvikCarrier(){
 
     // kinematics
     kinematics = new Kinematics(WHEEL_TRACK_MM,WHEEL_DIAMETER_MM);
+    kinematics_movement=0;
 
 }
 
@@ -603,6 +604,13 @@ void Arduino_AlvikCarrier::errorLed(const int error_code){
 /*                                             Kinematics                                             */
 /******************************************************************************************************/
 
+
+
+
+
+
+
+
 void Arduino_AlvikCarrier::drive(const float linear, const float angular){
     kinematics->forward(linear, angular);
     setRpm(kinematics->getLeftVelocity(),kinematics->getRightVelocity());
@@ -620,7 +628,6 @@ void Arduino_AlvikCarrier::rotate(const float angle){
             kinematics->inverse(motor_control_left->getRPM(),motor_control_right->getRPM());
             kinematics->updatePose();
             error=final_angle-kinematics->getTheta();
-            Serial.println(error);
         }
         if (angle>0){
             drive(0,40);
@@ -649,7 +656,6 @@ void Arduino_AlvikCarrier::move(const float distance){
             kinematics->inverse(motor_control_left->getRPM(),motor_control_right->getRPM());
             kinematics->updatePose();
             error=final_travel-kinematics->getTravel();
-            Serial.println(kinematics->getTravel());
         }
         if (distance>0){
             drive(40,0);
@@ -663,3 +669,13 @@ void Arduino_AlvikCarrier::move(const float distance){
     motor_control_right->brake();
 }
 
+
+
+
+void Arduino_AlvikCarrier::updateKinematics(){
+    kinematics->inverse(motor_control_left->getRPM(),motor_control_right->getRPM());
+    kinematics->updatePose();
+    if (kinematics_movement!=0){
+        // do controls
+    }
+}

src/Arduino_AlvikCarrier.h

@@ -69,6 +69,9 @@ class Arduino_AlvikCarrier{
         uint8_t version_low;
 
 
+        uint8_t kinematics_movement;
+
+
 
     public:        
         Kinematics * kinematics;
@@ -195,6 +198,7 @@ class Arduino_AlvikCarrier{
 
 
         // Kinematics
+        void updateKinematics();
         void drive(const float linear, const float angular);                            // set mm/s and deg/s of the robot
 
         void move(const float distance);                                                // move of distance millimeters