Anon Anon / Mbed 2 deprecated MicroMousewithFloodFill

Updated with the Algorithm

Dependencies:   QEI mbed

Fork of MM_rat_Assignment4-newwest by Evan Brown

main.cpp@2:7da65637e512, 2017-11-22 (annotated)

Committer:
evenbrownie
Date:
Wed Nov 22 01:40:27 2017 +0000
Revision:
2:7da65637e512
Parent:
1:fb18b43590e6
Child:
3:4e34437daef4
P;

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 2:7da65637e512 26 float leftFrontError = 0;
evenbrownie 2:7da65637e512 27 float rightFrontError = 0;
evenbrownie 2:7da65637e512 28 float adjust_l = 0;
evenbrownie 2:7da65637e512 29 float adjust_r = 0;
evenbrownie 2:7da65637e512 30
evenbrownie 2:7da65637e512 31 float leftIRBase;
evenbrownie 2:7da65637e512 32 float rightIRBase;
evenbrownie 2:7da65637e512 33 float leftFrontIRBase;
evenbrownie 2:7da65637e512 34 float rightFrontIRBase;
evenbrownie 2:7da65637e512 35 float p =0.45f;
evenbrownie 2:7da65637e512 36 float i;
evenbrownie 2:7da65637e512 37 float d;
evenbrownie 2:7da65637e512 38 LeftIR = 1;
evenbrownie 2:7da65637e512 39 RightIR = 1;
evenbrownie 2:7da65637e512 40 leftIRBase = LeftReceiver;
evenbrownie 2:7da65637e512 41 rightIRBase = RightReceiver;
evenbrownie 2:7da65637e512 42 LeftIR = 0;
evenbrownie 2:7da65637e512 43 RightIR = 0;
evenbrownie 2:7da65637e512 44 wait_ms(10);
evenbrownie 2:7da65637e512 45 FrontLeftIR = 1;
evenbrownie 2:7da65637e512 46 leftFrontIRBase = FrontLeftReceiver;
evenbrownie 2:7da65637e512 47 FrontRightIR = 1;
evenbrownie 2:7da65637e512 48 rightFrontIRBase= FrontRightReceiver;
evenbrownie 2:7da65637e512 49 FrontLeftIR = 0;
evenbrownie 2:7da65637e512 50 FrontRightIR = 0;
evenbrownie 2:7da65637e512 51 wait_ms(10);
evenbrownie 2:7da65637e512 52
Showboo 0:f7fc09f9c7ce 53 pid lman, rman;
Showboo 1:fb18b43590e6 54 lman.kp = .009f;
Showboo 0:f7fc09f9c7ce 55 lman.ki = .0f;
Showboo 0:f7fc09f9c7ce 56 lman.kd = .0f;
Showboo 0:f7fc09f9c7ce 57 rman.kp = .5f;
Showboo 0:f7fc09f9c7ce 58 rman.ki = .1f;
Showboo 0:f7fc09f9c7ce 59 rman.kd = .4f;
Showboo 0:f7fc09f9c7ce 60
Showboo 0:f7fc09f9c7ce 61 lpwmf.period(0.01f);
Showboo 1:fb18b43590e6 62 lpwmb.period(0.01f);
Showboo 0:f7fc09f9c7ce 63 lpwmf = 0; //Previously started on, replace with lpwmf = lbase to make it start on (not a good idea)
Showboo 1:fb18b43590e6 64 rpwmb=0;
Showboo 0:f7fc09f9c7ce 65 rpwmf.period(0.01f);
Showboo 1:fb18b43590e6 66 rpwmb.period(0.01f);
Showboo 1:fb18b43590e6 67 rpwmb=0;
Showboo 0:f7fc09f9c7ce 68 rpwmf = 0;
Showboo 1:fb18b43590e6 69
Showboo 0:f7fc09f9c7ce 70 pid ir_lman, ir_rman;
Showboo 1:fb18b43590e6 71 ir_lman.kp = .0001f;
Showboo 0:f7fc09f9c7ce 72 ir_lman.ki = .0f;
Showboo 0:f7fc09f9c7ce 73 ir_lman.kd = .0f;
Showboo 0:f7fc09f9c7ce 74 ir_rman.kp = .5f;
Showboo 0:f7fc09f9c7ce 75 ir_rman.ki = .1f;
Showboo 0:f7fc09f9c7ce 76 ir_rman.kd = .4f;
Showboo 0:f7fc09f9c7ce 77 t_time.start();
Showboo 0:f7fc09f9c7ce 78 while(1){
Showboo 0:f7fc09f9c7ce 79 float dt = t_time.read();
Showboo 1:fb18b43590e6 80 float lspeed = 0; float rspeed = 0;
Showboo 1:fb18b43590e6 81 LeftIR = 1;
evenbrownie 2:7da65637e512 82 RightIR = 1;
evenbrownie 2:7da65637e512 83 wait(.01);
evenbrownie 2:7da65637e512 84 leftValue = LeftReceiver;
evenbrownie 2:7da65637e512 85 rightValue = RightReceiver;
evenbrownie 2:7da65637e512 86 LeftIR = 0;
evenbrownie 2:7da65637e512 87 RightIR = 0;
evenbrownie 2:7da65637e512 88 FrontLeftIR = 1;
evenbrownie 2:7da65637e512 89 leftFrontValue = FrontLeftReceiver;
evenbrownie 2:7da65637e512 90 FrontRightIR = 1;
evenbrownie 2:7da65637e512 91 rightFrontValue = FrontRightReceiver;
evenbrownie 2:7da65637e512 92 wait(.01);
evenbrownie 2:7da65637e512 93 FrontLeftIR = 0;
evenbrownie 2:7da65637e512 94 FrontRightIR = 0;
evenbrownie 2:7da65637e512 95
evenbrownie 2:7da65637e512 96
evenbrownie 2:7da65637e512 97 //ProcessIR(dt, ir_lman, lspeed, rspeed);
evenbrownie 2:7da65637e512 98 leftError = leftIRBase - leftValue;
evenbrownie 2:7da65637e512 99 rightError = rightIRBase - rightValue;
evenbrownie 2:7da65637e512 100 leftFrontError = leftFrontIRBase - leftFrontValue;
evenbrownie 2:7da65637e512 101 rightFrontError = rightFrontIRBase - rightFrontValue;
evenbrownie 2:7da65637e512 102
evenbrownie 2:7da65637e512 103 adjust_l = p*leftError ;
evenbrownie 2:7da65637e512 104 adjust_r = p*rightError ;
evenbrownie 2:7da65637e512 105 lspeed = constrain(0.05f, 1.0f, lbase - adjust_l);
evenbrownie 2:7da65637e512 106 rspeed = constrain(0.05f, 1.0f, rbase-adjust_r);
Showboo 0:f7fc09f9c7ce 107 lpwmb = 0; rpwmb = 0;
Showboo 1:fb18b43590e6 108 lpwmf = lspeed; rpwmf = rspeed;
Showboo 0:f7fc09f9c7ce 109 t_time.reset();
Showboo 0:f7fc09f9c7ce 110 }
Showboo 0:f7fc09f9c7ce 111 }