Anon Anon / Mbed 2 deprecated MicroMousewithFloodFill

Updated with the Algorithm

Dependencies:   QEI mbed

Fork of MM_rat_Assignment4-newwest by Evan Brown

main.cpp@5:bfabc00a73e8, 2017-11-27 (annotated)

Committer:
evenbrownie
Date:
Mon Nov 27 17:45:06 2017 +0000
Revision:
5:bfabc00a73e8
Parent:
4:c545d63ee454
Child:
6:ce498700a28c
PID latest adjustments

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 2:7da65637e512 28 float leftFrontError = 0;
evenbrownie 2:7da65637e512 29 float rightFrontError = 0;
evenbrownie 3:4e34437daef4 30 float totalLeftError = 0;
evenbrownie 3:4e34437daef4 31 float totalRightError =0;
evenbrownie 2:7da65637e512 32 float adjust_l = 0;
evenbrownie 2:7da65637e512 33 float adjust_r = 0;
evenbrownie 2:7da65637e512 34
evenbrownie 2:7da65637e512 35 float leftIRBase;
evenbrownie 2:7da65637e512 36 float rightIRBase;
evenbrownie 2:7da65637e512 37 float leftFrontIRBase;
evenbrownie 2:7da65637e512 38 float rightFrontIRBase;
evenbrownie 3:4e34437daef4 39 float p =0.5f;
evenbrownie 5:bfabc00a73e8 40 float i =0.01f;
evenbrownie 3:4e34437daef4 41 float d=0.1f;
evenbrownie 2:7da65637e512 42 LeftIR = 1;
evenbrownie 2:7da65637e512 43 RightIR = 1;
evenbrownie 2:7da65637e512 44 leftIRBase = LeftReceiver;
evenbrownie 2:7da65637e512 45 rightIRBase = RightReceiver;
evenbrownie 2:7da65637e512 46 LeftIR = 0;
evenbrownie 2:7da65637e512 47 RightIR = 0;
evenbrownie 2:7da65637e512 48 wait_ms(10);
evenbrownie 2:7da65637e512 49 FrontLeftIR = 1;
evenbrownie 2:7da65637e512 50 leftFrontIRBase = FrontLeftReceiver;
evenbrownie 2:7da65637e512 51 FrontRightIR = 1;
evenbrownie 2:7da65637e512 52 rightFrontIRBase= FrontRightReceiver;
evenbrownie 2:7da65637e512 53 FrontLeftIR = 0;
evenbrownie 2:7da65637e512 54 FrontRightIR = 0;
evenbrownie 2:7da65637e512 55 wait_ms(10);
evenbrownie 2:7da65637e512 56
Showboo 0:f7fc09f9c7ce 57 pid lman, rman;
Showboo 1:fb18b43590e6 58 lman.kp = .009f;
Showboo 0:f7fc09f9c7ce 59 lman.ki = .0f;
Showboo 0:f7fc09f9c7ce 60 lman.kd = .0f;
Showboo 0:f7fc09f9c7ce 61 rman.kp = .5f;
Showboo 0:f7fc09f9c7ce 62 rman.ki = .1f;
Showboo 0:f7fc09f9c7ce 63 rman.kd = .4f;
Showboo 0:f7fc09f9c7ce 64
Showboo 0:f7fc09f9c7ce 65 lpwmf.period(0.01f);
Showboo 1:fb18b43590e6 66 lpwmb.period(0.01f);
Showboo 0:f7fc09f9c7ce 67 lpwmf = 0; //Previously started on, replace with lpwmf = lbase to make it start on (not a good idea)
Showboo 1:fb18b43590e6 68 rpwmb=0;
Showboo 0:f7fc09f9c7ce 69 rpwmf.period(0.01f);
Showboo 1:fb18b43590e6 70 rpwmb.period(0.01f);
Showboo 1:fb18b43590e6 71 rpwmb=0;
Showboo 0:f7fc09f9c7ce 72 rpwmf = 0;
Showboo 1:fb18b43590e6 73
Showboo 0:f7fc09f9c7ce 74 pid ir_lman, ir_rman;
Showboo 1:fb18b43590e6 75 ir_lman.kp = .0001f;
Showboo 0:f7fc09f9c7ce 76 ir_lman.ki = .0f;
Showboo 0:f7fc09f9c7ce 77 ir_lman.kd = .0f;
Showboo 0:f7fc09f9c7ce 78 ir_rman.kp = .5f;
Showboo 0:f7fc09f9c7ce 79 ir_rman.ki = .1f;
Showboo 0:f7fc09f9c7ce 80 ir_rman.kd = .4f;
Showboo 0:f7fc09f9c7ce 81 t_time.start();
Showboo 0:f7fc09f9c7ce 82 while(1){
Showboo 0:f7fc09f9c7ce 83 float dt = t_time.read();
Showboo 1:fb18b43590e6 84 float lspeed = 0; float rspeed = 0;
Showboo 1:fb18b43590e6 85 LeftIR = 1;
evenbrownie 2:7da65637e512 86 RightIR = 1;
evenbrownie 2:7da65637e512 87 wait(.01);
evenbrownie 2:7da65637e512 88 leftValue = LeftReceiver;
evenbrownie 2:7da65637e512 89 rightValue = RightReceiver;
evenbrownie 2:7da65637e512 90 LeftIR = 0;
evenbrownie 2:7da65637e512 91 RightIR = 0;
evenbrownie 2:7da65637e512 92 FrontLeftIR = 1;
evenbrownie 2:7da65637e512 93 leftFrontValue = FrontLeftReceiver;
evenbrownie 2:7da65637e512 94 FrontRightIR = 1;
evenbrownie 2:7da65637e512 95 rightFrontValue = FrontRightReceiver;
evenbrownie 2:7da65637e512 96 wait(.01);
evenbrownie 2:7da65637e512 97 FrontLeftIR = 0;
evenbrownie 2:7da65637e512 98 FrontRightIR = 0;
evenbrownie 3:4e34437daef4 99 // pc.printf( "%f \n" , leftValue);
evenbrownie 3:4e34437daef4 100 //pc.printf("%f \n", rightValue);
evenbrownie 4:c545d63ee454 101 if(leftValue> 0.38f && leftFrontValue > 0.22f)
evenbrownie 3:4e34437daef4 102 {
evenbrownie 3:4e34437daef4 103 turn_right();
evenbrownie 3:4e34437daef4 104 }
evenbrownie 4:c545d63ee454 105 else if(rightFrontValue > 0.21f && rightValue > 0.4f)
evenbrownie 3:4e34437daef4 106 {
evenbrownie 3:4e34437daef4 107 turn_left();
evenbrownie 3:4e34437daef4 108 }
evenbrownie 3:4e34437daef4 109
evenbrownie 2:7da65637e512 110
evenbrownie 2:7da65637e512 111
evenbrownie 2:7da65637e512 112 //ProcessIR(dt, ir_lman, lspeed, rspeed);
evenbrownie 2:7da65637e512 113 leftError = leftIRBase - leftValue;
evenbrownie 2:7da65637e512 114 rightError = rightIRBase - rightValue;
evenbrownie 3:4e34437daef4 115 totalRightError += rightError;
evenbrownie 3:4e34437daef4 116 totalLeftError += leftError;
evenbrownie 2:7da65637e512 117 leftFrontError = leftFrontIRBase - leftFrontValue;
evenbrownie 2:7da65637e512 118 rightFrontError = rightFrontIRBase - rightFrontValue;
evenbrownie 2:7da65637e512 119
evenbrownie 4:c545d63ee454 120 adjust_l = p*leftError-d*prevleftError-i*totalLeftError ;
evenbrownie 4:c545d63ee454 121 adjust_r = p*rightError-d*prevrightError-i*totalRightError ;
evenbrownie 3:4e34437daef4 122
evenbrownie 3:4e34437daef4 123 prevleftError = leftError;
evenbrownie 3:4e34437daef4 124 prevrightError = rightError;
evenbrownie 3:4e34437daef4 125 if(adjust_l >0)
evenbrownie 3:4e34437daef4 126 {
evenbrownie 3:4e34437daef4 127 adjust_l=0;
evenbrownie 3:4e34437daef4 128 }
evenbrownie 3:4e34437daef4 129 if(adjust_r >0)
evenbrownie 3:4e34437daef4 130 {
evenbrownie 3:4e34437daef4 131 adjust_r=0;
evenbrownie 3:4e34437daef4 132 }
evenbrownie 2:7da65637e512 133 lspeed = constrain(0.05f, 1.0f, lbase - adjust_l);
evenbrownie 2:7da65637e512 134 rspeed = constrain(0.05f, 1.0f, rbase-adjust_r);
Showboo 0:f7fc09f9c7ce 135 lpwmb = 0; rpwmb = 0;
evenbrownie 3:4e34437daef4 136 lpwmf = lspeed; rpwmf = rspeed;
evenbrownie 3:4e34437daef4 137 //pc.printf("left %f \n", leftValue);
evenbrownie 3:4e34437daef4 138 //pc.printf("right %f \n\n", rightValue);
evenbrownie 3:4e34437daef4 139 //pc.printf("front left%f \n", leftFrontValue);
evenbrownie 3:4e34437daef4 140 //pc.printf("front right%f \n", rightFrontValue);
evenbrownie 3:4e34437daef4 141 //pc.printf(" \n");
Showboo 0:f7fc09f9c7ce 142 t_time.reset();
Showboo 0:f7fc09f9c7ce 143 }
Showboo 0:f7fc09f9c7ce 144 }
evenbrownie 3:4e34437daef4 145
evenbrownie 3:4e34437daef4 146
evenbrownie 3:4e34437daef4 147