Revision 11:dc410a980771, committed 2017-04-26

Comitter:

britneyd

Date:

Wed Apr 26 20:58:31 2017 +0000

Parent:

10:5ef5fe8c7775

Commit message:

Code for robot;

Changed in this revision

--- a/communication.h	Sun Apr 16 19:52:52 2017 +0000
+++ b/communication.h	Wed Apr 26 20:58:31 2017 +0000
@@ -42,11 +42,13 @@
 // to reject data that doesn't have the right header. So the first
 // 8 bytes in txBuffer look like a valid header. The remaining 120
 // bytes can be used for anything you like.
-char txBuffer[128];
-char rxBuffer[128];
+#define RF_BUFLEN 200
+char txBuffer[RF_BUFLEN];
+char rxBuffer[RF_BUFLEN];
 int rxLen;
 
 
+
 //***************** Do not change these methods (please) *****************//
 /**
 * Receive data from the MRF24J40.
@@ -66,6 +68,7 @@
                 //Make sure our header is valid first
                 if(data[i] != header[i]) {
                     //mrf.Reset();
+                    //pc.printf("Header not valid\r\n");
                     return 0;
                 }
             } else {
@@ -74,7 +77,12 @@
         }
         //pc.printf("Received: %s length:%d\r\n", data, ((int)len)-10);
     }
-    return ((int)len)-10;
+    int retVal = (int)len-10;
+    if (retVal > 0) {
+        //pc.printf("In rf_receive: got %s\n", data);
+        //pc.printf("Received %d bytes of data\r\n", retVal);
+    }
+    return retVal;
 }
 
 /**
@@ -118,6 +126,7 @@
 //Pulls data out fo rxBuffer and updates global variables accordingly 
 void communication_protocal(int len)
 {
+    //pc.printf("Found MATCH\r\n");
     bool found_name = false;
     bool found_num = false;
     bool complete_name = false;
@@ -167,7 +176,7 @@
         
         //If we have a complete name AND number value (ie. start and end of each != NONE)
         if(found_name & found_num & complete_name & complete_num) {
-            //pc.printf("Found MATCH\r\n");
+            
             //Reset flags
             found_name = false;
             found_num = false;
@@ -201,12 +210,35 @@
                 knob4 = (uint8_t)atoi(*num);
             else if(strcmp(*name, "Button\0")==0) 
                 button = (bool)atoi(*num);
-            
+            else if(strcmp(*name, "Theta\0")==0) {
+                theta_ReadIn = (float)atof(*num);    
+                pc.printf("Theta read in: %f\r\n", theta_ReadIn);
+            }
+            else if(strcmp(*name, "Distance\0")==0){ 
+                dis_ReadIn = (float)atof(*num);
+                pc.printf("Dis read in: %f\r\n", dis_ReadIn);
+            }
             //Reset flags
+            /*if(index == 0){
+                 waypoints[2*index] = theta_ReadIn;
+                 waypoints[2*index+1] = dis_ReadIn;
+                 pc.printf("Theta read in: %f\n", theta_ReadIn);
+                 pc.printf("Dis read in: %f\n", dis_ReadIn);
+            }
+            else if((theta_ReadIn==0)&&(dis_ReadIn==0)){
+                moving = 1;
+            }
+            else if((waypoints[2*index-2]!= theta_ReadIn)&&(waypoints[2*index-1]!= dis_ReadIn)){
+                waypoints[2*index] = theta_ReadIn;
+                waypoints[2*index+1] = dis_ReadIn;
+                pc.printf("Theta read in: %f\n", theta_ReadIn);
+                pc.printf("Dis read in: %f\n", dis_ReadIn);
+            }*/
             name_start = NONE;
             name_end = NONE;
             num_start = NONE;
             num_end = NONE;
+            //index++;
         }
     }
 }
\ No newline at end of file

--- a/main.cpp	Sun Apr 16 19:52:52 2017 +0000
+++ b/main.cpp	Wed Apr 26 20:58:31 2017 +0000
@@ -23,7 +23,7 @@
 #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 maxAccel 10
 //#define CIRC 2*PI*WHEEL_RADIUS
 
 //For RF Communication
@@ -49,6 +49,8 @@
 #define X 500
 #define Y 0
 
