Mouse code for the MacroRat
main.cpp
- Committer:
- christine222
- Date:
- 2017-05-21
- Revision:
- 24:e7063765d6f0
- Parent:
- 23:690b0ca34ee9
- Child:
- 25:f827a8b7880e
File content as of revision 24:e7063765d6f0:
#include "mbed.h" #include "irpair.h" #include "main.h" #include "motor.h" #include <stdlib.h> #include "ITG3200.h" #include "stm32f4xx.h" #include "QEI.h" /* Constants for when HIGH_PWM_VOLTAGE = 0.2 #define IP_CONSTANT 6 #define II_CONSTANT 0 #define ID_CONSTANT 1 */ // Constants for when HIGH_PWM_VOLTAGE = 0.1 // #define IP_CONSTANT 8.85 // #define II_CONSTANT 0.005 // #define ID_CONSTANT 3.15 #define IP_CONSTANT 13.5 #define II_CONSTANT 0.095 #define ID_CONSTANT 8.85 const int desiredCount180 = 2900; const int desiredCountR = 1500; const int desiredCountL = 1575; const int oneCellCount = 5400; const int oneCellCountMomentum = 4400;//4800; // one cell count is actually approximately 5400, but this value is considering momentum! float receiverOneReading = 0.0; float receiverTwoReading = 0.0; float receiverThreeReading = 0.0; float receiverFourReading = 0.0; float ir1base = 0.0; float ir2base = 0.0; float ir3base = 0.0; float ir4base = 0.0; //IRSAvg= >: 0.143701, 0.128285 //facing wall ir2 and ir3 //0.144562, 0.113971 in maze // normal hall ir2 and ir3 //0.013665, 0.010889 in maze //0.014462, 0.009138 // 0.013888, 0.010530 //no walls ir2 and ir3 //0.003265, 0.002904 in maze9 //0.003162, 0.003123 //0.003795, //0.005297, 0.007064 float noWallR = 0.007; float noWallL = 0.007; void pidOnEncoders(); void turnLeft() { double speed0 = 0.11; double speed1 = -0.13; int counter = 0; int initial0 = encoder0.getPulses(); int initial1 = encoder1.getPulses(); int desiredCount0 = initial0 - desiredCountL; int desiredCount1 = initial1 + desiredCountL; int count0 = initial0; int count1 = initial1; double error0 = count0 - desiredCount0; double error1 = count1 - desiredCount1; while(1) { if(!(abs(error0) < 1) && !(abs(error1) < 1)) { count0 = encoder0.getPulses(); count1 = encoder1.getPulses(); error0 = count0 - desiredCount0; error1 = count1 - desiredCount1; right_motor.move(speed0); left_motor.move(speed1); counter = 0; } else { counter++; right_motor.brake(); left_motor.brake(); } if (counter > 60) { break; } } right_motor.brake(); left_motor.brake(); turnFlag = 0; // zeroing out the flags! currDir -= 1; } void turnRight() { double speed0 = -0.11; double speed1 = 0.13; int counter = 0; int initial0 = encoder0.getPulses(); int initial1 = encoder1.getPulses(); int desiredCount0 = initial0 + desiredCountR; int desiredCount1 = initial1 - desiredCountR; int count0 = initial0; int count1 = initial1; double error0 = count0 - desiredCount0; double error1 = count1 - desiredCount1; while(1) { if(!(abs(error0) < 1) && !(abs(error1) < 1)) { count0 = encoder0.getPulses(); count1 = encoder1.getPulses(); error0 = count0 - desiredCount0; error1 = count1 - desiredCount1; right_motor.move(speed0); left_motor.move(speed1); counter = 0; } else { counter++; right_motor.brake(); left_motor.brake(); } if (counter > 60) { break; } } right_motor.brake(); left_motor.brake(); turnFlag = 0; currDir += 1; } void turnLeft180() { double speed0 = 0.15; double speed1 = -0.15; int counter = 0; int initial0 = encoder0.getPulses(); int initial1 = encoder1.getPulses(); int desiredCount0 = initial0 - desiredCountL*2; int desiredCount1 = initial1 + desiredCountL*2; int count0 = initial0; int count1 = initial1; double error0 = count0 - desiredCount0; double error1 = count1 - desiredCount1; while(1) { if(!(abs(error0) < 1) && !(abs(error1) < 1)) { count0 = encoder0.getPulses(); count1 = encoder1.getPulses(); error0 = count0 - desiredCount0; error1 = count1 - desiredCount1; right_motor.move(speed0); left_motor.move(speed1); counter = 0; } else { counter++; right_motor.brake(); left_motor.brake(); } if (counter > 60) { break; } } right_motor.brake(); left_motor.brake(); currDir -= 2; } void turnRight180() { double speed0 = -0.16; double speed1 = 0.16; int counter = 0; int initial0 = encoder0.getPulses(); int initial1 = encoder1.getPulses(); int desiredCount0 = initial0 + desiredCount180; int desiredCount1 = initial1 - desiredCount180; int count0 = initial0; int count1 = initial1; double error0 = count0 - desiredCount0; double error1 = count1 - desiredCount1; while(1) { if(!(abs(error0) < 1) && !(abs(error1) < 1)) { count0 = encoder0.getPulses(); count1 = encoder1.getPulses(); error0 = count0 - desiredCount0; error1 = count1 - desiredCount1; right_motor.move(speed0); left_motor.move(speed1); counter = 0; } else { counter++; right_motor.brake(); left_motor.brake(); } if (counter > 60) { break; } } right_motor.brake(); left_motor.brake(); currDir += 2; } void moveForwardCellEncoder(double cellNum){ int desiredCount0 = encoder0.getPulses() + oneCellCountMomentum*cellNum; int desiredCount1 = encoder1.getPulses() + oneCellCountMomentum*cellNum; left_motor.forward(0.125); right_motor.forward(0.095); wait_ms(1); while (encoder0.getPulses() <= desiredCount0 && encoder1.getPulses() <= desiredCount1){ receiverTwoReading = IRP_2.getSamples(100); receiverThreeReading = IRP_3.getSamples(100); // serial.printf("Average 2: %f Average 3: %f Sensor 2: %f Sensor 3: %f\n", IRP_2.sensorAvg, IRP_3.sensorAvg, receiverTwoReading, receiverThreeReading); if (receiverThreeReading < ir3base){ // redLed.write(1); // blueLed.write(0); turnFlag |= RIGHT_FLAG; } else if (receiverTwoReading < ir2base){ // redLed.write(0); // blueLed.write(1); turnFlag |= LEFT_FLAG; } pidOnEncoders(); } left_motor.brake(); right_motor.brake(); } void handleTurns(){ if (turnFlag == 0x1){ // moveForwardCellEncoder(0.3); turnLeft(); } else if (turnFlag == 0x2){ // moveForwardCellEncoder(0.3); turnRight(); } else if (turnFlag == 0x3){ // moveForwardCellEncoder(0.3); turnLeft(); turnRight(); } } void pidBrake(){ int count0; int count1; count0 = encoder0.getPulses(); count1 = encoder1.getPulses(); int initial0 = count0; int initial1 = count1; double kp = 0.00011; int error = count0 - count1; int counter = 0; right_motor.move(0.7); left_motor.move(0.7); double speed0 = 0.7; double speed1 = 0.7; while(1) { if(abs(error) < 3){ right_motor.brake(); left_motor.brake(); counter++; }else{ count0 = encoder0.getPulses() - initial0; count1 = encoder1.getPulses() - initial1; error = count0 - count1; speed0 = -error*kp + speed0; speed1 = error*kp + speed1; right_motor.move(speed0); left_motor.move(speed1); counter = 0; } if (counter > 10){ break; } } return; } void moveForwardEncoder(){ int count0; int count1; count0 = encoder0.getPulses(); count1 = encoder1.getPulses(); int initial1 = count1; int initial0 = count0; int diff = count0 - count1; double kp = 0.00015; double kd = 0.00019; int prev = 0; double speed0 = 0.10; double speed1 = 0.12; right_motor.move(speed0); left_motor.move(speed1); while( ((encoder0.getPulses() - initial0) <= (oneCellCountMomentum-200) && (encoder1.getPulses() - initial1) <= (oneCellCountMomentum-200)) || IRP_1.getSamples(50) > IRP_1.sensorAvg*0.8 || IRP_4.getSamples(50) > IRP_4.sensorAvg*0.8){ //while( (IRP_1.getSamples(50) + IRP_4.getSamples(50))/2 < ((IRP_1.sensorAvg+IRP_2.sensorAvg)/2)*0.4 ){ //serial.printf("IRS= >: %f, %f \r\n", IRP_2.getSamples( 100 ), IRP_3.getSamples( 100 )); count0 = encoder0.getPulses() - initial0; count1 = encoder1.getPulses() - initial1; int x = count0 - count1; //double d = kp * x + kd * ( x - prev ); double kppart = kp * x; double kdpart = kd * (x-prev); double d = kppart + kdpart; //serial.printf( "x: %d,\t prev: %d,\t d: %f,\t kppart: %f,\t kdpart: %f\n", x, prev, d, kppart, kdpart ); if( x < diff - 40 ) // count1 is bigger, right wheel pushed forward { left_motor.move( speed1-0.8*d ); right_motor.move( speed0+d ); } else if( x > diff + 40 ) { left_motor.move( speed1-0.8*d ); right_motor.move( speed0+d ); } // else // { // left_motor.brake(); // right_motor.brake(); // } prev = x; } //pidOnEncoders(); //pidBrake(); right_motor.brake(); left_motor.brake(); return; } void moveForwardWallEncoder(){ int count0; int count1; count0 = encoder0.getPulses(); count1 = encoder1.getPulses(); int initial1 = count1; int initial0 = count0; int diff = count0 - count1; double kp = 0.00015; double kd = 0.00019; int prev = 0; double speed0 = 0.07; double speed1 = 0.07; right_motor.move(speed0); left_motor.move(speed1); if( IRP_1.getSamples(50) > IRP_1.sensorAvg*0.8 || IRP_4.getSamples(50) > IRP_4.sensorAvg*0.8){ return; } //while((encoder0.getPulses() - initial0) <= (oneCellCountMomentum-200) && (encoder1.getPulses() - initial1) <= (oneCellCountMomentum-200)) { while( (IRP_1.getSamples(50) + IRP_4.getSamples(50))/2 < ((IRP_1.sensorAvg+IRP_2.sensorAvg)/2)*0.4 ){ //serial.printf("IRS= >: %f, %f \r\n", IRP_2.getSamples( 100 ), IRP_3.getSamples( 100 )); count0 = encoder0.getPulses() - initial0; count1 = encoder1.getPulses() - initial1; int x = count0 - count1; //double d = kp * x + kd * ( x - prev ); double kppart = kp * x; double kdpart = kd * (x-prev); double d = kppart + kdpart; //serial.printf( "x: %d,\t prev: %d,\t d: %f,\t kppart: %f,\t kdpart: %f\n", x, prev, d, kppart, kdpart ); if( x < diff - 40 ) // count1 is bigger, right wheel pushed forward { left_motor.move( speed1-0.8*d ); right_motor.move( speed0+d ); } else if( x > diff + 40 ) { left_motor.move( speed1-0.8*d ); right_motor.move( speed0+d ); } // else // { // left_motor.brake(); // right_motor.brake(); // } prev = x; } //pidOnEncoders(); //pidBrake(); right_motor.brake(); left_motor.brake(); return; } void moveForwardUntilWallIr() { double currentError = 0; double previousError = 0; double derivError = 0; double sumError = 0; double HIGH_PWM_VOLTAGE = 0.1; double rightSpeed = 0.14; double leftSpeed = 0.17; float ir2 = IRP_2.getSamples( SAMPLE_NUM ); float ir3 = IRP_3.getSamples( SAMPLE_NUM ); int count = encoder0.getPulses(); while ((IRP_1.getSamples( SAMPLE_NUM ) + IRP_4.getSamples( SAMPLE_NUM ) )/2 < 0.05f) { // while the front facing IR's arent covered if((IRP_2.getSamples(SAMPLE_NUM) < 0.005 || IRP_3.getSamples(SAMPLE_NUM) < 0.005)) { //moveForwardWallEncoder(); }else if(IRP_2.getSamples(SAMPLE_NUM) < 0.005){ // left wall gone //moveForwardRightWall(); }else if(IRP_3.getSamples(SAMPLE_NUM) < 0.005){ // right wall gone //moveForwardLeftWall(); }else{ // will move forward using encoders only // if cell ahead doesn't have a wall on either one side or both sides int pulseCount = (encoder0.getPulses()-count) % 5600; if (pulseCount > 5400 && pulseCount < 5800) { blueLed.write(0); } else { blueLed.write(1); } sumError += currentError; currentError = ( (IRP_2.getSamples( SAMPLE_NUM ) - ir2base) ) - ( (IRP_3.getSamples( SAMPLE_NUM ) - ir3base) ) ; derivError = currentError - previousError; double PIDSum = IP_CONSTANT*currentError + II_CONSTANT*sumError + ID_CONSTANT*derivError; if (PIDSum > 0) { // this means the leftWheel is faster than the right. So right speeds up, left slows down rightSpeed = HIGH_PWM_VOLTAGE - abs(PIDSum*HIGH_PWM_VOLTAGE); leftSpeed = HIGH_PWM_VOLTAGE + abs(PIDSum*HIGH_PWM_VOLTAGE); } else { // r is faster than L. speed up l, slow down r rightSpeed = HIGH_PWM_VOLTAGE + abs(PIDSum*HIGH_PWM_VOLTAGE); leftSpeed = HIGH_PWM_VOLTAGE - abs(PIDSum*HIGH_PWM_VOLTAGE); } if (leftSpeed > 0.30) leftSpeed = 0.30; if (leftSpeed < 0) leftSpeed = 0; if (rightSpeed > 0.30) rightSpeed = 0.30; if (rightSpeed < 0) rightSpeed = 0; right_motor.forward(rightSpeed); left_motor.forward(leftSpeed); previousError = currentError; ir2 = IRP_2.getSamples( SAMPLE_NUM ); ir3 = IRP_3.getSamples( SAMPLE_NUM ); } //backward(); //wait_ms(40); //brake(); left_motor.brake(); right_motor.brake(); } } void printDipFlag() { if (DEBUGGING) serial.printf("Flag value is %d", dipFlags); } void enableButton1() { dipFlags |= BUTTON1_FLAG; printDipFlag(); } void enableButton2() { dipFlags |= BUTTON2_FLAG; printDipFlag(); } void enableButton3() { dipFlags |= BUTTON3_FLAG; printDipFlag(); } void enableButton4() { dipFlags |= BUTTON4_FLAG; printDipFlag(); } void disableButton1() { dipFlags &= ~BUTTON1_FLAG; printDipFlag(); } void disableButton2() { dipFlags &= ~BUTTON2_FLAG; printDipFlag(); } void disableButton3() { dipFlags &= ~BUTTON3_FLAG; printDipFlag(); } void disableButton4() { dipFlags &= ~BUTTON4_FLAG; printDipFlag(); } void pidOnEncoders() { int count0; int count1; count0 = encoder0.getPulses(); count1 = encoder1.getPulses(); int diff = count0 - count1; double kp = 0.00011; double kd = 0.00014; int prev = 0; int counter = 0; while(1) { count0 = encoder0.getPulses(); count1 = encoder1.getPulses(); int x = count0 - count1; //double d = kp * x + kd * ( x - prev ); double kppart = kp * x; double kdpart = kd * (x-prev); double d = kppart + kdpart; //serial.printf( "x: %d,\t prev: %d,\t d: %f,\t kppart: %f,\t kdpart: %f\n", x, prev, d, kppart, kdpart ); if( x < diff - 40 ) // count1 is bigger, right wheel pushed forward { left_motor.move( -d ); right_motor.move( d ); } else if( x > diff + 40 ) { left_motor.move( -d ); right_motor.move( d ); } // else // { // left_motor.brake(); // right_motor.brake(); // } prev = x; counter++; if (counter == 5) break; } } void nCellEncoderAndIR(double cellCount){ double currentError = 0; double previousError = 0; double derivError = 0; double sumError = 0; double HIGH_PWM_VOLTAGE = 0.15; double rightSpeed = 0.095; double leftSpeed = 0.115; int initial0 = encoder0.getPulses(); int initial1 = encoder1.getPulses(); float ir2 = IRP_2.getSamples( SAMPLE_NUM ); float ir3 = IRP_3.getSamples( SAMPLE_NUM ); float ir1 = IRP_1.getSamples(50); float ir4 = IRP_4.getSamples(50); int state = 0; if(IRP_3.sensorAvg > noWallR){ ir3base = IRP_3.sensorAvg; } if(IRP_2.sensorAvg > noWallL){ ir2base = IRP_2.sensorAvg; } for (int i = 0; i < cellCount; i++){ while ( ((encoder0.getPulses()-initial0) <= oneCellCountMomentum && (encoder1.getPulses()-initial1) <= oneCellCountMomentum) && ir1 < IRP_1.sensorAvg*0.7 && ir4 < IRP_4.sensorAvg*0.7 ){ ir2 = IRP_2.getSamples(50); ir3 = IRP_3.getSamples(50); ir1 = IRP_1.getSamples(50); ir4 = IRP_4.getSamples(50); if((ir3 < ir3base/4) && (ir2 < ir2base/4)){ // both sides gone redLed.write(1); greenLed.write(1); blueLed.write(1); wait_ms(100); redLed.write(1); greenLed.write(1); blueLed.write(0); wait_ms(200); redLed.write(1); greenLed.write(1); blueLed.write(0); wait_ms(200); redLed.write(1); greenLed.write(1); blueLed.write(0); wait_ms(200); redLed.write(1); greenLed.write(1); blueLed.write(0); moveForwardEncoder(); }else if (ir3 < ir3base/4){// right wall gone // RED RED RED RED RED currentError = (ir3 - ir2base); redLed.write(0); greenLed.write(1); blueLed.write(1); }else if (ir2 < ir2base){// left wall gone // BLUE BLUE BLUE BLUE currentError = (ir3base - ir2); redLed.write(1); greenLed.write(1); blueLed.write(0); }else{ // both walls there currentError = (ir2 - ir2base) - (ir3 - ir3base); redLed.write(1); greenLed.write(0); blueLed.write(1); } sumError += currentError; derivError = currentError - previousError; double PIDSum = IP_CONSTANT*currentError + II_CONSTANT*sumError + ID_CONSTANT*derivError; if (PIDSum > 0) { // this means the leftWheel is faster than the right. So right speeds up, left slows down rightSpeed = HIGH_PWM_VOLTAGE - abs(PIDSum*HIGH_PWM_VOLTAGE); leftSpeed = HIGH_PWM_VOLTAGE + abs(PIDSum*HIGH_PWM_VOLTAGE); } else { // r is faster than L. speed up l, slow down r rightSpeed = HIGH_PWM_VOLTAGE + abs(PIDSum*HIGH_PWM_VOLTAGE); leftSpeed = HIGH_PWM_VOLTAGE - abs(PIDSum*HIGH_PWM_VOLTAGE); } if (leftSpeed > 0.30) leftSpeed = 0.30; if (leftSpeed < 0) leftSpeed = 0; if (rightSpeed > 0.30) rightSpeed = 0.30; if (rightSpeed < 0) rightSpeed = 0; right_motor.forward(rightSpeed); left_motor.forward(leftSpeed); pidOnEncoders(); previousError = currentError; ir2 = IRP_2.getSamples( SAMPLE_NUM ); ir3 = IRP_3.getSamples( SAMPLE_NUM ); if(IRP_3.sensorAvg > noWallR){ continue; }else if(ir3 > noWallR){ ir3base = ir3; } if(IRP_2.sensorAvg > noWallL){ continue; }else if(ir2 > noWallL){ ir2base = ir2; } } } left_motor.brake(); right_motor.brake(); return; } int main() { //Set highest bandwidth. //gyro.setLpBandwidth(LPFBW_42HZ); serial.baud(9600); //serial.printf("The gyro's address is %s", gyro.getWhoAmI()); wait (0.1); redLed.write(1); greenLed.write(0); blueLed.write(1); //left_motor.forward(0.1); //right_motor.forward(0.1); // PA_1 is A of right // PA_0 is B of right // PA_5 is A of left // PB_3 is B of left //QEI encoder0( PA_5, PB_3, NC, PULSES, QEI::X4_ENCODING ); // QEI encoder1( PA_1, PA_0, NC, PULSES, QEI::X4_ENCODING ); // TODO: Setting all the registers and what not for Quadrature Encoders /* RCC->APB1ENR |= 0x1001; // Enable clock for Tim2 (Bit 0) and Tim5 (Bit 3) RCC->AHB1ENR |= 0x11; // Enable GPIO port clock enables for Tim2(A) and Tim5(B) GPIOA->AFR[0] |= 0x10; // Set GPIO alternate function modes for Tim2 GPIOB->AFR[0] |= 0x100; // Set GPIO alternate function modes for Tim5 */ // set GPIO pins to alternate for the pins corresponding to A/B for eacah encoder, and 2 alternate function registers need to be selected for each type // of alternate function specified // 4 modes sets AHB1ENR // Now TMR: enable clock with timer, APB1ENR // set period, autoreload value, ARR value 2^32-1, CR1 - TMR resets itself, ARPE and EN // // Encoder mode: disable timer before changing timer to encoder // CCMR1/2 1/2 depends on channel 1/2 or 3/4, depends on upper bits, depending which channels you use // CCMR sets sample rate and set the channel to input // CCER, which edge to trigger on, cannot be 11(not allowed for encoder mode), CCER for both channels // SMCR - encoder mode // CR1 reenabales // then read CNT reg of timer dipButton1.rise(&enableButton1); dipButton2.rise(&enableButton2); dipButton3.rise(&enableButton3); dipButton4.rise(&enableButton4); dipButton1.fall(&disableButton1); dipButton2.fall(&disableButton2); dipButton3.fall(&disableButton3); dipButton4.fall(&disableButton4); if(dipFlags == 0x1){ turnRight180(); wait_ms(1000); }else{ turnRight(); IRP_1.calibrateSensor(); IRP_4.calibrateSensor(); wait_ms(300); turnLeft(); wait_ms(300); } //right_motor.forward( 0.2 ); //left_motor.forward( 0.2 ); //turnRight180(); //wait_ms(1500); while (1) { //wait_ms(1500); //turnRight(); //wait_ms(1500); //turnLeft(); nCellEncoderAndIR(1); wait_ms(500); // turnRight(); // wait_ms(500); // nCellEncoderAndIR(1); // wait_ms(500); // turnRight(); // wait_ms(500); // nCellEncoderAndIR(1); // wait_ms(500); // turnLeft(); // wait_ms(500); // nCellEncoderAndIR(2); // wait_ms(500); // turnRight(); // wait_ms(500); // nCellEncoderAndIR(1); // wait_ms(500); // turnRight(); // wait_ms(500); // nCellEncoderAndIR(5); // break; // turnRight180(); // int number = rand() % 4 + 1; // switch(number){ // case(1):{//turn right // turnRight(); // break; // } // case(2):{ // turn left // turnLeft(); // break; // } // case(3):{// keep going // break; // } // case(4):{// turnaround // turnRight180(); // break; // } // default:{// keep going // break; // } // } // float irbase2 = IRP_2.sensorAvg/initAverageL/averageDivL; // float irbase3 = IRP_3.sensorAvg/initAverageR/averageDivR; // float ir3 = IRP_2.getSamples(100)/initAverageL; // float ir2 = IRP_3.getSamples(100)/initAverageR; /* counter2++; counter3++; ir2tot += IRP_2.getSamples(100); ir3tot += IRP_3.getSamples(100); ir2 = ir2tot/counter2; ir3 = ir3tot/counter3; serial.printf("IRS= >: %f, %f \r\n", ir2, ir3); */ //serial.printf("%f, %f \n", IRP_2.sensorAvg/initAverageL/averageDivL, IRP_3.sensorAvg/initAverageR/averageDivR); //serial.printf("IRBASEnowall= >: %f, %f \r\n", irbase2, irbase3); //break; //serial.printf("IRS= >: %f, %f \r\n", IRP_2.getSamples(100), IRP_3.getSamples(100)); //serial.printf("IRSAvg= >: %f, %f \r\n", ir2, ir3); //serial.printf("IRSAvg= >: %f, %f \r\n", IRP_2.sensorAvg, IRP_3.sensorAvg); //////////////////////////////////////////////////////////////// //nCellEncoderAndIR(3); //break; //serial.printf("IRS= >: %f, %f, %f, %f \r\n", IRP_1.getSamples( 100 ), IRP_2.getSamples( 100 ), IRP_3.getSamples( 100 ), IRP_4.getSamples(100)); //serial.printf("IRS= >: %f, %f \r\n", IRP_2.getSamples( 100 ), IRP_3.getSamples( 100 )); //break; // moveForwardCellEncoder(1); // wait(0.5); // handleTurns(); // wait(0.5); // moveForwardCellEncoder(1); // wait(0.5); // handleTurns(); //break; //pidOnEncoders(); // moveForwardUntilWallIr(); //serial.printf("%i, %i, %i\n", gyro.getGyroX(), gyro.getGyroY(), gyro.getGyroZ()); //serial.printf("Pulse Count=> e0:%d, e1:%d \r\n", encoder0.getPulses(),encoder1.getPulses()); // double currentError = ( (IRP_2.getSamples( SAMPLE_NUM ) - IRP_2.sensorAvg) ) - ( (IRP_3.getSamples( SAMPLE_NUM ) - IRP_3.sensorAvg) ) ; //serial.printf("IRS= >: %f, %f, %f, %f, %f \r\n", IRP_1.getSamples( 100 ), IRP_2.getSamples( 100 ), IRP_3.getSamples( 100 ), IRP_4.getSamples(100), currentError ); //reading = Rec_4.read(); // serial.printf("reading: %f\n", reading); } }