Revision 10:5ef5fe8c7775, committed 2017-04-16

Comitter:

britneyd

Date:

Sun Apr 16 19:52:52 2017 +0000

Parent:

9:a8fd0bd49279

Commit message:

Final project;

Changed in this revision

--- a/main.cpp	Thu Mar 30 22:13:44 2017 +0000
+++ b/main.cpp	Sun Apr 16 19:52:52 2017 +0000
@@ -19,6 +19,13 @@
 #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 maxAccel 5
+//#define CIRC 2*PI*WHEEL_RADIUS
+
 //For RF Communication
 #define JSTICK_H 8
 #define JSTICK_V 9
@@ -39,6 +46,9 @@
 //JoyStick
 #define POTV p19
 #define POTH p20
+#define X 500
+#define Y 0
+
 
 //PID
 #define MAX_THROTTLE 100
@@ -48,6 +58,7 @@
 float Kd1;
 float Kp2;
 float Kd2;
+int i;
 
 //Controller Values
 uint8_t knob1, knob2, knob3, knob4;
@@ -57,6 +68,7 @@
 float throttle = 0; //From joystick
 float steering = 0; //From joystick
 int robot_pos = 0; //Robots position
+float motor1_old = 0; 
 
 Timer timer;
 int timer_value;
@@ -80,6 +92,10 @@
 bool button;
 #include "communication.h"
 
+//Waypoint
+//[(10m, 0th). (5m, 90th)]
+//(10x, 0y), (x, y)]
+
 // ================================================================
 // ===                      INITIAL SETUP                       ===
 // ================================================================
@@ -95,9 +111,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");
@@ -117,7 +140,7 @@
     enable = ENABLE;
 
     while(1) {
-        //Led 4 to indicate if robot it Running or Not
+        //Led 4 to indicate if robot is Running or Not
         led4 = ROBOT_ON;
 
         //Led 2 to indicate a button press
@@ -127,7 +150,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,7 +159,24 @@
             }
         }
 
+        //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; ///total dist traveled
+        robot_theta = (wheel1_dist - wheel2_dist) /WHEEL_BASE; //angle
 
+        distance_traveled = robot_pos - robot_pos_old;
+    
+    
+        //Curr Robot X and Y
+        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)
+
+        pc.printf("X axis: %d",world_x);
+        pc.printf("Y axis: %d \n",world_y);
+        pc.printf("robot_pos: %d \n",robot_pos);
         //Timer
         timer_value = timer.read_us();
 
@@ -149,26 +190,64 @@
         medium_loop_counter++;
         dt = (timer_value - timer_old);
         timer_old = timer_value;
+        robot_pos_old = robot_pos;
         /*****************************************/
 
         //Running: Motor Control Enabled
         if(ROBOT_ON) {
+            //TEMPWAYPOINTS
+            //Get Waypoint
+            //i = 0;
+            //X = WAYPOINS[i][0]
+            //Y = WAYPOINS[i][1]
+            //calc target theta
+            float target_theta = 0;
+            float target_distance = 5000;
+            //if(robot is at waypoint)
+            //get new waypoint
+            //i++
+            //else {
+                //}
+            
+            
+            
             //Enable Motor
             enable = ENABLE;
 
-            //Calculate motor inputs
-            motor1 = int16_t(throttle/2 + steering/8);
-            motor2 = int16_t(throttle/2 - steering/8);
+            //User Control ONLY 
+            float theta_error = target_theta - robot_theta;
+
+            if(theta_error > 1 && theta_error < -1) {
+                //Angular Controller
+                float turn_speed = theta_error*0.5;
+                motor1 = turn_speed;
+                motor2 = -turn_speed; 
+            } else {
+                //Dist Controller
+                float dist_error = target_distance - robot_pos;
+                float speed = dist_error*0.5;
+                motor1 = speed;
+                motor2 = speed;
+            }
+            
+            
+            //Acceleratoin Cap
+            float motor1Acel = motor1 - motor1_old;
+            
+            if(motor1Acel > maxAccel) { //macAcel = 5
+                motor1 = maxAccel;
+            }
 
             //Cap the max and min values [-100, 100]
             motor1 = CAP(motor1, MAX_CONTROL_OUTPUT);
             motor2 = CAP(motor2, MAX_CONTROL_OUTPUT);
+            motor1_old = motor1;
 
             //Set Motor Speed here
             setMotor1Speed(motor1);
             setMotor2Speed(motor2);
 
-        } 
+        }
         //Robot is off so disable the motors
         else {
             enable = DISABLE;