Create WheeledRobot.cpp

cpp/WheeledRobot.cpp

@@ -0,0 +1,65 @@
+/**
+* 
+* \mainpage BreezySLAM: Simple, efficient SLAM in C++
+*
+* WheeledRobot.cpp - C++ code for WheeledRobot class
+*
+* Copyright (C) 2014 Simon D. Levy
+
+* This code is free software: you can redistribute it and/or modify
+* it under the terms of the GNU Lesser General Public License as 
+* published by the Free Software Foundation, either version 3 of the 
+* License, or (at your option) any later version.
+* 
+* This code is distributed in the hope that it will be useful,     
+* but WITHOUT ANY WARRANTY without even the implied warranty of
+* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+* GNU General Public License for more details.
+* 
+* You should have received a copy of the GNU Lesser General Public License 
+* along with this code.  If not, see <http://www.gnu.org/licenses/>.
+*/
+
+#include <stdio.h>
+#include <math.h>
+
+#include <iostream>
+#include <vector>
+using namespace std; 
+
+#include "WheeledRobot.hpp"
+#include "Velocities.hpp"
+
+Velocities 
+WheeledRobot::computeVelocities(
+    double timestamp, 
+    double left_wheel_odometry, 
+    double right_wheel_odometry)
+{                
+    Velocities velocities;
+    
+    double timestamp_seconds_curr, left_wheel_degrees_curr, right_wheel_degrees_curr;
+    
+    this->extractOdometry(timestamp, left_wheel_odometry, right_wheel_odometry, 
+        timestamp_seconds_curr, left_wheel_degrees_curr, right_wheel_degrees_curr);
+    
+    if (this->timestamp_seconds_prev > 0)
+    {             
+        double left_diff_degrees = left_wheel_degrees_curr - this->left_wheel_degrees_prev;
+        double right_diff_degrees = right_wheel_degrees_curr - this->right_wheel_degrees_prev;
+        
+        velocities.dxy_mm =  this->wheel_radius_mm * (radians(left_diff_degrees) + radians(right_diff_degrees));
+        
+        velocities.dtheta_degrees = this->wheel_radius_mm / this->half_axle_length_mm *
+        (right_diff_degrees - left_diff_degrees);
+        
+        velocities.dt_seconds = timestamp_seconds_curr - timestamp_seconds_prev;
+    }
+    
+    // Store current odometry for next time
+    this->timestamp_seconds_prev = timestamp_seconds_curr;
+    this->left_wheel_degrees_prev = left_wheel_degrees_curr;
+    this->right_wheel_degrees_prev = right_wheel_degrees_curr;      
+    
+    return velocities;
+}