Anon Anon
/
MicroMousewithFloodFill
Updated with the Algorithm
Fork of MM_rat_Assignment4-newwest by
main.cpp
- Committer:
- Showboo
- Date:
- 2017-12-08
- Revision:
- 9:97941581fe81
- Parent:
- 8:22e399fe87a4
File content as of revision 9:97941581fe81:
#include "mbed.h" #include "QEI.h" #include "header.h" #include "algorithm.h" /* int main() { using namespace std; bool pushed = true; PushButton.mode(PullDown); t_time.start(); initmotors_and_pid(); //Sets the PWM frequency while(pushed){ if(PushButton.read() == 0){ //If PushButton is pressed down pushed = false; } wait(0.1); } while(!pushed){ RightEncoder.reset(); LeftEncoder.reset(); //usePIDEncoder(); //algorithm(); RightEncoder.reset(); LeftEncoder.reset(); } } */ int main(){ using namespace std; bool pushed = true; PushButton.mode(PullDown); t_time.start(); initmotors_and_pid(); //Sets the PWM frequency while(pushed){ if(PushButton.read() == 0){ //If PushButton is pressed down pushed = false; } wait(0.1); } while(!pushed){ using namespace constants; float dt = t_time.read(); float lspeed = 0; float rspeed = 0; prevRightFrontValue = rightFrontValue; //update previous values for change in IRfront prevLeftFrontValue = leftFrontValue; //IR SAMPLING 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); //TURNING CONDITIONS if(leftValue> 0.34f && leftFrontValue > 0.17f) //.34L .15LF Wall on left and in front { brake(); wait(0.2); turn_right(1); // pc.printf("I turned right\n"); } else if(rightValue> 0.55f && rightFrontValue > 0.14f) //.35R .15RF Wall on right and in front { brake(); wait(0.2); turn_left(1); //pc.printf("I turned left\n"); } else if((leftValue < 0.34 && leftFrontValue < 0.0195f)) // No wall on left and in front { LeftEncoder.reset(); RightEncoder.reset(); //brake(); //wait(0.4); usePIDEncoder(); //pc.printf("Use encoder PID for left side\n"); } else if((rightValue < 0.34f && rightFrontValue < 0.0205f)) // No wall on right and in front { LeftEncoder.reset(); RightEncoder.reset(); //brake(); //wait(0.4); usePIDEncoder(); //pc.printf("Use encoder PID for right side\n"); } else if(leftValue < 0.35f && leftFrontValue < 0.167f && rightValue < 0.35f && rightFrontValue < 0.167f) { LeftEncoder.reset(); RightEncoder.reset(); //brake(); //wait(0.4); usePIDEncoder(); //pc.printf("Use encoder PID for left and right sides"); } //ProcessIR(dt, ir_lman, lspeed, rspeed); leftError = leftIRBase - leftValue; rightError = rightIRBase - rightValue; totalRightError += rightError; totalLeftError += leftError; leftFrontError = leftFrontIRBase - leftFrontValue; rightFrontError = rightFrontIRBase - rightFrontValue; changeLeftFrontValue = leftFrontValue-prevLeftFrontValue; changeRightFrontValue = rightFrontValue - prevRightFrontValue; if(changeLeftFrontValue <0){ changeLeftFrontValue = 0; } if(changeRightFrontValue <0){ changeRightFrontValue = 0; } totalFrontChange = changeRightFrontValue + changeLeftFrontValue; 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 - frontP*totalFrontChange); rspeed = constrain(0.05f, 1.0f, rbase-adjust_r - frontP*totalFrontChange); 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\n", rightFrontValue); //pc.printf(" \n"); t_time.reset(); } }