Anon Anon / Mbed 2 deprecated MicroMousewithFloodFill

Updated with the Algorithm

Dependencies:   QEI mbed

Fork of MM_rat_Assignment4-newwest by Evan Brown

main.cpp@7:7cdb0381e1b8, 2017-11-28 (annotated)

Committer:
evenbrownie
Date:
Tue Nov 28 19:58:02 2017 +0000
Revision:
7:7cdb0381e1b8
Parent:
6:ce498700a28c
Child:
8:22e399fe87a4
PID with front stopping capability

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 0:f7fc09f9c7ce 4
Showboo 0:f7fc09f9c7ce 5 inline void pulse_ir(int in){
Showboo 1:fb18b43590e6 6 for(int i = 0; i < 4; i++){
Showboo 0:f7fc09f9c7ce 7 LeftIR = in;
Showboo 1:fb18b43590e6 8
Showboo 0:f7fc09f9c7ce 9 FrontLeftIR = in;
Showboo 0:f7fc09f9c7ce 10 FrontRightIR = in;
Showboo 0:f7fc09f9c7ce 11 RightIR = in;
Showboo 1:fb18b43590e6 12 }
Showboo 0:f7fc09f9c7ce 13 }
Showboo 0:f7fc09f9c7ce 14
evenbrownie 2:7da65637e512 15
evenbrownie 2:7da65637e512 16
evenbrownie 2:7da65637e512 17
Showboo 0:f7fc09f9c7ce 18 int main() {
evenbrownie 2:7da65637e512 19
evenbrownie 2:7da65637e512 20 float leftValue;
evenbrownie 2:7da65637e512 21 float rightValue;
evenbrownie 2:7da65637e512 22 float leftFrontValue;
evenbrownie 2:7da65637e512 23 float rightFrontValue;
evenbrownie 2:7da65637e512 24 float leftError = 0;
evenbrownie 2:7da65637e512 25 float rightError = 0;
evenbrownie 3:4e34437daef4 26 float prevrightError = 0;
evenbrownie 3:4e34437daef4 27 float prevleftError = 0;
evenbrownie 7:7cdb0381e1b8 28 float prevLeftFrontValue= 0;
evenbrownie 7:7cdb0381e1b8 29 float prevRightFrontValue = 0;
evenbrownie 7:7cdb0381e1b8 30 float changeLeftFrontValue = 0;
evenbrownie 7:7cdb0381e1b8 31 float changeRightFrontValue = 0;
evenbrownie 7:7cdb0381e1b8 32 float totalFrontChange = 0;
evenbrownie 2:7da65637e512 33 float leftFrontError = 0;
evenbrownie 2:7da65637e512 34 float rightFrontError = 0;
evenbrownie 3:4e34437daef4 35 float totalLeftError = 0;
evenbrownie 3:4e34437daef4 36 float totalRightError =0;
evenbrownie 2:7da65637e512 37 float adjust_l = 0;
evenbrownie 2:7da65637e512 38 float adjust_r = 0;
evenbrownie 2:7da65637e512 39
evenbrownie 2:7da65637e512 40 float leftIRBase;
evenbrownie 2:7da65637e512 41 float rightIRBase;
evenbrownie 2:7da65637e512 42 float leftFrontIRBase;
evenbrownie 2:7da65637e512 43 float rightFrontIRBase;
evenbrownie 7:7cdb0381e1b8 44 float p =0.2f; //.32
evenbrownie 7:7cdb0381e1b8 45 float i =0.0001f;
evenbrownie 7:7cdb0381e1b8 46 float d=0.15f;
evenbrownie 7:7cdb0381e1b8 47 float frontP = 1.0;
evenbrownie 2:7da65637e512 48 LeftIR = 1;
evenbrownie 2:7da65637e512 49 RightIR = 1;
evenbrownie 2:7da65637e512 50 leftIRBase = LeftReceiver;
evenbrownie 2:7da65637e512 51 rightIRBase = RightReceiver;
evenbrownie 2:7da65637e512 52 LeftIR = 0;
evenbrownie 2:7da65637e512 53 RightIR = 0;
evenbrownie 2:7da65637e512 54 wait_ms(10);
evenbrownie 2:7da65637e512 55 FrontLeftIR = 1;
evenbrownie 2:7da65637e512 56 leftFrontIRBase = FrontLeftReceiver;
evenbrownie 2:7da65637e512 57 FrontRightIR = 1;
evenbrownie 2:7da65637e512 58 rightFrontIRBase= FrontRightReceiver;
evenbrownie 2:7da65637e512 59 FrontLeftIR = 0;
evenbrownie 2:7da65637e512 60 FrontRightIR = 0;
evenbrownie 2:7da65637e512 61 wait_ms(10);
evenbrownie 2:7da65637e512 62
Showboo 0:f7fc09f9c7ce 63 pid lman, rman;
Showboo 1:fb18b43590e6 64 lman.kp = .009f;
Showboo 0:f7fc09f9c7ce 65 lman.ki = .0f;
Showboo 0:f7fc09f9c7ce 66 lman.kd = .0f;
Showboo 0:f7fc09f9c7ce 67 rman.kp = .5f;
Showboo 0:f7fc09f9c7ce 68 rman.ki = .1f;
Showboo 0:f7fc09f9c7ce 69 rman.kd = .4f;
Showboo 0:f7fc09f9c7ce 70
Showboo 0:f7fc09f9c7ce 71 lpwmf.period(0.01f);
Showboo 1:fb18b43590e6 72 lpwmb.period(0.01f);
Showboo 0:f7fc09f9c7ce 73 lpwmf = 0; //Previously started on, replace with lpwmf = lbase to make it start on (not a good idea)
Showboo 1:fb18b43590e6 74 rpwmb=0;
Showboo 0:f7fc09f9c7ce 75 rpwmf.period(0.01f);
Showboo 1:fb18b43590e6 76 rpwmb.period(0.01f);
Showboo 1:fb18b43590e6 77 rpwmb=0;
Showboo 0:f7fc09f9c7ce 78 rpwmf = 0;
Showboo 1:fb18b43590e6 79
Showboo 0:f7fc09f9c7ce 80 pid ir_lman, ir_rman;
Showboo 1:fb18b43590e6 81 ir_lman.kp = .0001f;
Showboo 0:f7fc09f9c7ce 82 ir_lman.ki = .0f;
Showboo 0:f7fc09f9c7ce 83 ir_lman.kd = .0f;
Showboo 0:f7fc09f9c7ce 84 ir_rman.kp = .5f;
Showboo 0:f7fc09f9c7ce 85 ir_rman.ki = .1f;
Showboo 0:f7fc09f9c7ce 86 ir_rman.kd = .4f;
Showboo 0:f7fc09f9c7ce 87 t_time.start();
Showboo 0:f7fc09f9c7ce 88 while(1){
Showboo 0:f7fc09f9c7ce 89 float dt = t_time.read();
Showboo 1:fb18b43590e6 90 float lspeed = 0; float rspeed = 0;
evenbrownie 7:7cdb0381e1b8 91 prevRightFrontValue = rightFrontValue;
evenbrownie 7:7cdb0381e1b8 92 prevLeftFrontValue = leftFrontValue;
Showboo 1:fb18b43590e6 93 LeftIR = 1;
evenbrownie 2:7da65637e512 94 RightIR = 1;
evenbrownie 2:7da65637e512 95 wait(.01);
evenbrownie 2:7da65637e512 96 leftValue = LeftReceiver;
evenbrownie 2:7da65637e512 97 rightValue = RightReceiver;
evenbrownie 2:7da65637e512 98 LeftIR = 0;
evenbrownie 2:7da65637e512 99 RightIR = 0;
evenbrownie 2:7da65637e512 100 FrontLeftIR = 1;
evenbrownie 2:7da65637e512 101 leftFrontValue = FrontLeftReceiver;
evenbrownie 2:7da65637e512 102 FrontRightIR = 1;
evenbrownie 2:7da65637e512 103 rightFrontValue = FrontRightReceiver;
evenbrownie 2:7da65637e512 104 wait(.01);
evenbrownie 2:7da65637e512 105 FrontLeftIR = 0;
evenbrownie 2:7da65637e512 106 FrontRightIR = 0;
evenbrownie 3:4e34437daef4 107 // pc.printf( "%f \n" , leftValue);
evenbrownie 3:4e34437daef4 108 //pc.printf("%f \n", rightValue);
evenbrownie 7:7cdb0381e1b8 109 if(leftValue> 0.34f && leftFrontValue > 0.15f)
evenbrownie 3:4e34437daef4 110 {
evenbrownie 3:4e34437daef4 111 turn_right();
evenbrownie 3:4e34437daef4 112 }
evenbrownie 6:ce498700a28c 113 else if(rightFrontValue > 0.15f && rightValue > 0.35f)
evenbrownie 3:4e34437daef4 114 {
evenbrownie 3:4e34437daef4 115 turn_left();
evenbrownie 3:4e34437daef4 116 }
evenbrownie 3:4e34437daef4 117
evenbrownie 2:7da65637e512 118
evenbrownie 2:7da65637e512 119
evenbrownie 2:7da65637e512 120 //ProcessIR(dt, ir_lman, lspeed, rspeed);
evenbrownie 2:7da65637e512 121 leftError = leftIRBase - leftValue;
evenbrownie 2:7da65637e512 122 rightError = rightIRBase - rightValue;
evenbrownie 3:4e34437daef4 123 totalRightError += rightError;
evenbrownie 3:4e34437daef4 124 totalLeftError += leftError;
evenbrownie 2:7da65637e512 125 leftFrontError = leftFrontIRBase - leftFrontValue;
evenbrownie 2:7da65637e512 126 rightFrontError = rightFrontIRBase - rightFrontValue;
evenbrownie 7:7cdb0381e1b8 127 changeLeftFrontValue = leftFrontValue-prevLeftFrontValue;
evenbrownie 7:7cdb0381e1b8 128 changeRightFrontValue = rightFrontValue - prevRightFrontValue;
evenbrownie 7:7cdb0381e1b8 129
evenbrownie 7:7cdb0381e1b8 130 if(changeLeftFrontValue <0){
evenbrownie 7:7cdb0381e1b8 131 changeLeftFrontValue = 0;
evenbrownie 7:7cdb0381e1b8 132 }
evenbrownie 7:7cdb0381e1b8 133
evenbrownie 7:7cdb0381e1b8 134 if(changeRightFrontValue <0){
evenbrownie 7:7cdb0381e1b8 135 changeRightFrontValue = 0;
evenbrownie 7:7cdb0381e1b8 136 }
evenbrownie 7:7cdb0381e1b8 137 totalFrontChange = changeRightFrontValue + changeLeftFrontValue;
evenbrownie 7:7cdb0381e1b8 138
evenbrownie 2:7da65637e512 139
evenbrownie 4:c545d63ee454 140 adjust_l = p*leftError-d*prevleftError-i*totalLeftError ;
evenbrownie 4:c545d63ee454 141 adjust_r = p*rightError-d*prevrightError-i*totalRightError ;
evenbrownie 3:4e34437daef4 142
evenbrownie 3:4e34437daef4 143 prevleftError = leftError;
evenbrownie 3:4e34437daef4 144 prevrightError = rightError;
evenbrownie 7:7cdb0381e1b8 145 // if(adjust_l >0)
evenbrownie 3:4e34437daef4 146 {
evenbrownie 7:7cdb0381e1b8 147 // adjust_l=0;
evenbrownie 3:4e34437daef4 148 }
evenbrownie 7:7cdb0381e1b8 149 // if(adjust_r >0)
evenbrownie 3:4e34437daef4 150 {
evenbrownie 7:7cdb0381e1b8 151 // adjust_r=0;
evenbrownie 3:4e34437daef4 152 }
evenbrownie 7:7cdb0381e1b8 153 lspeed = constrain(0.05f, 1.0f, lbase - adjust_l - frontP*totalFrontChange);
evenbrownie 7:7cdb0381e1b8 154 rspeed = constrain(0.05f, 1.0f, rbase-adjust_r - frontP*totalFrontChange);
Showboo 0:f7fc09f9c7ce 155 lpwmb = 0; rpwmb = 0;
evenbrownie 3:4e34437daef4 156 lpwmf = lspeed; rpwmf = rspeed;
evenbrownie 3:4e34437daef4 157 //pc.printf("left %f \n", leftValue);
evenbrownie 3:4e34437daef4 158 //pc.printf("right %f \n\n", rightValue);
evenbrownie 3:4e34437daef4 159 //pc.printf("front left%f \n", leftFrontValue);
evenbrownie 3:4e34437daef4 160 //pc.printf("front right%f \n", rightFrontValue);
evenbrownie 3:4e34437daef4 161 //pc.printf(" \n");
Showboo 0:f7fc09f9c7ce 162 t_time.reset();
Showboo 0:f7fc09f9c7ce 163 }
Showboo 0:f7fc09f9c7ce 164 }
evenbrownie 3:4e34437daef4 165
evenbrownie 3:4e34437daef4 166
evenbrownie 3:4e34437daef4 167