Mouse code for the MacroRat
Diff: main.cpp
- Revision:
- 13:2032db00f168
- Parent:
- 12:5790e56a056f
- Child:
- 14:9e7bb03ddccb
diff -r 5790e56a056f -r 2032db00f168 main.cpp --- a/main.cpp Fri May 12 23:25:07 2017 +0000 +++ b/main.cpp Sat May 13 02:40:49 2017 +0000 @@ -17,46 +17,209 @@ */ // Constants for when HIGH_PWM_VOLTAGE = 0.1 -#define IP_CONSTANT 15 +#define IP_CONSTANT 13 #define II_CONSTANT 0 -#define ID_CONSTANT 0 +#define ID_CONSTANT 1. + +const int desiredCountR = 1300; +const int desiredCountL = 1400; + +void turnLeft(){ + double speed0 = 0.15; + double speed1 = 0.15; + double kp = 0.01; + + int counter = 0; + + int initialCount0 = encoder0.getPulses(); + int initialCount1 = encoder1.getPulses(); + + double desiredCount0 = initialCount0 - desiredCountL; + double desiredCount1 = initialCount1 + desiredCountL; + + int count0 = initialCount0; + int count1 = initialCount1; + + 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; + + speed0 = error0 * kp + speed0; + speed1 = error1 * kp + speed1; + + 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(); +} void turnRight() { //e1 should be negative encoder0 is left, encoder1 is right double speed0 = 0.15; double speed1 = 0.15; - double kp = 0.00001; + double kp = 0.01; + + int counter = 0; + + int initialCount0 = encoder0.getPulses(); + int initialCount1 = encoder1.getPulses(); + + double desiredCount0 = initialCount0 + desiredCountR; + double desiredCount1 = initialCount1 - desiredCountR; + + int count0 = initialCount0; + int count1 = initialCount1; - double desiredCount0 = 1500; - double desiredCount1 = -1500; + 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; // moves forward + error1 = count1 - desiredCount1; // moves backwards + + speed0 = error0 * kp + speed0; + speed1 = error1 * kp + speed1; - double error0 = 10.1; - double error1 = 10.1; + right_motor.move(speed0); + left_motor.move(speed1); + counter = 0; + }else{ + counter++; + right_motor.brake(); + left_motor.brake(); + } - //right_motor.move(speed0); - //left_motor.move(speed1); + if (counter > 60){ + break; + } + + // serial.printf("ERROR=> 0:%f, 1:%f, SPEED=> 0:%f, 1:%f\n", error0, error1, speed0, speed1); + // serial.printf("Pulse Count=> e0:%d, e1:%d \r\n", encoder0.getPulses(), encoder1.getPulses()); + } + + right_motor.brake(); + left_motor.brake(); +} + +void turnLeft180(){ + double speed0 = 0.15; + double speed1 = 0.15; + double kp = 0.01; + + int counter = 0; int initialCount0 = encoder0.getPulses(); int initialCount1 = encoder1.getPulses(); - int count0 = encoder0.getPulses(); - int count1 = encoder1.getPulses(); + double desiredCount0 = initialCount0 - 3000; + double desiredCount1 = initialCount1 + 2700; + + int count0 = initialCount0; + int count1 = initialCount1; + + 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; - while(!(abs(error0) < 10) && !(abs(error1) < 10)){ + speed0 = error0 * kp + speed0; + speed1 = error1 * kp + speed1; - count0 = encoder0.getPulses() - initialCount0; - count1 = encoder1.getPulses() - initialCount1; + 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(); +} - error0 = count0 + desiredCount0; // moves forward - error1 = count1 + desiredCount1; // moves backwards +void turnRight180(){ + double speed0 = 0.15; + double speed1 = 0.15; + double kp = 0.01; + + int counter = 0; + + int initialCount0 = encoder0.getPulses(); + int initialCount1 = encoder1.getPulses(); - speed0 = error0 * kp + speed0; - speed1 = error1 * kp + speed1; + double desiredCount0 = initialCount0 + desiredCountR*2; + double desiredCount1 = initialCount1 - desiredCountR*2; + + int count0 = initialCount0; + int count1 = initialCount1; + + double error0 = count0 - desiredCount0; + double error1 = count1 - desiredCount1; + + while(1){ - right_motor.move(speed0); - left_motor.move(speed1); + if(!(abs(error0) < 1) && !(abs(error1) < 1)){ + count0 = encoder0.getPulses(); + count1 = encoder1.getPulses(); + + error0 = count0 - desiredCount0; // moves forward + error1 = count1 - desiredCount1; // moves backwards + + speed0 = error0 * kp + speed0; + speed1 = error1 * kp + speed1; - serial.printf("ERROR=> 0:%f, 1:%f, SPEED=> 0:%f, 1:%f\n", error0, error1, speed0, speed1); - serial.printf("Pulse Count=> e0:%d, e1:%d \r\n", encoder0.getPulses(),encoder1.getPulses()); + right_motor.move(speed0); + left_motor.move(speed1); + counter = 0; + }else{ + counter++; + right_motor.brake(); + left_motor.brake(); + } + + if (counter > 60){ + break; + } + + // serial.printf("ERROR=> 0:%f, 1:%f, SPEED=> 0:%f, 1:%f\n", error0, error1, speed0, speed1); + // serial.printf("Pulse Count=> e0:%d, e1:%d \r\n", encoder0.getPulses(), encoder1.getPulses()); } right_motor.brake(); @@ -121,10 +284,6 @@ left_motor.brake(); right_motor.brake(); - while( 1 ) - { - - } } void printDipFlag() { @@ -194,10 +353,11 @@ // 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->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 @@ -227,9 +387,27 @@ while (1) { - //moveForwardUntilWallIr(); - turnRight(); + // moveForwardUntilWallIr(); + // wait(2); + //turnRight180(); + wait(0.5); + turnLeft180(); + wait(1); + //turnRight180(); wait(1); + //turnLeft(); + + // wait(1); + // turnLeft(); + // wait(1); + // turnRight(); + // wait(1); + // turnRight(); + // turnRight180(); + // wait(1); + // turnLeft180(); + // wait(1); + break; //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) ) ;