Restored Matlab, Java

2018-06-30 22:36:15 -04:00
parent 16ee48385f
commit 0ac92380c5
33 changed files with 3033 additions and 21 deletions
--- a/matlab/WheeledRobot.m
+++ b/matlab/WheeledRobot.m
@@ -0,0 +1,112 @@
+classdef WheeledRobot
+    %WheeledRobot An abstract class supporting ododmetry for wheeled robots.
+    %    Your implementing class should provide the method:
+    %
+    %       extractOdometry(obj, timestamp, leftWheel, rightWheel) -->
+    %          (timestampSeconds, leftWheelDegrees, rightWheelDegrees)
+    %
+    %    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/>.
+    
+    properties (Access = 'private')
+        
+        wheelRadiusMillimeters
+        halfAxleLengthMillimeters
+        
+        timestampSecondsPrev
+        leftWheelDegreesPrev
+        rightWheelDegreesPrev
+        
+    end
+    
+    methods (Access = 'protected')
+        
+        function s = str(obj)
+            % Returns a string representation of this WheeledRobot
+            s = sprintf('<Wheel radius=%f mm Half axle Length=%f mm>', ...
+                obj.wheelRadiusMillimeters, obj.halfAxleLengthMillimeters);
+        end
+        
+        function robot = WheeledRobot(wheelRadiusMillimeters, halfAxleLengthMillimeters)
+            % Constructs a WheeledRobot object
+            %    robot = WheeledRobot(wheelRadiusMillimeters, halfAxleLengthMillimeters)
+            robot.wheelRadiusMillimeters = wheelRadiusMillimeters;
+            robot.halfAxleLengthMillimeters = halfAxleLengthMillimeters;
+            
+        end
+        
+        function r = deg2rad(~, d)
+            % Converts degrees to radians
+            r = d * pi / 180;
+        end
+        
+    end
+    
+    methods (Abstract)
+        
+        extractOdometry(obj, timestamp, leftWheel, rightWheel)
+        
+    end
+    
+    methods (Access = 'public')
+        
+        function [poseChange, obj] = computePoseChange(obj, timestamp, leftWheelOdometry, rightWheelOdometry)
+            % Computes forward and angular poseChange based on odometry.
+            %
+            % Parameters:
+            
+            % timestamp          time stamp, in whatever units your robot uses
+            % leftWheelOdometry  odometry for left wheel, in whatever form your robot uses
+            % rightWheelOdometry odometry for right wheel, in whatever form your robot uses
+            %
+            % Returns a tuple (dxyMillimeters, dthetaDegrees, dtSeconds)
+            
+            % dxyMillimeters     forward distance traveled, in millimeters
+            % dthetaDegrees change in angular position, in degrees
+            % dtSeconds     elapsed time since previous odometry, in seconds
+            
+            dxyMillimeters = 0;
+            dthetaDegrees = 0;
+            dtSeconds = 0;
+            
+            [timestampSecondsCurr, leftWheelDegreesCurr, rightWheelDegreesCurr] = ...
+                obj.extractOdometry(timestamp, leftWheelOdometry, rightWheelOdometry);
+            
+            if obj.timestampSecondsPrev
+                
+                leftDiffDegrees = leftWheelDegreesCurr - obj.leftWheelDegreesPrev;
+                rightDiffDegrees = rightWheelDegreesCurr - obj.rightWheelDegreesPrev;
+                
+                dxyMillimeters =  obj.wheelRadiusMillimeters * ...
+                    (obj.deg2rad(leftDiffDegrees) + obj.deg2rad(rightDiffDegrees));
+                
+                dthetaDegrees =  (obj.wheelRadiusMillimeters / obj.halfAxleLengthMillimeters) * ...
+                    (rightDiffDegrees - leftDiffDegrees);
+                
+                dtSeconds = timestampSecondsCurr - obj.timestampSecondsPrev;
+            end
+            
+            % Store current odometry for next time
+            obj.timestampSecondsPrev = timestampSecondsCurr;
+            obj.leftWheelDegreesPrev = leftWheelDegreesCurr;
+            obj.rightWheelDegreesPrev = rightWheelDegreesCurr;
+            
+            % Return linear velocity, angular velocity, time difference
+            poseChange = [dxyMillimeters, dthetaDegrees, dtSeconds];
+            
+        end
+    end    
+end
+