Mouse code for the MacroRat
main.cpp
- Committer:
- christine222
- Date:
- 2017-05-13
- Revision:
- 13:2032db00f168
- Parent:
- 12:5790e56a056f
- Child:
- 14:9e7bb03ddccb
File content as of revision 13:2032db00f168:
#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 13 #define II_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.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 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 turnLeft180(){ 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 - 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; 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 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() { 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; 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; 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(); } 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() { //enableMotors(); //Set highest bandwidth. gyro.setLpBandwidth(LPFBW_42HZ); serial.baud(9600); serial.printf("The gyro's address is %s", gyro.getWhoAmI()); wait (0.1); // IR_1.write(1); // IR_2.write(1); // IR_3.write(1); // IR_4.write(1); redLed.write(1); greenLed.write(1); 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); while (1) { // 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) ) ; //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); // wait_ms(1000); // redLed.write(1); // greenLed.write(0); // wait_ms(1000); // greenLed.write(1); // blueLed.write(0); // wait_ms(1000); // blueLed.write(1); } }