updated kinematics

Author: gbr1

examples/kinematics/kinematics.ino

@@ -2,11 +2,42 @@
 
 Arduino_Alvik_Firmware alvik;
 
+unsigned long tmotor=0;
+unsigned long ttask=0;
+uint8_t task=0;
+
 void setup() {
   alvik.begin();
+  ttask=millis();
+  tmotor=millis();
+  task=0;
 }
 
 void loop() {
-  alvik.drive(1,1);
-  
+  if (millis()-tmotor>20){
+    tmotor=millis();
+    alvik.updateMotors();
+  }
+
+  if (millis()-ttask>2000){
+    ttask=millis();
+    switch (task){
+      case 0:
+        alvik.drive(0,90);
+        break;
+      case 1:
+        alvik.drive(0,0);
+        break;
+      case 2:
+        alvik.drive(0,-90);
+        break;
+      case 3:
+        alvik.drive(0,0);
+        break;
+    }
+    task++;
+    if (task>3){
+      task=0;
+    }
+  }
 }

src/Arduino_Alvik_Firmware.cpp

@@ -608,4 +608,5 @@ void Arduino_Alvik_Firmware::errorLed(const int error_code){
 
 void Arduino_Alvik_Firmware::drive(const float linear, const float angular){
     kinematics->forward(linear, angular);
+    setRpm(kinematics->getLeftVelocity(),kinematics->getRightVelocity());
 }

src/robotics/kinematics.h

@@ -16,10 +16,12 @@ class Kinematics{
         float rpm_to_rads;
         float rads_to_degs;
         float degs_to_rads;
+        float w;
 
 
     public:
         Kinematics(const float _wheel_track, const float _wheel_diameter){
+            w=0.0;
             left_velocity=0.0;
             right_velocity=0.0;
             linear_velocity=0.0;
@@ -37,8 +39,11 @@ class Kinematics{
         }
 
         void forward(const float linear, const float angular){
-            left_velocity=(2*linear-angular*wheel_track)/(wheel_diameter);
-            right_velocity=(2*linear+angular*wheel_track)/(wheel_diameter);
+            w = angular*degs_to_rads;
+            left_velocity=(2*linear-w*wheel_track)/(wheel_diameter);
+            left_velocity=rads_to_rpm*left_velocity;
+            right_velocity=(2*linear+w*wheel_track)/(wheel_diameter);
+            right_velocity=rads_to_rpm*right_velocity;
         }
 
         void inverse(const float left_vel, const float right_vel){