From a58d1f8ffb27ac51553176f61d39fc315668a26d Mon Sep 17 00:00:00 2001
From: Juan Jimeno <jimenojmm@gmail.com>
Date: Fri, 23 Nov 2018 01:10:51 +0800
Subject: [PATCH] fixed equation

---
 Kinematics.cpp | 47 ++++++++++++++++++++++++-----------------------
 Kinematics.h   |  9 +++++----
 2 files changed, 29 insertions(+), 27 deletions(-)

diff --git a/Kinematics.cpp b/Kinematics.cpp
index 7e48f2a..0d1aef0 100644
--- a/Kinematics.cpp
+++ b/Kinematics.cpp
@@ -28,13 +28,14 @@
 #include "Arduino.h"
 #include "Kinematics.h"
 
-Kinematics::Kinematics(int motor_max_rpm, float wheel_diameter, float base_width, int pwm_bits)
+Kinematics::Kinematics(int motor_max_rpm, float wheel_diameter, float fr_wheels_dist, float lr_wheels_dist, int pwm_bits):
+  wheel_diameter_(wheel_diameter),
+  circumference_(PI * wheel_diameter_),
+  max_rpm_(motor_max_rpm),
+  fr_wheels_dist_(fr_wheels_dist),
+  lr_wheels_dist_(lr_wheels_dist),
+  pwm_res_ (pow(2, pwm_bits) - 1)
 {
-  wheel_diameter_ = wheel_diameter;
-  circumference_ = PI * wheel_diameter_;
-  max_rpm_ = motor_max_rpm;
-  base_width_ = base_width;
-  pwm_res_ = pow(2, pwm_bits) - 1;
 }
 
 Kinematics::output Kinematics::getRPM(float linear_x, float linear_y, float angular_z)
@@ -47,7 +48,7 @@ Kinematics::output Kinematics::getRPM(float linear_x, float linear_y, float angu
   angular_vel_z_mins_ = angular_z * 60;
 
   //Vt = ω * radius
-  tangential_vel_ = angular_vel_z_mins_ * base_width_;
+  tangential_vel_ = angular_vel_z_mins_ * lr_wheels_dist_;
 
   x_rpm_ = linear_vel_x_mins_ / circumference_;
   y_rpm_ = linear_vel_y_mins_ / circumference_;
@@ -94,15 +95,15 @@ Kinematics::velocities Kinematics::getVelocities(int motor1, int motor2)
 {
   Kinematics::velocities vel;
 
-  double average_rpm_x = (motor1 + motor2) / 2; // RPM
+  float average_rpm_x = (float)(motor1 + motor2) / 2; // RPM
   //convert revolutions per minute to revolutions per second
-  double average_rps_x = average_rpm_x / 60; // RPS
-  vel.linear_x = (average_rps_x * (wheel_diameter_ * PI)); // m/s
+  float average_rps_x = average_rpm_x / 60; // RPS
+  vel.linear_x = (average_rps_x * circumference_); // m/s
 
-  double average_rpm_a = (motor2 - motor1) / 2;
+  float average_rpm_a = (float)(motor2 - motor1) / 2;
   //convert revolutions per minute to revolutions per second
-  double average_rps_a = average_rpm_a / 60;
-  vel.angular_z =  (average_rps_a * (wheel_diameter_ * PI)) / base_width_;
+  float average_rps_a = average_rpm_a / 60;
+  vel.angular_z =  (average_rps_a * circumference_) / (lr_wheels_dist_ / 2);
 
   return vel;
 }
@@ -111,20 +112,20 @@ Kinematics::velocities Kinematics::getVelocities(int motor1, int motor2, int mot
 {
   Kinematics::velocities vel;
 
-  double average_rpm_x = (motor1 + motor2 + motor3 + motor4) / 4; // RPM
+  float average_rpm_x = (float)(motor1 + motor2 + motor3 + motor4) / 4; // RPM
   //convert revolutions per minute to revolutions per second
-  double average_rps_x = average_rpm_x / 60; // RPS
-  vel.linear_x = (average_rps_x * (wheel_diameter_ * PI)); // m/s
+  float average_rps_x = average_rpm_x / 60; // RPS
+  vel.linear_x = (average_rps_x * circumference_); // m/s
 
-  double average_rpm_y = (-motor1 + motor2 + motor3 - motor4) / 4; // RPM
+  float average_rpm_y = (float)(-motor1 + motor2 + motor3 - motor4) / 4; // RPM
   //convert revolutions per minute in y axis to revolutions per second
-  double average_rps_y = average_rpm_y / 60; // RPS
-  vel.linear_y = (average_rps_y * (wheel_diameter_ * PI)); // m/s
+  float average_rps_y = average_rpm_y / 60; // RPS
+  vel.linear_y = (average_rps_y * circumference_); // m/s
 
-  double average_rpm_a = (-motor1 + motor2 - motor3 + motor4) / 4;
+  float average_rpm_a = (float)(-motor1 + motor2 - motor3 + motor4) / 4;
   //convert revolutions per minute to revolutions per second
-  double average_rps_a = average_rpm_a / 60;
-  vel.angular_z =  (average_rps_a * (wheel_diameter_ * PI)) / base_width_;
+  float average_rps_a = average_rpm_a / 60;
+  vel.angular_z =  (average_rps_a * circumference_) / ((fr_wheels_dist_ / 2) + (lr_wheels_dist_ / 2));
 
   return vel;
 }
@@ -132,5 +133,5 @@ Kinematics::velocities Kinematics::getVelocities(int motor1, int motor2, int mot
 int Kinematics::rpmToPWM(int rpm)
 {
   //remap scale of target RPM vs MAX_RPM to PWM
-  return (((double) rpm / (double) max_rpm_) * pwm_res_);
+  return (((float) rpm / (float) max_rpm_) * pwm_res_);
 }
diff --git a/Kinematics.h b/Kinematics.h
index 2d1864a..02dcf1a 100644
--- a/Kinematics.h
+++ b/Kinematics.h
@@ -46,7 +46,7 @@ class Kinematics
       float linear_y;
       float angular_z;
     };
-    Kinematics(int motor_max_rpm, float wheel_diameter, float base_width, int pwm_bits);
+    Kinematics(int motor_max_rpm, float wheel_diameter, float fr_wheels_dist, float lr_wheels_dist, int pwm_bits);
     velocities getVelocities(int motor1, int motor2);
     velocities getVelocities(int motor1, int motor2, int motor3, int motor4);
     output getRPM(float linear_x, float linear_y, float angular_z);
@@ -63,9 +63,10 @@ class Kinematics
     float y_rpm_;
     float tan_rpm_;
     int max_rpm_;
-    double wheel_diameter_;
-    float base_width_;
-    double pwm_res_;
+    float wheel_diameter_;
+    float fr_wheels_dist_;
+    float lr_wheels_dist_;
+    float pwm_res_;
 };
 
 #endif