Evan Brown
/
MM_rat_Assignment4-newwest
p
Fork of MM_rat_Assignment4 by
main.cpp
- Committer:
- evenbrownie
- Date:
- 2017-11-27
- Revision:
- 5:bfabc00a73e8
- Parent:
- 4:c545d63ee454
- Child:
- 6:ce498700a28c
File content as of revision 5:bfabc00a73e8:
#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 prevrightError = 0; float prevleftError = 0; float leftFrontError = 0; float rightFrontError = 0; float totalLeftError = 0; float totalRightError =0; float adjust_l = 0; float adjust_r = 0; float leftIRBase; float rightIRBase; float leftFrontIRBase; float rightFrontIRBase; float p =0.5f; float i =0.01f; float d=0.1f; 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; // pc.printf( "%f \n" , leftValue); //pc.printf("%f \n", rightValue); if(leftValue> 0.38f && leftFrontValue > 0.22f) { turn_right(); } else if(rightFrontValue > 0.21f && rightValue > 0.4f) { turn_left(); } //ProcessIR(dt, ir_lman, lspeed, rspeed); leftError = leftIRBase - leftValue; rightError = rightIRBase - rightValue; totalRightError += rightError; totalLeftError += leftError; leftFrontError = leftFrontIRBase - leftFrontValue; rightFrontError = rightFrontIRBase - rightFrontValue; adjust_l = p*leftError-d*prevleftError-i*totalLeftError ; adjust_r = p*rightError-d*prevrightError-i*totalRightError ; prevleftError = leftError; prevrightError = rightError; if(adjust_l >0) { adjust_l=0; } if(adjust_r >0) { adjust_r=0; } 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; //pc.printf("left %f \n", leftValue); //pc.printf("right %f \n\n", rightValue); //pc.printf("front left%f \n", leftFrontValue); //pc.printf("front right%f \n", rightFrontValue); //pc.printf(" \n"); t_time.reset(); } }