+#define COMMUNICATION_FORMAT "Jstick_h: %f Jstick_v: %f Button: %d Theta: %f Distance: %f"
+
 
 //PID
 #define MAX_THROTTLE 100
@@ -58,16 +60,22 @@
 float Kd1;
 float Kp2;
 float Kd2;
-int i;
+float theta_ReadIn;
+float dis_ReadIn;
+int i = 0;
+const int waypointBufferSize = 100;
+float waypoints[waypointBufferSize];
+int index = 0;
+int buffer_index = 0;
 
 //Controller Values
 uint8_t knob1, knob2, knob3, knob4;
-int8_t jstick_h, jstick_v;
+float jstick_h, jstick_v;
 
 //Control Variables
 float throttle = 0; //From joystick
 float steering = 0; //From joystick
-int robot_pos = 0; //Robots position
+float robot_pos = 0;//Robots position
 float motor1_old = 0; 
 
 Timer timer;
@@ -89,7 +97,8 @@
 DigitalOut led4(LED4);
 
 //Button
-bool button;
+int button;
+int moving;
 #include "communication.h"
 
 //Waypoint
@@ -140,6 +149,7 @@
     enable = ENABLE;
 
     while(1) {
+        //pc.printf("Something happened!\r\n");
         //Led 4 to indicate if robot is Running or Not
         led4 = ROBOT_ON;
 
@@ -147,15 +157,17 @@
         led2 = button;
 
         //If button is pressed reset motor position
+        
         if(button) {
+            //button = 0; 
             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");
-                last_button_time = timer.read_us();
+                //pc.printf("BUTTON WAS PRESSED \r\n");
+                last_button_time = timer.read_us(); 
             }
         }
 
@@ -174,9 +186,11 @@
         
         //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);
+        //pc.printf("X axis: %d",world_x);
+        //pc.printf("Y axis: %d \n",world_y);
+        //pc.printf("robot_pos: %d \n",robot_pos);
+        //pc.printf("robot_theta: %f\n", robot_theta);
+        
         //Timer
         timer_value = timer.read_us();
 
@@ -184,25 +198,25 @@
         throttle = jstick_v;
         steering = jstick_h;
 
-        /**** Update Values DO NOT MODIFY ********/
+        // Update Values DO NOT MODIFY
         loop_counter++;
         slow_loop_counter++;
         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;
+            float target_theta = theta_ReadIn*PI/180;//waypoints[2*i];
+            float target_distance = dis_ReadIn;//waypoints[2*i+1];
+            pc.printf("Target_theta = %f\r\n", target_theta);
+            pc.printf("Target_distance = %f\r\n", target_distance);
             //if(robot is at waypoint)
             //get new waypoint
             //i++
@@ -216,74 +230,127 @@
 
             //User Control ONLY 
             float theta_error = target_theta - robot_theta;
+            float dist_error = target_distance - robot_pos;
+            const float THETA_ERROR_THRESHOLD = 0.1;
+            const float DIS_ERROR_THRESHOLD = 1.0;
+            const float MINIMUM_TURNING_SPEED = 1.0;
+            const float MINIMUM_MOVING_SPEED = 1.0;
 
