Mouse code for the MacroRat
Diff: main.cpp
- Revision:
- 14:9e7bb03ddccb
- Parent:
- 13:2032db00f168
- Child:
- 15:b80555a4a8b9
diff -r 2032db00f168 -r 9e7bb03ddccb main.cpp --- a/main.cpp Sat May 13 02:40:49 2017 +0000 +++ b/main.cpp Sat May 13 23:49:02 2017 +0000 @@ -6,10 +6,9 @@ #include <stdlib.h> #include "ITG3200.h" -#include "stm32f4xx.h" +#include "stm32f4xx.h" #include "QEI.h" - /* Constants for when HIGH_PWM_VOLTAGE = 0.2 #define IP_CONSTANT 6 #define II_CONSTANT 0 @@ -17,229 +16,263 @@ */ // Constants for when HIGH_PWM_VOLTAGE = 0.1 -#define IP_CONSTANT 13 +#define IP_CONSTANT 12 #define II_CONSTANT 0 -#define ID_CONSTANT 1. +#define ID_CONSTANT 2 + +const int desiredCountR = 1400; +const int desiredCountL = 1475; -const int desiredCountR = 1300; -const int desiredCountL = 1400; +const int oneCellCount = 5300; -void turnLeft(){ +void turnLeft() +{ double speed0 = 0.15; - double speed1 = 0.15; - double kp = 0.01; + double speed1 = -0.15; int counter = 0; - - int initialCount0 = encoder0.getPulses(); - int initialCount1 = encoder1.getPulses(); + int initial0 = encoder0.getPulses(); + int initial1 = encoder1.getPulses(); - double desiredCount0 = initialCount0 - desiredCountL; - double desiredCount1 = initialCount1 + desiredCountL; + int desiredCount0 = initial0 - desiredCountL; + int desiredCount1 = initial1 + desiredCountL; - int count0 = initialCount0; - int count1 = initialCount1; + int count0 = initial0; + int count1 = initial1; double error0 = count0 - desiredCount0; double error1 = count1 - desiredCount1; - while(1){ - if(!(abs(error0) < 1) && !(abs(error1) < 1)){ + 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; + error0 = count0 - desiredCount0; + error1 = count1 - desiredCount1; right_motor.move(speed0); left_motor.move(speed1); counter = 0; - }else{ + } else { counter++; right_motor.brake(); left_motor.brake(); } - if (counter > 60){ + 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; +void turnRight() +{ + double speed0 = -0.15; double speed1 = 0.15; - double kp = 0.01; int counter = 0; - - int initialCount0 = encoder0.getPulses(); - int initialCount1 = encoder1.getPulses(); + int initial0 = encoder0.getPulses(); + int initial1 = encoder1.getPulses(); - double desiredCount0 = initialCount0 + desiredCountR; - double desiredCount1 = initialCount1 - desiredCountR; + int desiredCount0 = initial0 + desiredCountR; + int desiredCount1 = initial1 - desiredCountR; - int count0 = initialCount0; - int count1 = initialCount1; + int count0 = initial0; + int count1 = initial1; double error0 = count0 - desiredCount0; double error1 = count1 - desiredCount1; - while(1){ - if(!(abs(error0) < 1) && !(abs(error1) < 1)){ + 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; + error0 = count0 - desiredCount0; + error1 = count1 - desiredCount1; right_motor.move(speed0); left_motor.move(speed1); counter = 0; - }else{ + } else { counter++; right_motor.brake(); left_motor.brake(); } - if (counter > 60){ + 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(){ +void turnLeft180() +{ double speed0 = 0.15; - double speed1 = 0.15; - double kp = 0.01; + double speed1 = -0.15; int counter = 0; - - int initialCount0 = encoder0.getPulses(); - int initialCount1 = encoder1.getPulses(); + int initial0 = encoder0.getPulses(); + int initial1 = encoder1.getPulses(); - double desiredCount0 = initialCount0 - 3000; - double desiredCount1 = initialCount1 + 2700; + int desiredCount0 = initial0 - desiredCountL*2; + int desiredCount1 = initial1 + desiredCountL*2; - int count0 = initialCount0; - int count1 = initialCount1; + int count0 = initial0; + int count1 = initial1; double error0 = count0 - desiredCount0; double error1 = count1 - desiredCount1; - while(1){ - if(!(abs(error0) < 1) && !(abs(error1) < 1)){ + 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; + error0 = count0 - desiredCount0; + error1 = count1 - desiredCount1; right_motor.move(speed0); left_motor.move(speed1); counter = 0; - }else{ + } else { counter++; right_motor.brake(); left_motor.brake(); } - if (counter > 60){ + if (counter > 60) { break; } - } - - right_motor.brake(); - left_motor.brake(); -} - -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(); - - 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){ - - 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; - - 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(); left_motor.brake(); } -void moveForwardUntilWallIr() { +void turnRight180() +{ + double speed0 = -0.15; + double speed1 = 0.15; + + int counter = 0; + int initial0 = encoder0.getPulses(); + int initial1 = encoder1.getPulses(); + + int desiredCount0 = initial0 + desiredCountR*2; + int desiredCount1 = initial1 - desiredCountR*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(); +} + +void moveForwardOneCellEncoder(){ // 0 is left wheel! + double speed0 = 0.15; + double speed1 = 0.15; + + int counter = 0; + int initial0 = encoder0.getPulses(); + int initial1 = encoder1.getPulses(); + + double kpLeft = 0.000005; + double kpRight = 0.000005; + + int desiredCount0 = initial0 + oneCellCount; + int desiredCount1 = initial1 + oneCellCount; + + int count0 = initial0; + int count1 = initial1; + + int error = 0; + + while (1){ + if (count0 < desiredCount0 && count1 < desiredCount1){ + count0 = encoder0.getPulses(); + count1 = encoder1.getPulses(); + + error = count0 - count1; // if error is positive, then left has moved more, so we want to decrease its speed and increase the right speed + + if (error > 0){ + speed0 = 0.15 - error*kpLeft; + speed1 = 0.15 + error*kpRight; + } + else if (error <= 0){ + speed0 = 0.15 + error*kpLeft; + speed1 = 0.15 - error*kpRight; + } + // serial.printf("The error is %d \n", error); + // serial.printf("The Left speed is: %f and the right speed is: %f \n", speed0, speed1); + right_motor.move(speed0); + left_motor.move(speed1); + counter = 0; + } + else { + break; + } + } + greenLed.write(1); + blueLed.write(0); + right_motor.brake(); + left_motor.brake(); +} + +void moveForwardUntilWallIr() +{ double currentError = 0; double previousError = 0; double derivError = 0; double sumError = 0; - + double HIGH_PWM_VOLTAGE = 0.1; - + double rightSpeed = 0.1; double leftSpeed = 0.1; - + 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) { int pulseCount = (encoder0.getPulses()-count) % 5600; @@ -248,77 +281,86 @@ } 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; + 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); + 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); + } 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); + right_motor.forward(rightSpeed); left_motor.forward(leftSpeed); - + previousError = currentError; - + ir2 = IRP_2.getSamples( SAMPLE_NUM ); - ir3 = IRP_3.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(); } -void printDipFlag() { +void printDipFlag() +{ if (DEBUGGING) serial.printf("Flag value is %d", dipFlags); } -void enableButton1() { +void enableButton1() +{ dipFlags |= BUTTON1_FLAG; printDipFlag(); } -void enableButton2() { +void enableButton2() +{ dipFlags |= BUTTON2_FLAG; printDipFlag(); } -void enableButton3() { +void enableButton3() +{ dipFlags |= BUTTON3_FLAG; printDipFlag(); } -void enableButton4() { +void enableButton4() +{ dipFlags |= BUTTON4_FLAG; printDipFlag(); } -void disableButton1() { +void disableButton1() +{ dipFlags &= ~BUTTON1_FLAG; printDipFlag(); } -void disableButton2() { +void disableButton2() +{ dipFlags &= ~BUTTON2_FLAG; printDipFlag(); } -void disableButton3() { +void disableButton3() +{ dipFlags &= ~BUTTON3_FLAG; printDipFlag(); } -void disableButton4() { +void disableButton4() +{ dipFlags &= ~BUTTON4_FLAG; printDipFlag(); } @@ -339,9 +381,9 @@ // IR_4.write(1); redLed.write(1); - greenLed.write(1); + greenLed.write(0); blueLed.write(1); - + //left_motor.forward(0.1); //right_motor.forward(0.1); @@ -351,14 +393,14 @@ // 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 - */ - + /* 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 @@ -372,49 +414,36 @@ // 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); - while (1) { + moveForwardOneCellEncoder(); - // moveForwardUntilWallIr(); - // wait(2); - //turnRight180(); - wait(0.5); - turnLeft180(); - wait(1); - //turnRight180(); - wait(1); - //turnLeft(); + // moveForwardUntilWallIr(); + // wait(2); + // turnRight(); + // wait(1); + // moveForwardOneCellEncoder(); + // moveForwardUntilWallIr(); - // 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) ) ; - - //serial.printf("IRS= >: %f, %f, %f, %f, %f \r\n", + + //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);