Revision 9:97941581fe81, committed 2017-12-08

Comitter:

Showboo

Date:

Fri Dec 08 05:14:27 2017 +0000

Parent:

8:22e399fe87a4

Commit message:

Ready for Competition

Changed in this revision

diff -r 22e399fe87a4 -r 97941581fe81 algorithm.h
--- a/algorithm.h	Tue Nov 28 21:45:07 2017 +0000
+++ b/algorithm.h	Fri Dec 08 05:14:27 2017 +0000
@@ -8,7 +8,7 @@
 #include "header.h"
 struct geoCoord{
     geoCoord(int x_in, int y_in){x = x_in; y = y_in;}
-    geoCoord(){x = 0; y = 0;}
+    geoCoord(){x = 0; y = 0; distance = 99999.0;}
     int x, y;
     double distance;
 };
@@ -37,10 +37,20 @@
     float l,r,lf,rf;
     FlashIRs(l, r, lf, rf);
     if(l < 0.17f && r < 0.17f){
+         printf("goForward is using Encoder!");
+        RightEncoder.reset();
+        LeftEncoder.reset();
         usePIDEncoder();    
+        RightEncoder.reset();
+        LeftEncoder.reset();
     }
     else{
+        printf("goForward is using IR PID!");
+        RightEncoder.reset();
+        LeftEncoder.reset();
         usePIDBoth(); //Goes straight and uses PID using IR and the encoders
+        RightEncoder.reset();
+        LeftEncoder.reset();
     }
     //Should be able to detect if there aren't walls and just work on the encoders
     //And should also record the distance for just one cell.
@@ -48,6 +58,7 @@
 }
 
 void MoveTo(geoCoord* current_coord, geoCoord* next, int curr_orientation){ //Note that turning automatically sets the global orientation constant
+    printf("Moving to: (%d, %d) From: (%d, %d):  \n", current_coord->x, current_coord->y, next->x, next->y);
     int diff_x = next->x - current_coord->x;
     int diff_y = next->y - current_coord->y;
     if(diff_x == 1){
@@ -93,9 +104,6 @@
             turn_left(1);
         }
     }
-    else{ //We've made it to the end!
-        wait_ms(1000000000);
-    }
     goForward(1);
     return;
 }
