Revision 10:4b5f975c21c4, committed 2017-03-31

Comitter:

csharer

Date:

Fri Mar 31 21:31:04 2017 +0000

Parent:

9:a8fd0bd49279

Commit message:

added localization

Changed in this revision

--- a/main.cpp	Thu Mar 30 22:13:44 2017 +0000
+++ b/main.cpp	Fri Mar 31 21:31:04 2017 +0000
@@ -19,6 +19,12 @@
 #include "stepper_motors.h"
 #include "MRF24J40.h"
 
+#define WHEEL_BASE 100 //mm
+#define NUM_STEPS 1600 //number of steps for 1 turn of wheel
+#define WHEEL_RADIUS 51 //units in mm
+#define PI 3.1415
+//#define CIRC 2*PI*WHEEL_RADIUS
+
 //For RF Communication
 #define JSTICK_H 8
 #define JSTICK_V 9
@@ -95,9 +101,16 @@
     //Set the Channel. 0 is default, 15 is max
     uint8_t channel = 9;
     mrf.SetChannel(channel);
-    
+
     //Used for button press
     int last_button_time = 0;
+    float wheel1_dist = 0;
+    float wheel2_dist = 0;
+    float robot_theta = 0;
+    float distance_traveled = 0;
+    float world_x = 0;
+    float world_y = 0;
+    float robot_pos_old = 0;
 
     pc.baud(115200);
     pc.printf("Start\r\n");
@@ -127,7 +140,8 @@
         if(button) {
             pos_M1 = 0; //Reset position of Motor 1
             pos_M2 = 0; //Reset position of motor 2
-            
+
+            //Read Button Press to Toggle Motors
             if((timer.read_us() - last_button_time) > 250000) {
                 ROBOT_ON = ROBOT_ON^1;
                 pc.printf("BUTTON WAS PRESSED \r\n");
@@ -135,6 +149,19 @@
             }
         }
 
+        //Conver Motor Position from Steps to Distance
+        wheel1_dist = 2*PI*WHEEL_RADIUS* pos_M1/NUM_STEPS;
+        wheel2_dist = 2*PI*WHEEL_RADIUS* pos_M2/NUM_STEPS;
+        robot_pos = (wheel1_dist + wheel2_dist) / 2;
+        robot_theta = (wheel1_dist - wheel2_dist) /WHEEL_BASE; //angle
+
+        distance_traveled = robot_pos - robot_pos_old;
+
+        world_x += distance_traveled * sin(robot_theta); //import sin
+        world_y += distance_traveled * cos(robot_theta);
+        
+        //Pose(x,y,th) = P(world_x, world_y, robot_th)
+
 
         //Timer
         timer_value = timer.read_us();
@@ -149,6 +176,7 @@
         medium_loop_counter++;
         dt = (timer_value - timer_old);
         timer_old = timer_value;
+        robot_pos_old = robot_pos;
         /*****************************************/
 
         //Running: Motor Control Enabled
@@ -168,7 +196,7 @@
             setMotor1Speed(motor1);
             setMotor2Speed(motor2);
 
-        } 
+        }
         //Robot is off so disable the motors
         else {
             enable = DISABLE;