David's dead reckoning code for the LVBots competition on March 6th. Uses the mbed LPC1768, DRV8835, QTR-3RC, and two DC motors with encoders.
Dependencies: PololuEncoder Pacer mbed GeneralDebouncer
Diff: test.cpp
- Revision:
- 33:58a0ab6e9ad2
- Parent:
- 32:83a13b06093c
- Child:
- 37:23000a47ed2b
diff -r 83a13b06093c -r 58a0ab6e9ad2 test.cpp --- a/test.cpp Tue Mar 04 04:32:51 2014 +0000 +++ b/test.cpp Wed Mar 05 02:50:09 2014 +0000 @@ -16,56 +16,75 @@ void __attribute__((noreturn)) infiniteReckonerReportLoop(); void printBar(const char * name, uint16_t adcResult); -uint16_t readPin18() +void testCloseness() { - // Set PCADC bit in PCONP register. (Table 46). - LPC_SC->PCONP |= (1 << 12); - - // Select PCLK_ADC in PCLKSEL0 register. (Table 40, Table 531). - LPC_SC->PCLKSEL0 |= (3 << 24); // PCLK for ADC = CCLK/8 - - // Enable ADC0 through PINSEL registers. (Section 8.5). - LPC_PINCON->PINSEL1 = (LPC_PINCON->PINSEL1 & ~(3 << 20)) | (1 << 20); - - // Pin 18: P0.26/AD0.3/AOUT/RXD3 - - // 7:0 Bitmap to select what channel to use - // 15:8 Select clock. - // 16 BURST = 0 - // 20:17 Reserved - // 21 PDN = 1, A/D converter is operational - // 26:24 START = 001 to start conversion now - LPC_ADC->ADCR = (1 << 3) | (0xFF << 8) | (1 << 21); - LPC_ADC->ADCR |= (1 << 24); - - while(!(LPC_ADC->ADGDR >> 31 & 1)) // while not done + led1 = 1; + while(1) { + updateReckonerFromEncoders(); + float magn = magnitude(); + + led3 = (magn < (1<<(14+7))); + led4 = (magn < (1<<(14+9))); } - //return 2; - return LPC_ADC->ADGDR & 0xFFFF; +} + +void showOrientationWithLeds34() +{ + led3 = reckoner.cos > 0; + led4 = reckoner.sin > 0; } -uint16_t readP10() +void testTurnInPlace() { - DigitalInOut pin(p10); - pin.mode(PullNone); - pin.output(); - pin = 1; - wait_us(20); - uint16_t value = 1000; + led1 = 1; + while(!button1DefinitelyPressed()) + { + updateReckonerFromEncoders(); + showOrientationWithLeds34(); + } + led2 = 1; + + Pacer motorUpdatePacer(10000); Timer timer; timer.start(); - pin.input(); - while(timer.read_us() < 1000) + motorsSpeedSet(-300, 300); + while(timer.read_ms() < 4000) + { + updateReckonerFromEncoders(); + showOrientationWithLeds34(); + } + timer.reset(); + + float integral = 0; + while (timer.read_ms() < 4000) { - if(pin.read() == 0) + if (motorUpdatePacer.pace()) { - return timer.read_us(); + int16_t rotationSpeed; + float s = (float)reckoner.sin / (1 << 30); + integral += s; + rotationSpeed = -(s * 2400 + integral * 20); + + if (rotationSpeed > 450) + { + rotationSpeed = 450; + } + if (rotationSpeed < -450) + { + rotationSpeed = -450; + } + + int16_t speedLeft = -rotationSpeed; + int16_t speedRight = rotationSpeed; + motorsSpeedSet(speedLeft, speedRight); } } - return value; + + infiniteReckonerReportLoop(); } + void testSensorGlitches() { AnalogIn testInput(p18); @@ -98,7 +117,10 @@ } } **/ - reading = readP10(); + + uint16_t values[LINE_SENSOR_COUNT]; + readSensors(values); + reading = values[0]; if(reading > 100) { @@ -208,8 +230,14 @@ { updateReckonerFromEncoders(); } + + setLeds(1, 0, 1, 0); driveHomeAlmost(); - finalSettleIn(); + + //setLeds(0, 1, 1, 0); + //finalSettleIn(); + + setLeds(1, 1, 1, 1); infiniteReckonerReportLoop(); } @@ -250,10 +278,10 @@ while(1) { updateReckonerFromEncoders(); - led1 = (reckoner.cos > 0); - led2 = (reckoner.sin > 0); - led3 = (reckoner.x > 0); - led4 = (reckoner.y > 0); + + led1 = (reckoner.x > 0); + led2 = (reckoner.y > 0); + showOrientationWithLeds34(); if (reportPacer.pace()) { @@ -421,13 +449,12 @@ Pacer reportPacer(200000); while(1) { + showOrientationWithLeds34(); if(reportPacer.pace()) { - led4 = 1; pc.printf("%11d %11d %11d %11d | %11f %11f\r\n", reckoner.cos, reckoner.sin, reckoner.x, reckoner.y, determinant(), dotProduct()); - led4 = 0; } }