Mouse code for the MacroRat
Diff: main.cpp
- Revision:
- 11:8fc2b703086b
- Parent:
- 10:810d1849da9d
- Child:
- 12:5790e56a056f
diff -r 810d1849da9d -r 8fc2b703086b main.cpp --- a/main.cpp Sat May 06 22:49:31 2017 +0000 +++ b/main.cpp Sun May 07 01:13:42 2017 +0000 @@ -9,19 +9,17 @@ #include "stm32f4xx.h" #include "QEI.h" -// PID -#define P_CONSTANT 0 -#define I_CONSTANT 0 -#define D_CONSTANT 0 +/* Constants for when HIGH_PWM_VOLTAGE = 0.2 #define IP_CONSTANT 6 #define II_CONSTANT 0 #define ID_CONSTANT 1 +*/ -#include "QEI.h" -#define PULSES 880 -#define SAMPLE_NUM 100 - +// Constants for when HIGH_PWM_VOLTAGE = 0.1 +#define IP_CONSTANT 15 +#define II_CONSTANT 0 +#define ID_CONSTANT 0 void moveForwardUntilWallIr() { double currentError = 0; @@ -29,15 +27,24 @@ double derivError = 0; double sumError = 0; - double HIGH_PWM_VOLTAGE = 0.2; + double HIGH_PWM_VOLTAGE = 0.1; - double rightSpeed = 0.2; - double leftSpeed = 0.2; + double rightSpeed = 0.1; + double leftSpeed = 0.1; 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) { + int count = encoder0.getPulses(); + while ((IRP_1.getSamples( SAMPLE_NUM ) + IRP_4.getSamples( SAMPLE_NUM ) )/2 < 0.05f) { + int pulseCount = (encoder0.getPulses()-count) % 5600; + if (pulseCount > 5400 && pulseCount < 5800) { + blueLed.write(0); + } else { + blueLed.write(1); + } + + 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; @@ -78,29 +85,41 @@ } } +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(); } int main() @@ -119,7 +138,7 @@ // IR_4.write(1); redLed.write(1); - greenLed.write(0); + greenLed.write(1); blueLed.write(1); //left_motor.forward(0.1); @@ -129,8 +148,8 @@ // 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 ); + //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) @@ -150,10 +169,11 @@ 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",