Evan Brown
/
MM_rat_Assignment4-newwest
p
Fork of MM_rat_Assignment4 by
main.cpp
- Committer:
- evenbrownie
- Date:
- 2017-11-22
- Revision:
- 2:7da65637e512
- Parent:
- 1:fb18b43590e6
- Child:
- 3:4e34437daef4
File content as of revision 2:7da65637e512:
#include "mbed.h" #include "QEI.h" #include "header.h" inline void pulse_ir(int in){ for(int i = 0; i < 4; i++){ LeftIR = in; FrontLeftIR = in; FrontRightIR = in; RightIR = in; } } int main() { float leftValue; float rightValue; float leftFrontValue; float rightFrontValue; float leftError = 0; float rightError = 0; float leftFrontError = 0; float rightFrontError = 0; float adjust_l = 0; float adjust_r = 0; float leftIRBase; float rightIRBase; float leftFrontIRBase; float rightFrontIRBase; float p =0.45f; float i; float d; LeftIR = 1; RightIR = 1; leftIRBase = LeftReceiver; rightIRBase = RightReceiver; LeftIR = 0; RightIR = 0; wait_ms(10); FrontLeftIR = 1; leftFrontIRBase = FrontLeftReceiver; FrontRightIR = 1; rightFrontIRBase= FrontRightReceiver; FrontLeftIR = 0; FrontRightIR = 0; wait_ms(10); pid lman, rman; lman.kp = .009f; lman.ki = .0f; lman.kd = .0f; rman.kp = .5f; rman.ki = .1f; rman.kd = .4f; lpwmf.period(0.01f); lpwmb.period(0.01f); lpwmf = 0; //Previously started on, replace with lpwmf = lbase to make it start on (not a good idea) rpwmb=0; rpwmf.period(0.01f); rpwmb.period(0.01f); rpwmb=0; rpwmf = 0; pid ir_lman, ir_rman; ir_lman.kp = .0001f; ir_lman.ki = .0f; ir_lman.kd = .0f; ir_rman.kp = .5f; ir_rman.ki = .1f; ir_rman.kd = .4f; t_time.start(); while(1){ float dt = t_time.read(); float lspeed = 0; float rspeed = 0; LeftIR = 1; RightIR = 1; wait(.01); leftValue = LeftReceiver; rightValue = RightReceiver; LeftIR = 0; RightIR = 0; FrontLeftIR = 1; leftFrontValue = FrontLeftReceiver; FrontRightIR = 1; rightFrontValue = FrontRightReceiver; wait(.01); FrontLeftIR = 0; FrontRightIR = 0; //ProcessIR(dt, ir_lman, lspeed, rspeed); leftError = leftIRBase - leftValue; rightError = rightIRBase - rightValue; leftFrontError = leftFrontIRBase - leftFrontValue; rightFrontError = rightFrontIRBase - rightFrontValue; adjust_l = p*leftError ; adjust_r = p*rightError ; lspeed = constrain(0.05f, 1.0f, lbase - adjust_l); rspeed = constrain(0.05f, 1.0f, rbase-adjust_r); lpwmb = 0; rpwmb = 0; lpwmf = lspeed; rpwmf = rspeed; t_time.reset(); } }