Revision 8:22e399fe87a4, committed 2017-11-28

Comitter:

Showboo

Date:

Tue Nov 28 21:45:07 2017 +0000

Parent:

7:7cdb0381e1b8

Child:

9:97941581fe81

Commit message:

Updated for the algorithm

Changed in this revision

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/algorithm.h	Tue Nov 28 21:45:07 2017 +0000
@@ -0,0 +1,143 @@
+#ifndef ALGO_H
+#define ALGO_H
+#include <cmath>
+#include <stack>
+#include <iostream>
+#include <vector>
+#include <algorithm>
+#include "header.h"
+struct geoCoord{
+    geoCoord(int x_in, int y_in){x = x_in; y = y_in;}
+    geoCoord(){x = 0; y = 0;}
+    int x, y;
+    double distance;
+};
+
+bool compare(geoCoord* left, geoCoord* right){
+    return left->distance > right->distance;
+}
+
+int isWall(){
+    float ir_r[4]; //ir_readings, in the order of l, r, lf, rf
+    FlashIRs(ir_r[0], ir_r[1], ir_r[2], ir_r[3]);
+    int return_val = 0;
+    if (ir_r[2] > 0.2f && ir_r[3] > 0.2f){
+     return_val = return_val | 1;
+    }
+    if(ir_r[1] > 0.3f){
+      return_val = return_val | 7;
+    }
+    if(ir_r[0] > 0.3f){
+      return_val = return_val | 3;
+    }
+    return return_val; //This implementation is wrong but I got hungry.
+}
+
+void goForward(int times){
+    float l,r,lf,rf;
+    FlashIRs(l, r, lf, rf);
+    if(l < 0.17f && r < 0.17f){
+        usePIDEncoder();    
+    }
+    else{
+        usePIDBoth(); //Goes straight and uses PID using IR and the encoders
+    }
+    //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.
+    return;
+}
+
+void MoveTo(geoCoord* current_coord, geoCoord* next, int curr_orientation){ //Note that turning automatically sets the global orientation constant
+    int diff_x = next->x - current_coord->x;
+    int diff_y = next->y - current_coord->y;
+    if(diff_x == 1){
+        if(curr_orientation == SOUTH)
+            turn_left(1); 
+        else if(curr_orientation == WEST){
+            turn_right(2);
+        }
+        else if(curr_orientation == NORTH){
+            turn_right(1);
+        }
+    }
+    else if(diff_x == -1){
+        if(curr_orientation == SOUTH){
+            turn_right(1);
+        }
+        else if(curr_orientation == NORTH){
+            turn_left(1);
+        }
+        else if(curr_orientation == EAST){
+            turn_right(2);
+        }
+    }
+    else if(diff_y == 1){
+        if(curr_orientation == SOUTH){
+            turn_right(2);
+        }
+        else if(curr_orientation == EAST){
+            turn_left(1);
+        }
+        else if(curr_orientation == WEST){
+            turn_right(1);
+        }
+    }
+    else if(diff_y == -1){
+        if(curr_orientation == NORTH){
+            turn_right(2);
+        }
+        else if(curr_orientation == EAST){
+            turn_right(1);
+        }
+        else if(curr_orientation == WEST){
+            turn_left(1);
+        }
+    }
+    else{ //We've made it to the end!
+        wait_ms(1000000000);
+    }
+    goForward(1);
+    return;
+}
+
+geoCoord cellarray[16][16];
+geoCoord Target(8,8); //Target cell was assumed to be here.
+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].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
+        }
+    }
+    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();
+        int r_val = isWall();
+        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)
+                cells_to_traverse.push(&cellarray[current->x][current->y + 1]);
+            else if(i == 3 && current->x > 0)
+                cells_to_traverse.push(&cellarray[current->x - 1][current->y]);
+            else if(i == 7 && current->x < 15)
+                cells_to_traverse.push(&cellarray[current->x+1][current->y]);
+            else if(i == 5 && current->y > 0)
+                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++){
+            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++){
+            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
+        cells_to_traverse.pop();
+    }
+}
+#endif
\ No newline at end of file

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/constants.h	Tue Nov 28 21:45:07 2017 +0000
@@ -0,0 +1,36 @@
+#ifndef CONSTANTS_H
+#define CONSTANTS_H
+namespace constants{
+    const int cell_length = 130;
+    float leftValue;
+    float rightValue;
+    float leftFrontValue;
+    float rightFrontValue;
+    //Errors for all the IR Readings that are used in PID later below.
+    float leftError = 0;
+    float rightError = 0;
+    float prevrightError = 0;
+    float prevleftError = 0;
+    float prevLeftFrontValue= 0;
+    float prevRightFrontValue = 0;
+    float changeLeftFrontValue = 0;
+    float changeRightFrontValue = 0;
+    float totalFrontChange = 0;
+    float leftFrontError = 0;
+    float rightFrontError = 0;
+    float totalLeftError = 0;
+    float totalRightError =0;
+    //Adjustment values that are set for the PID from the IR Sensors. 
+    float adjust_l = 0;
+    float adjust_r = 0;
+    float leftIRBase;
+    float rightIRBase;
+    float leftFrontIRBase;
+    float rightFrontIRBase;
+    //PID For the IR Sensors.
+    float p = 0.2f;    //.32
+    float i = 0.0001f;
+    float d = 0.15f; 
+    float frontP = 1.0;
+};
+#endif
\ No newline at end of file