-            if(theta_error > 1 && theta_error < -1) {
+            if(theta_error > THETA_ERROR_THRESHOLD || theta_error < -THETA_ERROR_THRESHOLD) {
                 //Angular Controller
-                float turn_speed = theta_error*0.5;
+                //pc.printf("theta_error: %f\n", theta_error);
+                float turn_speed = theta_error*5.0;
+                if(turn_speed < MINIMUM_TURNING_SPEED && theta_error > 0){
+                    turn_speed = MINIMUM_TURNING_SPEED;
+                }
+                else if(turn_speed > -MINIMUM_TURNING_SPEED && theta_error < 0){
+                    turn_speed = -MINIMUM_TURNING_SPEED;
+                }   
                 motor1 = turn_speed;
                 motor2 = -turn_speed; 
-            } else {
+            } else if(dist_error > DIS_ERROR_THRESHOLD || dist_error < -DIS_ERROR_THRESHOLD){
                 //Dist Controller
-                float dist_error = target_distance - robot_pos;
+                
+                //pc.printf("dist_error: %d\n",dist_error);
                 float speed = dist_error*0.5;
+                if(speed < MINIMUM_MOVING_SPEED && dist_error > 0){
+                    speed = MINIMUM_MOVING_SPEED;
+                }
+                else if(speed > -MINIMUM_TURNING_SPEED && dist_error < 0){
+                    speed = -MINIMUM_MOVING_SPEED;
+                } 
                 motor1 = speed;
                 motor2 = speed;
-            }
-            
-            
-            //Acceleratoin Cap
-            float motor1Acel = motor1 - motor1_old;
-            
-            if(motor1Acel > maxAccel) { //macAcel = 5
-                motor1 = maxAccel;
-            }
+                
+                            
+             
+                //Acceleratoin Cap
+                float motor1Acel = motor1 - motor1_old;
+                
+                if(motor1Acel > maxAccel) { //macAcel = 5
+                    motor1 = maxAccel;
+                    motor2 = maxAccel;
+                }
+                else if((motor1<0)&&(motor1Acel<-maxAccel)){
+                    motor1 = -maxAccel;
+                    motor2 = -maxAccel;
+                }
+    
+                //Cap the max and min values [-100, 100]
+                motor1 = CAP(motor1, MAX_CONTROL_OUTPUT);
+                motor2 = CAP(motor2, MAX_CONTROL_OUTPUT);
+                //motor1 = -50;
+                //motor2 = -50;
+                motor1_old = motor1;
+            }else{
+                motor1 = 0.0;
+                motor2 = 0.0; 
+                robot_pos = 0.0;
+                }
 
-            //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);
+            setMotor1Speed(-motor1);
+            setMotor2Speed(-motor2);
 
         }
         //Robot is off so disable the motors
         else {
             enable = DISABLE;
         }
+        
+        
 
-        /* Here are some loops that trigger at different intervals, this
-        * will allow you to do things at a slower rate, thus saving speed
-        * it is important to keep this fast so we dont miss IMU readings */
+        //Here are some loops that trigger at different intervals, this
+        //will allow you to do things at a slower rate, thus saving speed
+        //it is important to keep this fast so we dont miss IMU readings
 
         //Fast Loop: Good for printing to serial monitor
         if(loop_counter >= 5) {
             loop_counter = 0;
-            pc.printf("ROBOT_ON:%d pos_M1:%d pos_M2:%d robot_pos%d \r\n", ROBOT_ON, pos_M1, pos_M2, robot_pos);
+            //pc.printf("ROBOT_ON:%d pos_M1:%d pos_M2:%d robot_pos%d \r\n", ROBOT_ON, pos_M1, pos_M2, robot_pos);
         }
+        
+        
 
         //Meduim Loop: Good for sending and receiving
         if (medium_loop_counter >= 10) {
             medium_loop_counter = 0; // Read  status
 
             //Recieve Data
-            rxLen = rf_receive(rxBuffer, 128);
+            rxLen = rf_receive(rxBuffer, RF_BUFLEN);
+            
+            //pc.printf("Tried to receive via rf_receive, rxLen was %d\r\n", rxLen);
             if(rxLen > 0) {
                 led1 = led1^1;
                 //Process data with our protocal
-                communication_protocal(rxLen);
+                //pc.printf("There we go\r\n");
+                //pc.printf("Got %s in rxBuffer\r\n", rxBuffer);
+                int numParsed = sscanf(rxBuffer, COMMUNICATION_FORMAT, &jstick_h, &jstick_v, &button, &theta_ReadIn, &dis_ReadIn);
+                if (numParsed != 5) {
+                   // pc.printf("Couldn't parse all parameters. Parsed only %d\r\n", numParsed);
+                } else {
+                    /*pc.printf("Jstick_h = %f\r\n", jstick_h);
+                    pc.printf("Jstick_v = %f\r\n", jstick_v);
+                    pc.printf("button = %d\r\n", button);
+                    pc.printf("theta = %f\r\n", theta_ReadIn);
+                    pc.printf("distance = %f\r\n", dis_ReadIn);*/
+                }
+                //if((button==1)&&(moving==0)){
+                    communication_protocal(rxLen);
+                //}
             }
 
         }  // End of medium loop
 
+        
         //Slow Loop: Good for sending if speed is not an issue
         if(slow_loop_counter >= 99) {
             slow_loop_counter = 0;
-
-            /* Send Data To Controller goes here *
-             *                                   */
+            //Send Data To Controller goes here 
+        
         } //End of Slow Loop