@@ -105,17 +113,25 @@
 geoCoord Start(0,0);
 
 void algorithm(){ //Implementation of floodfill algorithm
+for(int i = 0; i < 16; i++){
+        for(int k = 0; k < 16; k++){
+         cellarray[i][k].x = i; cellarray[i][k].y = k; //Initializes coordinates of geocoord array
+        }
+    }
     for(int i = 0; i < 16; i++){
         for(int k = 0; k < 16; k++){
-            cellarray[i][k].distance = std::sqrt((double)((double)cellarray[i][k].x - (double)Start.x)*((double)cellarray[i][k].x - (double)Start.x) + ((double)cellarray[i][k].y - (double)Start.y)*((double)cellarray[i][k].y - (double)Start.y)); //Initializes the distances of the cellarray
+            cellarray[i][k].distance = std::sqrt((double)(cellarray[i][k].x - Start.x)*(double)(cellarray[i][k].x - Start.x) + (double)(cellarray[i][k].y - Start.y)*(double)(cellarray[i][k].y - Start.y)); //Initializes the distances of the cellarray
+            printf("Cell Distance to (%d, %d): %.2f \n", i, k, cellarray[i][k].distance);
         }
     }
     std::stack<geoCoord*> cells_to_traverse;
     cells_to_traverse.push(&Start);
     while(cells_to_traverse.size() > 0){
         geoCoord* current = cells_to_traverse.top();
-        cells_to_traverse.pop();
+        printf("CurrentGeoCoord (%d, %d): %.2f \n", current->x, current->y, current->distance);
+        cells_to_traverse.pop();//Theory: It's not pushing the correct cells.
         int r_val = isWall();
+        printf("r_val: %d \n", r_val);
         for(int i = 1; i < 8; i+= 2) //1 for Forward, 3 for Left, 5 for South, 7 for Right. South probably isn't going to be a thing
         if(r_val&i == i){ //Potentially pushes 4 cells onto the stack
             if(i == 1 && current->y < 15)
@@ -128,12 +144,13 @@
                 cells_to_traverse.push(&cellarray[current->x][current->y-1]); //Checks bounds for the potential cells to push.
         }
         std::vector<geoCoord*> neighboring_cells;
-        for(int i = 0; i < cells_to_traverse.size(); i++){
+        unsigned int t_size = cells_to_traverse.size();
+        for(int i = 0; i < t_size; i++){
             neighboring_cells.push_back(cells_to_traverse.top()); //We temporarily put neighboring cells into a vector and sort them
             cells_to_traverse.pop();
         }
         std::sort(neighboring_cells.begin(), neighboring_cells.end(), compare);
-        for(int i = 0; i < neighboring_cells.size(); i++){
+        for(int i = 0; i < t_size; i++){
             cells_to_traverse.push(neighboring_cells[i]); //Puts the sorted vector into reverse order back into the stack
         }
         MoveTo(current, cells_to_traverse.top(), global_orientation); //Moves to the cell that is the closest to the target cell

diff -r 22e399fe87a4 -r 97941581fe81 constants.h
--- a/constants.h	Tue Nov 28 21:45:07 2017 +0000
+++ b/constants.h	Fri Dec 08 05:14:27 2017 +0000
@@ -28,9 +28,9 @@
     float leftFrontIRBase;
     float rightFrontIRBase;
     //PID For the IR Sensors.
-    float p = 0.2f;    //.32
+    float p = 0.4f;    //.32
     float i = 0.0001f;
-    float d = 0.15f; 
-    float frontP = 1.0;
+    float d = 0.17f; 
+    float frontP = 3.0;
 };
 #endif
\ No newline at end of file

diff -r 22e399fe87a4 -r 97941581fe81 header.h
--- a/header.h	Tue Nov 28 21:45:07 2017 +0000
+++ b/header.h	Fri Dec 08 05:14:27 2017 +0000
@@ -15,6 +15,7 @@
 // Front Left PC_1
 // Front Right PA_4
 // Side Left PA_0
+DigitalIn PushButton(PC_13);
 DigitalOut LeftIR(PB_7);
 DigitalOut FrontLeftIR(PB_0);
 DigitalOut FrontRightIR(PC_11);
@@ -34,8 +35,8 @@
 Serial pc(SERIAL_TX, SERIAL_RX); 
 QEI LeftEncoder(lfront, lback, NC, 4096, QEI::X4_ENCODING);
 QEI RightEncoder(rfront, rback, NC, 4096, QEI::X4_ENCODING);       
-const float rbase = 0.15f; //.09
-const float lbase = 0.163f;  //.1
+const float rbase = 0.146f; //.09
+const float lbase = 0.17f;  //.1
 
 static int global_orientation = 0;
 struct pid;
@@ -52,7 +53,7 @@
         kd = 0.8f;
       }
   float integral;
-  int prev;
+  float prev;
   float kp; //the ks should be negative to counteract error
   float ki;
   float kd;
@@ -64,22 +65,43 @@
     lpwmf.period(0.01f);
     lpwmb.period(0.01f);
     lpwmf = 0; //Previously started on, replace with lpwmf = lbase to make it start on (not a good idea)
-    rpwmb=0;
+    rpwmb = 0;
     rpwmf.period(0.01f);
     rpwmb.period(0.01f);
-    rpwmb=0;
+    rpwmb = 0;
     rpwmf = 0;
-    lman.kp = .009f;
-    lman.ki = .0f;
-    lman.kd = .0f;
-    rman.kp = .5f;
-    rman.ki = .1f;
-    rman.kd = .4f;
+    lman.kp = .0021f;
+    lman.ki = .0000f;
+    lman.kd = -.0005f;
+    rman.kp = .0019f;
+    rman.ki = .0000f;
+    rman.kd = -.0005;
 }
 void resetpid(struct pid *e) {
   e->integral = 0.0f;
   e->prev = 0.0f;
 }
+
+inline void brake(){
+    lpwmf = 1.0f;
+    lpwmb = 1.0f;
+    rpwmf = 1.0f;
+    rpwmb = 1.0f;
+}
+
+void printIRSensors(float leftValue, float rightValue, float leftFrontValue, float rightFrontValue){
+    pc.printf("left %f \n", leftValue);
+    pc.printf("right %f \n\n", rightValue);
+    pc.printf("front left%f \n", leftFrontValue);
+    pc.printf("front right%f \n", rightFrontValue);
+    pc.printf(" \n");
+}
+
+void printEncoders(){
+    pc.printf("LeftEncoder: %d \n", LeftEncoder.getPulses());
+    pc.printf("lEncoderDifference %f \n", LeftEncoder.getPulses());
+}
+
 float getFix(struct pid *e, float error, float time) {
   float d = (error - e->prev)/time;
   e->integral += error * time;
@@ -108,38 +130,52 @@
 
 void turn_right(int num_times){ //Turns clockwise
     int l_init = LeftEncoder.getPulses();
-    pc.printf("l_init %d \n", l_init);
-    for(int i = 0; i < num_times; i++){
+    //pc.printf("l_init %d \n", l_init);
+    /*for(int i = 0; i < num_times; i++){
         if(global_orientation == 1){
             global_orientation = 7; //It's now east
         }    
         else
             global_orientation-=2;
-    }
-    while((LeftEncoder.getPulses() - l_init) < 166*num_times){
+    }*/
+    while((LeftEncoder.getPulses() - l_init) < 214*num_times){
         lpwmb = 0.0f;
         rpwmf = 0.0f;
         lpwmf = lbase;
         rpwmb = rbase;
+        //printEncoders();   
     }
     lpwmf = 0.0f;
     rpwmb = 0.0f;
     wait(1.0);
+    return;
 }
 
 void turn_left(int num_times){
     int r_init = RightEncoder.getPulses();
-    pc.printf("r_init %d \n", r_init);
-    while((RightEncoder.getPulses() - r_init) < 150*num_times){
+    //pc.printf("r_init %d \n", r_init);
+    while((RightEncoder.getPulses() - r_init) < 142*num_times){
         lpwmf = 0.0f;
         rpwmb = 0.0f;
         lpwmb = lbase;
         rpwmf = rbase;
+        //printEncoders();
     }
     lpwmb = 0.0f;
     rpwmf = 0.0f;
     wait(1.0);
 }
+
+void FlashFrontIRs(float& flIR, float& frIR){
+    using namespace constants;
+    FrontLeftIR = 1;
+    FrontRightIR = 1;
+    wait_ms(10);
+    flIR = FrontLeftReceiver;
+    frIR = FrontRightReceiver;
+    return;
+}
+
 void FlashIRs(float& leftIRBase, float& rightIRBase, float& leftFrontIRBase, float& rightFrontIRBase){
     using namespace constants;
     LeftIR = 1;
@@ -158,19 +194,6 @@
     FrontRightIR = 0;
 }
 
-void printIRSensors(float leftValue, float rightValue, float leftFrontValue, float rightFrontValue){
-    pc.printf("left %f \n", leftValue);
-    pc.printf("right %f \n\n", rightValue);
-    pc.printf("front left%f \n", leftFrontValue);
-    pc.printf("front right%f \n", rightFrontValue);
-    pc.printf(" \n");
-}
-
-void printEncoders(){
-    pc.printf("LeftEncoder: %d \n", LeftEncoder.getPulses());
-    pc.printf("lEncoderDifference %f \n", LeftEncoder.getPulses());
-}
-
 void usePIDBoth(){
      using namespace constants;
         while(LeftEncoder.getPulses() < 130 && RightEncoder.getPulses() < 130){       
@@ -211,8 +234,8 @@
            
            prevleftError = leftError;
            prevrightError = rightError;
-            lspeed = constrain(0.05f, 1.0f, lbase - adjust_l - frontP*totalFrontChange);
-            rspeed = constrain(0.05f, 1.0f, rbase-adjust_r - frontP*totalFrontChange);
+            lspeed = constrain(0.03f, 1.0f, lbase - adjust_l - frontP*totalFrontChange);
+            rspeed = constrain(0.03f, 1.0f, rbase-adjust_r - frontP*totalFrontChange);
            //Set the motors to the desired speed. 0 Values indicate to the H-Bridge to go forward
            lpwmb = 0; rpwmb = 0;
            lpwmf = lspeed; rpwmf = rspeed;
@@ -220,38 +243,37 @@
         }
 }
 
-void usePIDEncoder(){
+void usePIDEncoder(){ //We only use this when there is nothing to the sides of the walls
     using namespace constants;
-        while(LeftEncoder.getPulses() < 130 && RightEncoder.getPulses() < 130){ //Only traverse one cell at a time.
-            float dt = t_time.read();
-            float left_encoder = LeftEncoder.getPulses();
-            float right_encoder = RightEncoder.getPulses();
-            printf("Left Encoder: %.2f %%\n", left_encoder);
-            printf("Right Encoder: %.2f %%\n", right_encoder);
-            float error = left_encoder - right_encoder; //Can be pos or neg
-            if(error == 0){
+        //brake();
+        //wait(0.1);
+        while(LeftEncoder.getPulses() < 435 && RightEncoder.getPulses() < 435){ //Only traverse one cell at a time.
+            float dt = 1.0f;
+            float FLIR, FRIR;
+            FlashFrontIRs(FLIR, FRIR);
+            /*if(FLIR > 0.15f || FRIR > 0.15f){
+                brake();
+                return;
+            }*/
+            float left_encoder = (float)LeftEncoder.getPulses();
+            float right_encoder = (float)RightEncoder.getPulses();
+            float error = (float)left_encoder - (float)right_encoder; //Can be pos or neg
+            if(error == 0.0f){
                 resetpid(&lman);
                 resetpid(&rman);
             }
             float adjust_r = getFix(&rman, error, dt);
-            lpwmf = lbase;
+            float adjust_l = getFix(&lman, error, dt);
+            lpwmf = constrain(0.06f, 0.45f, lbase-adjust_l);
             lpwmb = 0.0f;
-            rpwmf = constrain(0.04f, 0.5f, rbase-adjust_r);
+            rpwmf = constrain(0.06f, 0.45f, rbase+adjust_r);
             rpwmb = 0.0f;
-            if(RightEncoder.getPulses() > cell_length && LeftEncoder.getPulses() > cell_length){ //We've traversed a cell, update cell count;
-                RightEncoder.reset(); 
-                LeftEncoder.reset();   
-            }
-        if(leftValue> 0.6f && leftFrontValue > 0.6f)//Maybe we should brake in here?
-        {
-            return;
-        }
-        else if(rightFrontValue > 0.6f && rightValue > 0.6f)
-        {
-            return;
-        }   
-       t_time.reset();
+            printf("Left Encoder: %.2f Right Encoder: %.2f DT: %.2f Error: %.2f adjust_l: %.2f adjust_r: %.2f \n", left_encoder, right_encoder, dt, error, adjust_l, adjust_r);
+            t_time.reset();
     }
-  }
+    RightEncoder.reset();
+    LeftEncoder.reset();
+    return;
+}
     
 #endif
\ No newline at end of file

diff -r 22e399fe87a4 -r 97941581fe81 main.cpp
--- a/main.cpp	Tue Nov 28 21:45:07 2017 +0000
+++ b/main.cpp	Fri Dec 08 05:14:27 2017 +0000
@@ -2,10 +2,150 @@
 #include "QEI.h"
 #include "header.h"
 #include "algorithm.h"
+/*
 int main() {
+    using namespace std;
+    bool pushed = true;
+    PushButton.mode(PullDown);
+    t_time.start();
     initmotors_and_pid(); //Sets the PWM frequency
-    algorithm();
+    while(pushed){
+        if(PushButton.read() == 0){ //If PushButton is pressed down
+            pushed = false;
+        }
+        wait(0.1);
+    }
+    while(!pushed){
+        RightEncoder.reset(); 
+        LeftEncoder.reset(); 
+        //usePIDEncoder();
+        //algorithm();
+        RightEncoder.reset(); 
+        LeftEncoder.reset(); 
+    }
 }
-
+*/
+int main(){
+    using namespace std;
+    bool pushed = true;
+    PushButton.mode(PullDown);
+    t_time.start();
+    initmotors_and_pid(); //Sets the PWM frequency
+    while(pushed){
+        if(PushButton.read() == 0){ //If PushButton is pressed down
+            pushed = false;
+        }
+        wait(0.1);
+    }
+    while(!pushed){   
+           using namespace constants;
+           float dt = t_time.read();
+           float lspeed = 0; float rspeed = 0;
+           prevRightFrontValue = rightFrontValue; //update previous values for change in IRfront
+           prevLeftFrontValue = leftFrontValue;
+                //IR SAMPLING
+                LeftIR = 1;
+                RightIR = 1;
+                wait(.01);
+                leftValue = LeftReceiver;
+                rightValue = RightReceiver;
+                LeftIR = 0;
+                RightIR = 0;
+                FrontLeftIR = 1;
+                leftFrontValue = FrontLeftReceiver;
+                FrontRightIR = 1;
+                rightFrontValue = FrontRightReceiver;
+                wait(.01);
+                FrontLeftIR = 0;
+                FrontRightIR = 0;
+               //pc.printf( "%f \n" , leftValue);
+               //pc.printf("%f \n", rightValue);
+          //TURNING CONDITIONS
+            if(leftValue> 0.34f && leftFrontValue > 0.17f)    //.34L .15LF   Wall on left and in front
+            {
+                brake();
+                wait(0.2);
+                turn_right(1);
+               // pc.printf("I turned right\n");
+            }
+            else if(rightValue> 0.55f && rightFrontValue > 0.14f)  //.35R .15RF Wall on right and in front
+            {
+                brake();
+                wait(0.2);
+                turn_left(1);
+                //pc.printf("I turned left\n");
+            }
+            else if((leftValue < 0.34 && leftFrontValue < 0.0195f))    // No wall on left and in front
+            {   
+                LeftEncoder.reset();
+                RightEncoder.reset();
+                //brake();
+                //wait(0.4);
+                usePIDEncoder();
+                //pc.printf("Use encoder PID for left side\n");
+            }
+            else if((rightValue < 0.34f && rightFrontValue < 0.0205f))  // No wall on right and in front
+            {    
+                LeftEncoder.reset();
+                RightEncoder.reset();
+                //brake();
+                //wait(0.4);
+                usePIDEncoder();
+                //pc.printf("Use encoder PID for right side\n");
+            }  
+            else if(leftValue < 0.35f && leftFrontValue < 0.167f && rightValue < 0.35f && rightFrontValue < 0.167f)
+            {
+                LeftEncoder.reset();
+                RightEncoder.reset();
+                //brake();
+                //wait(0.4);
+                usePIDEncoder();
+                //pc.printf("Use encoder PID for left and right sides");
+            }
+           //ProcessIR(dt, ir_lman, lspeed, rspeed);
+           leftError = leftIRBase - leftValue;
+           rightError = rightIRBase - rightValue;
+           totalRightError += rightError;
+           totalLeftError += leftError;
+           leftFrontError = leftFrontIRBase - leftFrontValue;
+           rightFrontError = rightFrontIRBase - rightFrontValue;
+           changeLeftFrontValue = leftFrontValue-prevLeftFrontValue;
+           changeRightFrontValue = rightFrontValue - prevRightFrontValue;
+           
+           if(changeLeftFrontValue <0){
+               changeLeftFrontValue = 0;
+               }
+               
+            if(changeRightFrontValue <0){
+               changeRightFrontValue = 0;
+               }
+               totalFrontChange = changeRightFrontValue + changeLeftFrontValue;
+               
+           
+           adjust_l = p*leftError-d*prevleftError-i*totalLeftError ;
+           adjust_r = p*rightError-d*prevrightError-i*totalRightError ;
+           
+           prevleftError = leftError;
+           prevrightError = rightError;
+          // if(adjust_l >0)
+           {
+            //   adjust_l=0;
+           }
+          // if(adjust_r >0)
+           {
+            //   adjust_r=0;
+           }
+        lspeed = constrain(0.05f, 1.0f, lbase-adjust_l - frontP*totalFrontChange);
+        rspeed = constrain(0.05f, 1.0f, rbase-adjust_r - frontP*totalFrontChange);
+            lpwmb = 0; rpwmb = 0;
+          lpwmf = lspeed; rpwmf = rspeed;
+           //pc.printf("left %f \n", leftValue);
+           //pc.printf("right %f \n\n", rightValue);
+           //pc.printf("front left%f \n", leftFrontValue);
+           //pc.printf("front right%f \n\n", rightFrontValue);
+           //pc.printf(" \n");
+           t_time.reset();
+        }
+    }
+    
 
-