Evan Brown
/
MM_rat_Assignment4-newwest
p
Fork of MM_rat_Assignment4 by
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?
User | Revision | Line number | New 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 | } |