Diff: 05_TrajectoryTracking/kinematics.cpp

Revision:

10:ea4b90be68dc

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/05_TrajectoryTracking/kinematics.cpp	Sat Apr 23 10:29:24 2016 +0000
@@ -0,0 +1,26 @@
+#include "kinematics.h"
+
+kinematics::kinematics()
+{
+    wheelDia = wheel_dia;
+    trackWidth = track_width;
+
+}
+
+void kinematics::VW_RPM(float velocity, float omega, float* left_rpm, float* right_rpm)
+{
+    *right_rpm = (velocity + (omega * trackWidth / 2)) * (60.0 / (M_PI * wheelDia));
+    *left_rpm = (velocity - (omega * trackWidth / 2)) * (60.0 / (M_PI * wheelDia));
+
+    /*
+    if(*right_rpm > 100)
+        *right_rpm = generalFunctions::constrain_f(*right_rpm, min_rpm, max_rpm);
+    else
+        *right_rpm = 0;
+
+    if(*left_rpm > 100)
+        *left_rpm = generalFunctions::constrain_f(*left_rpm, min_rpm, max_rpm);
+    else
+        *left_rpm = 0;    
+    */
+}
\ No newline at end of file