Mouse code for the MacroRat
Diff: main.cpp
- Revision:
- 9:1d8e4da058cd
- Parent:
- 8:a0760acdc59e
- Child:
- 10:810d1849da9d
diff -r a0760acdc59e -r 1d8e4da058cd main.cpp --- a/main.cpp Fri May 05 01:21:33 2017 +0000 +++ b/main.cpp Sat May 06 01:31:44 2017 +0000 @@ -9,12 +9,75 @@ #include "QEI.h" // PID -#define P_CONSTANT 0.0025 -#define I_CONSTANT 0.0000025 -#define D_CONSTANT 0.25 +#define P_CONSTANT 0 +#define I_CONSTANT 0 +#define D_CONSTANT 0 + +#define IP_CONSTANT 6 +#define II_CONSTANT 0 +#define ID_CONSTANT 1 #include "QEI.h" #define PULSES 880 +#define SAMPLE_NUM 100 + + +void moveForwardUntilWallIr() { + double currentError = 0; + double previousError = 0; + double derivError = 0; + double sumError = 0; + + double HIGH_PWM_VOLTAGE = 0.2; + + double rightSpeed = 0.2; + double leftSpeed = 0.2; + + float ir2 = IRP_2.getSamples( SAMPLE_NUM ); + float ir3 = IRP_3.getSamples( SAMPLE_NUM ); + + while ((IRP_1.getSamples( SAMPLE_NUM ) + IRP_4.getSamples( SAMPLE_NUM ) )/2 < 0.05f) { + currentError = ( (IRP_2.getSamples( SAMPLE_NUM ) - IRP_2.sensorAvg) ) - ( (IRP_3.getSamples( SAMPLE_NUM ) - IRP_3.sensorAvg) ) ; + 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 ); + + } + //sensor1Turn = IR_Sensor1.read(); + //sensor4Turn = IR_Sensor4.read(); + + //backward(); + wait_ms(40); + //brake(); + + left_motor.brake(); + right_motor.brake(); + while( 1 ) + { + + } + + +} int main() { @@ -35,8 +98,8 @@ greenLed.write(0); blueLed.write(1); - //rightWheelForward(0.1); - //leftWheelForward(0.1); + //left_motor.forward(0.1); + //right_motor.forward(0.1); // PA_1 is A of right // PA_0 is B of right @@ -46,11 +109,15 @@ QEI encoder1( PA_1, PA_0, NC, PULSES, QEI::X4_ENCODING ); while (1) { - + moveForwardUntilWallIr(); wait(0.1); //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()); - + //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); // redLed.write(0);