Anon Anon / Mbed 2 deprecated MicroMousewithFloodFill

Updated with the Algorithm

Dependencies:   QEI mbed

Fork of MM_rat_Assignment4-newwest by Evan Brown

main.cpp@9:97941581fe81, 2017-12-08 (annotated)

Committer:
Showboo
Date:
Fri Dec 08 05:14:27 2017 +0000
Revision:
9:97941581fe81
Parent:
8:22e399fe87a4 
Ready for Competition

Who changed what in which revision?

UserRevisionLine numberNew contents of line
Showboo 0:f7fc09f9c7ce 1 #include "mbed.h"
Showboo 0:f7fc09f9c7ce 2 #include "QEI.h"
Showboo 0:f7fc09f9c7ce 3 #include "header.h"
Showboo 8:22e399fe87a4 4 #include "algorithm.h"
Showboo 9:97941581fe81 5 /*
Showboo 0:f7fc09f9c7ce 6 int main() {
Showboo 9:97941581fe81 7 using namespace std;
Showboo 9:97941581fe81 8 bool pushed = true;
Showboo 9:97941581fe81 9 PushButton.mode(PullDown);
Showboo 9:97941581fe81 10 t_time.start();
Showboo 8:22e399fe87a4 11 initmotors_and_pid(); //Sets the PWM frequency
Showboo 9:97941581fe81 12 while(pushed){
Showboo 9:97941581fe81 13 if(PushButton.read() == 0){ //If PushButton is pressed down
Showboo 9:97941581fe81 14 pushed = false;
Showboo 9:97941581fe81 15 }
Showboo 9:97941581fe81 16 wait(0.1);
Showboo 9:97941581fe81 17 }
Showboo 9:97941581fe81 18 while(!pushed){
Showboo 9:97941581fe81 19 RightEncoder.reset();
Showboo 9:97941581fe81 20 LeftEncoder.reset();
Showboo 9:97941581fe81 21 //usePIDEncoder();
Showboo 9:97941581fe81 22 //algorithm();
Showboo 9:97941581fe81 23 RightEncoder.reset();
Showboo 9:97941581fe81 24 LeftEncoder.reset();
Showboo 9:97941581fe81 25 }
Showboo 0:f7fc09f9c7ce 26 }
Showboo 9:97941581fe81 27 */
Showboo 9:97941581fe81 28 int main(){
Showboo 9:97941581fe81 29 using namespace std;
Showboo 9:97941581fe81 30 bool pushed = true;
Showboo 9:97941581fe81 31 PushButton.mode(PullDown);
Showboo 9:97941581fe81 32 t_time.start();
Showboo 9:97941581fe81 33 initmotors_and_pid(); //Sets the PWM frequency
Showboo 9:97941581fe81 34 while(pushed){
Showboo 9:97941581fe81 35 if(PushButton.read() == 0){ //If PushButton is pressed down
Showboo 9:97941581fe81 36 pushed = false;
Showboo 9:97941581fe81 37 }
Showboo 9:97941581fe81 38 wait(0.1);
Showboo 9:97941581fe81 39 }
Showboo 9:97941581fe81 40 while(!pushed){
Showboo 9:97941581fe81 41 using namespace constants;
Showboo 9:97941581fe81 42 float dt = t_time.read();
Showboo 9:97941581fe81 43 float lspeed = 0; float rspeed = 0;
Showboo 9:97941581fe81 44 prevRightFrontValue = rightFrontValue; //update previous values for change in IRfront
Showboo 9:97941581fe81 45 prevLeftFrontValue = leftFrontValue;
Showboo 9:97941581fe81 46 //IR SAMPLING
Showboo 9:97941581fe81 47 LeftIR = 1;
Showboo 9:97941581fe81 48 RightIR = 1;
Showboo 9:97941581fe81 49 wait(.01);
Showboo 9:97941581fe81 50 leftValue = LeftReceiver;
Showboo 9:97941581fe81 51 rightValue = RightReceiver;
Showboo 9:97941581fe81 52 LeftIR = 0;
Showboo 9:97941581fe81 53 RightIR = 0;
Showboo 9:97941581fe81 54 FrontLeftIR = 1;
Showboo 9:97941581fe81 55 leftFrontValue = FrontLeftReceiver;
Showboo 9:97941581fe81 56 FrontRightIR = 1;
Showboo 9:97941581fe81 57 rightFrontValue = FrontRightReceiver;
Showboo 9:97941581fe81 58 wait(.01);
Showboo 9:97941581fe81 59 FrontLeftIR = 0;
Showboo 9:97941581fe81 60 FrontRightIR = 0;
Showboo 9:97941581fe81 61 //pc.printf( "%f \n" , leftValue);
Showboo 9:97941581fe81 62 //pc.printf("%f \n", rightValue);
Showboo 9:97941581fe81 63 //TURNING CONDITIONS
Showboo 9:97941581fe81 64 if(leftValue> 0.34f && leftFrontValue > 0.17f) //.34L .15LF Wall on left and in front
Showboo 9:97941581fe81 65 {
Showboo 9:97941581fe81 66 brake();
Showboo 9:97941581fe81 67 wait(0.2);
Showboo 9:97941581fe81 68 turn_right(1);
Showboo 9:97941581fe81 69 // pc.printf("I turned right\n");
Showboo 9:97941581fe81 70 }
Showboo 9:97941581fe81 71 else if(rightValue> 0.55f && rightFrontValue > 0.14f) //.35R .15RF Wall on right and in front
Showboo 9:97941581fe81 72 {
Showboo 9:97941581fe81 73 brake();
Showboo 9:97941581fe81 74 wait(0.2);
Showboo 9:97941581fe81 75 turn_left(1);
Showboo 9:97941581fe81 76 //pc.printf("I turned left\n");
Showboo 9:97941581fe81 77 }
Showboo 9:97941581fe81 78 else if((leftValue < 0.34 && leftFrontValue < 0.0195f)) // No wall on left and in front
Showboo 9:97941581fe81 79 {
Showboo 9:97941581fe81 80 LeftEncoder.reset();
Showboo 9:97941581fe81 81 RightEncoder.reset();
Showboo 9:97941581fe81 82 //brake();
Showboo 9:97941581fe81 83 //wait(0.4);
Showboo 9:97941581fe81 84 usePIDEncoder();
Showboo 9:97941581fe81 85 //pc.printf("Use encoder PID for left side\n");
Showboo 9:97941581fe81 86 }
Showboo 9:97941581fe81 87 else if((rightValue < 0.34f && rightFrontValue < 0.0205f)) // No wall on right and in front
Showboo 9:97941581fe81 88 {
Showboo 9:97941581fe81 89 LeftEncoder.reset();
Showboo 9:97941581fe81 90 RightEncoder.reset();
Showboo 9:97941581fe81 91 //brake();
Showboo 9:97941581fe81 92 //wait(0.4);
Showboo 9:97941581fe81 93 usePIDEncoder();
Showboo 9:97941581fe81 94 //pc.printf("Use encoder PID for right side\n");
Showboo 9:97941581fe81 95 }
Showboo 9:97941581fe81 96 else if(leftValue < 0.35f && leftFrontValue < 0.167f && rightValue < 0.35f && rightFrontValue < 0.167f)
Showboo 9:97941581fe81 97 {
Showboo 9:97941581fe81 98 LeftEncoder.reset();
Showboo 9:97941581fe81 99 RightEncoder.reset();
Showboo 9:97941581fe81 100 //brake();
Showboo 9:97941581fe81 101 //wait(0.4);
Showboo 9:97941581fe81 102 usePIDEncoder();
Showboo 9:97941581fe81 103 //pc.printf("Use encoder PID for left and right sides");
Showboo 9:97941581fe81 104 }
Showboo 9:97941581fe81 105 //ProcessIR(dt, ir_lman, lspeed, rspeed);
Showboo 9:97941581fe81 106 leftError = leftIRBase - leftValue;
Showboo 9:97941581fe81 107 rightError = rightIRBase - rightValue;
Showboo 9:97941581fe81 108 totalRightError += rightError;
Showboo 9:97941581fe81 109 totalLeftError += leftError;
Showboo 9:97941581fe81 110 leftFrontError = leftFrontIRBase - leftFrontValue;
Showboo 9:97941581fe81 111 rightFrontError = rightFrontIRBase - rightFrontValue;
Showboo 9:97941581fe81 112 changeLeftFrontValue = leftFrontValue-prevLeftFrontValue;
Showboo 9:97941581fe81 113 changeRightFrontValue = rightFrontValue - prevRightFrontValue;
Showboo 9:97941581fe81 114
Showboo 9:97941581fe81 115 if(changeLeftFrontValue <0){
Showboo 9:97941581fe81 116 changeLeftFrontValue = 0;
Showboo 9:97941581fe81 117 }
Showboo 9:97941581fe81 118
Showboo 9:97941581fe81 119 if(changeRightFrontValue <0){
Showboo 9:97941581fe81 120 changeRightFrontValue = 0;
Showboo 9:97941581fe81 121 }
Showboo 9:97941581fe81 122 totalFrontChange = changeRightFrontValue + changeLeftFrontValue;
Showboo 9:97941581fe81 123
Showboo 9:97941581fe81 124
Showboo 9:97941581fe81 125 adjust_l = p*leftError-d*prevleftError-i*totalLeftError ;
Showboo 9:97941581fe81 126 adjust_r = p*rightError-d*prevrightError-i*totalRightError ;
Showboo 9:97941581fe81 127
Showboo 9:97941581fe81 128 prevleftError = leftError;
Showboo 9:97941581fe81 129 prevrightError = rightError;
Showboo 9:97941581fe81 130 // if(adjust_l >0)
Showboo 9:97941581fe81 131 {
Showboo 9:97941581fe81 132 // adjust_l=0;
Showboo 9:97941581fe81 133 }
Showboo 9:97941581fe81 134 // if(adjust_r >0)
Showboo 9:97941581fe81 135 {
Showboo 9:97941581fe81 136 // adjust_r=0;
Showboo 9:97941581fe81 137 }
Showboo 9:97941581fe81 138 lspeed = constrain(0.05f, 1.0f, lbase-adjust_l - frontP*totalFrontChange);
Showboo 9:97941581fe81 139 rspeed = constrain(0.05f, 1.0f, rbase-adjust_r - frontP*totalFrontChange);
Showboo 9:97941581fe81 140 lpwmb = 0; rpwmb = 0;
Showboo 9:97941581fe81 141 lpwmf = lspeed; rpwmf = rspeed;
Showboo 9:97941581fe81 142 //pc.printf("left %f \n", leftValue);
Showboo 9:97941581fe81 143 //pc.printf("right %f \n\n", rightValue);
Showboo 9:97941581fe81 144 //pc.printf("front left%f \n", leftFrontValue);
Showboo 9:97941581fe81 145 //pc.printf("front right%f \n\n", rightFrontValue);
Showboo 9:97941581fe81 146 //pc.printf(" \n");
Showboo 9:97941581fe81 147 t_time.reset();
Showboo 9:97941581fe81 148 }
Showboo 9:97941581fe81 149 }
Showboo 9:97941581fe81 150
evenbrownie 3:4e34437daef4 151