+        
 
 
 

--- a/stepper_motors.h	Sun Apr 16 19:52:52 2017 +0000
+++ b/stepper_motors.h	Wed Apr 26 20:58:31 2017 +0000
@@ -14,13 +14,13 @@
 DigitalOut step_M1(MOTOR1_STEP);
 DigitalOut dir_M1(MOTOR1_DIR);
 DigitalOut enable(MOTOR_ENABLE); //enable for both motors
-int16_t speed_M1;  //Speed of motor 1
+float speed_M1;  //Speed of motor 1
 
 //MOTOR 2
 DigitalOut step_M2(MOTOR2_STEP);
 DigitalOut dir_M2(MOTOR2_DIR);
-int16_t speed_M2;  //Speed of motor 2
-int16_t motor1, motor2;
+float speed_M2;  //Speed of motor 2
+float motor1, motor2;
 
 //Motor Position
 int pos_M1 = 0, pos_M2 = 0;
@@ -31,13 +31,14 @@
 //ISR to step motor 1
 void ISR1(void)
 {
+    if (enable == DISABLE) return;
     //Step Motor
-    step_M1 = 1;
+    step_M1 = 1.0;
     wait_us(1);
-    step_M1 = 0;
+    step_M1 = 0.0;
     
     //Update Motor Postion
-    if(dir_M1)
+    if(!dir_M1)
         pos_M1++;
     else
         pos_M1--;    
@@ -45,26 +46,27 @@
 //ISR to step motor 2
 void ISR2(void)
 {
+    if (enable == DISABLE) return;
     //Step Motor
-    step_M2 = 1;
+    step_M2 = 1.0;
     wait_us(1);
-    step_M2 = 0;
+    step_M2 = 0.0;
     
     //Update Motor Position
-    if(dir_M2)
+    if(!dir_M2)
         pos_M2++;
     else
         pos_M2--;
 }
 
 //Set motor 1 speed. Speed [-100, 0, +100] = [Max Reverse, Stop, Max Forward]
-void setMotor1Speed(int16_t speed)
+void setMotor1Speed(float speed)
 {
     long timer_period;
     speed = CAP(speed, MAX_CONTROL_OUTPUT);
 
     //Calculate acceleration from the desired speed
-    int16_t desired_accel = speed_M1 - speed;
+    float desired_accel = speed_M1 - speed;
     if(desired_accel > MAX_ACCEL)
         speed_M1 -= MAX_ACCEL; //Change speed of motor by max acceleration
     else if(desired_accel < -MAX_ACCEL)
@@ -72,10 +74,10 @@
     else
         speed_M1 = speed;
 
-    if(speed_M1 == 0) {
+    if(speed_M1 == 0.0) {
         timer_period = ZERO_SPEED;
         dir_M1 = 0; ////sets motor direction
-    } else if (speed_M1 > 0) {
+    } else if (speed_M1 > 0.0) {
         timer_period = 10000 / speed_M1;
         dir_M1 = 1; //sets motor direction
     } else {
@@ -88,13 +90,13 @@
 }
 
 //Set motor 2 speed. Speed [-100, 0, +100] = [Max Reverse, Stop, Max Forward]
-void setMotor2Speed(int16_t speed)
+void setMotor2Speed(float speed)
 {
     long timer_period;
     speed = CAP(speed, MAX_CONTROL_OUTPUT);
 
     //Calculate acceleration from the desired speed
-    int16_t desired_accel = speed_M2 - speed;
+    float desired_accel = speed_M2 - speed;
     if(desired_accel > MAX_ACCEL)
         speed_M2 -= MAX_ACCEL; //Change speed of motor by max acceleration
     else if(desired_accel < -MAX_ACCEL)
@@ -102,10 +104,10 @@
     else
         speed_M2 = speed;
 
-    if(speed_M2 == 0) {
+    if(speed_M2 == 0.0) {
         timer_period = ZERO_SPEED;
         dir_M2 = 0; ////sets motor direction
-    } else if (speed_M2 > 0) {
+    } else if (speed_M2 > 0.0) {
         timer_period = 10000 / speed_M2;
         dir_M2 = 1; //sets motor direction
     } else {