--- a/header.h	Tue Nov 28 19:58:02 2017 +0000
+++ b/header.h	Tue Nov 28 21:45:07 2017 +0000
@@ -1,5 +1,10 @@
 #ifndef Header_H
 #define Header_H
+#include "constants.h"
+#define NORTH 1
+#define WEST 3
+#define SOUTH 5
+#define EAST 7
 //IR:
 //PB 7 Left Side
 //PB 0 Front Left
@@ -31,12 +36,14 @@
 QEI RightEncoder(rfront, rback, NC, 4096, QEI::X4_ENCODING);       
 const float rbase = 0.15f; //.09
 const float lbase = 0.163f;  //.1
-const float WALL_IR_L = 0.74f;
-const float WALL_IR_R = 0.74f;
-const float WALL_IR_FL = 0.74f;
-const float WALL_IR_FR = 0.74f;    
+
+static int global_orientation = 0;
+struct pid;
+
 Timer t_time;
+//Initializes different PIDs for use
 struct pid { //Note that because we have two different types of distance sensors (Andrew's works a little differently than Jeffrey's we should have two different errors. To stay straight though we can just use one side right?)
+  public:
   pid(){
         integral = 0.0f;
         prev = 0.0f;
@@ -51,6 +58,24 @@
   float kd;
 };
 
+pid lman, rman;
+
+void initmotors_and_pid(){
+    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;
+    rpwmf.period(0.01f);
+    rpwmb.period(0.01f);
+    rpwmb=0;
+    rpwmf = 0;
+    lman.kp = .009f;
+    lman.ki = .0f;
+    lman.kd = .0f;
+    rman.kp = .5f;
+    rman.ki = .1f;
+    rman.kd = .4f;
+}
 void resetpid(struct pid *e) {
   e->integral = 0.0f;
   e->prev = 0.0f;
@@ -81,80 +106,152 @@
     unsigned int statuscode;
 };
 
-unsigned int readIR(IRstruct& in_ir){
-    in_ir.leftIR = LeftReceiver.read();
-    in_ir.rightIR = RightReceiver.read();
-    in_ir.leftfrontIR = FrontLeftReceiver.read();
-    in_ir.rightfrontIR = FrontRightReceiver.read();
-    //pc.printf("LeftIR: %f RightIR: %f leftfronIR: %f rightfrontIR: %f \n", in_ir.leftIR, in_ir.rightIR, in_ir.leftfrontIR, in_ir.rightfrontIR); 
-    
-    //We then set the status codes to be analyzed later for what we should do
-    in_ir.statuscode = (in_ir.statuscode|0x0);
-    
-    //0000...FL,FR,L,R
-    //Bottom is more of an abstraction, we probably don't even need it
-    if(in_ir.leftIR > WALL_IR_L)
-        in_ir.statuscode = (in_ir.statuscode|2); // 0...0 | 1 -> 0...1
-    if(in_ir.rightIR > WALL_IR_R)
-        in_ir.statuscode = in_ir.statuscode|1;
-    if(in_ir.leftfrontIR > WALL_IR_FL)
-        in_ir.statuscode = in_ir.statuscode|8;
-    if(in_ir.rightfrontIR > WALL_IR_FR)
-        in_ir.statuscode = in_ir.statuscode|4;
-    return in_ir.statuscode;
-}
-
-void turn_right(){
+void turn_right(int num_times){ //Turns clockwise
     int l_init = LeftEncoder.getPulses();
     pc.printf("l_init %d \n", l_init);
-    while((LeftEncoder.getPulses() - l_init) < 166){
+    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){
         lpwmb = 0.0f;
         rpwmf = 0.0f;
         lpwmf = lbase;
         rpwmb = rbase;
-        //pc.printf("LeftEncoder: %d \n", LeftEncoder.getPulses());
-       // pc.printf("lEncoderDifference %f \n", LeftEncoder.getPulses() - l_init);
     }
-        //lpwmb = 0.0f;
-        //rpwmf = 0.0f;
-        lpwmf = 0.0f;
-        rpwmb = 0.0f;
-        wait(1.0);
+    lpwmf = 0.0f;
+    rpwmb = 0.0f;
+    wait(1.0);
 }
 
-void turn_left(){
+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){
+    while((RightEncoder.getPulses() - r_init) < 150*num_times){
         lpwmf = 0.0f;
         rpwmb = 0.0f;
         lpwmb = lbase;
         rpwmf = rbase;
-       // pc.printf("RightEncoder: %d \n", RightEncoder.getPulses());
-        //pc.printf("rEncoderDifference %f: \n", RightEncoder.getPulses()-r_init);
     }
     lpwmb = 0.0f;
     rpwmf = 0.0f;
-    wait( 1.0);
+    wait(1.0);
+}
+void FlashIRs(float& leftIRBase, float& rightIRBase, float& leftFrontIRBase, float& rightFrontIRBase){
+    using namespace constants;
+    LeftIR = 1;
+    RightIR = 1;
+    wait_ms(10);
+    leftIRBase = LeftReceiver;
+    rightIRBase = RightReceiver;
+    LeftIR = 0;
+    RightIR = 0;
+    FrontLeftIR = 1;
+    leftFrontIRBase = FrontLeftReceiver;
+    FrontRightIR = 1;
+    rightFrontIRBase= FrontRightReceiver;
+    wait_ms(10);
+    FrontLeftIR = 0;
+    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 ProcessIR(float dt, pid& IR_lman, float& lspeed, float& rspeed){
-    IRstruct ir_read;
-    int status = readIR(ir_read); 
-    if(ir_read.leftIR > WALL_IR_L && ir_read.leftfrontIR > WALL_IR_FL && ir_read.rightfrontIR > WALL_IR_FR && ir_read.rightIR < WALL_IR_R){ //High in front and left -> Turn right
-        turn_right();
-    }
-    else if(ir_read.rightIR > WALL_IR_R && ir_read.leftfrontIR > WALL_IR_FL && ir_read.rightfrontIR > WALL_IR_FR){ //High in front and right -> Turn left
-        turn_left();
+void usePIDBoth(){
+     using namespace constants;
+        while(LeftEncoder.getPulses() < 130 && RightEncoder.getPulses() < 130){       
+           float dt = t_time.read();
+           float lspeed = 0; float rspeed = 0;
+           prevRightFrontValue = rightFrontValue;
+           prevLeftFrontValue = leftFrontValue;
+           FlashIRs(leftValue, rightValue, leftFrontValue, rightFrontValue);
+           //If we detect a wall, turn right or turn left.
+            if(leftValue> 0.34f && leftFrontValue > 0.15f)
+            {
+                return;
+                //turn_right(1);
+            }
+            else if(rightFrontValue > 0.15f && rightValue > 0.35f)
+            {
+                return;
+                //turn_left(1);
+            }
+            //Calculates PID Adjustment from basic Algebra
+           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 ; //Sets the final adjustment values we expect for the motor from Sensor readings
+           
+           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);
+           //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;
+           t_time.reset();
+        }
+}
+
+void usePIDEncoder(){
+    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){
+                resetpid(&lman);
+                resetpid(&rman);
+            }
+            float adjust_r = getFix(&rman, error, dt);
+            lpwmf = lbase;
+            lpwmb = 0.0f;
+            rpwmf = constrain(0.04f, 0.5f, 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();
     }
-    float error_ir = (ir_read.rightIR - ir_read.leftIR);
-    /*if(error_ir < 0.0001f){    
-        resetpid(&IR_lman);
-    }*/
-    float adjust_l = getFix(&IR_lman, error_ir, dt);
-    lspeed = constrain(0.0f, 0.3f, lbase - adjust_l);
-    rspeed = constrain(0.0f, 0.3f, rbase);
-    pc.printf("adjust_l: %f lspeed: %f rspeed: %f error_ir: %f dt: %f \n", adjust_l, lspeed, rspeed, error_ir, dt);
-}
+  }
     
 #endif
\ No newline at end of file

--- a/main.cpp	Tue Nov 28 19:58:02 2017 +0000
+++ b/main.cpp	Tue Nov 28 21:45:07 2017 +0000
@@ -1,166 +1,10 @@
 #include "mbed.h"
 #include "QEI.h"
 #include "header.h"
-
-inline void pulse_ir(int in){
-    for(int i = 0; i < 4; i++){
-        LeftIR = in;
-        
-        FrontLeftIR = in;
-        FrontRightIR = in;
-        RightIR = in;
-    }
-}
-
-
-            
-
+#include "algorithm.h"
 int main() {
-
-float leftValue;
-float rightValue;
-float leftFrontValue;
-float rightFrontValue;
-float leftError = 0;
-float rightError = 0;
-float prevrightError = 0;
-float prevleftError = 0;
-float prevLeftFrontValue= 0;
-float prevRightFrontValue = 0;
-float changeLeftFrontValue = 0;
-float changeRightFrontValue = 0;
-float totalFrontChange = 0;
-float leftFrontError = 0;
-float rightFrontError = 0;
-float totalLeftError = 0;
-float totalRightError =0;
-float adjust_l = 0;
-float adjust_r = 0;
-
-float leftIRBase;
-float rightIRBase;
-float leftFrontIRBase;
-float rightFrontIRBase;
-float p =0.2f;    //.32
-float i =0.0001f;
-float d=0.15f; 
-float frontP = 1.0;
-    LeftIR = 1;
-            RightIR = 1;
-            leftIRBase = LeftReceiver;
-            rightIRBase = RightReceiver;
-            LeftIR = 0;
-            RightIR = 0;
-            wait_ms(10);
-            FrontLeftIR = 1;
-            leftFrontIRBase = FrontLeftReceiver;
-            FrontRightIR = 1;
-            rightFrontIRBase= FrontRightReceiver;
-            FrontLeftIR = 0;
-            FrontRightIR = 0;
-            wait_ms(10);
-            
-    pid lman, rman;
-    lman.kp = .009f;
-    lman.ki = .0f;
-    lman.kd = .0f;
-    rman.kp = .5f;
-    rman.ki = .1f;
-    rman.kd = .4f;
-    
-    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;
-    rpwmf.period(0.01f);
-    rpwmb.period(0.01f);
-    rpwmb=0;
-    rpwmf = 0;
-    
-    pid ir_lman, ir_rman;
-    ir_lman.kp = .0001f;
-    ir_lman.ki = .0f;
-    ir_lman.kd = .0f;
-    ir_rman.kp = .5f;
-    ir_rman.ki = .1f;
-    ir_rman.kd = .4f;    
-    t_time.start();
-    while(1){       
-       float dt = t_time.read();
-       float lspeed = 0; float rspeed = 0;
-       prevRightFrontValue = rightFrontValue;
-       prevLeftFrontValue = leftFrontValue;
-            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);
-        if(leftValue> 0.34f && leftFrontValue > 0.15f)
-        {
-            turn_right();
-        }
-        else if(rightFrontValue > 0.15f && rightValue > 0.35f)
-        {
-            turn_left();
-        }
-            
-            
-            
-       //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", rightFrontValue);
-       //pc.printf(" \n");
-       t_time.reset();
-    }
+    initmotors_and_pid(); //Sets the PWM frequency
+    algorithm();
 }