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
test.cpp
- Committer:
- DavidEGrayson
- Date:
- 2019-07-28
- Revision:
- 44:b4a00fbab06b
- Parent:
- 43:0e985a58f174
- Child:
- 45:81dd782bc0b4
File content as of revision 44:b4a00fbab06b:
// A file for testing routines that will not be used in the final firmware. #include <mbed.h> #include "motors.h" #include <Pacer.h> #include "main.h" #include "test.h" #include "leds.h" #include "encoders.h" #include "pc_serial.h" #include "line_sensors.h" #include "l3g.h" #include "turn_sensor.h" #include "reckoner.h" #include "buttons.h" void __attribute__((noreturn)) infiniteReckonerReportLoop(); void printBar(const char * name, uint16_t adcResult); void testLogger() { led1 = 1; while(!button1DefinitelyPressed()) { led3 = logger.isFull(); updateReckoner(); loggerService(); } led2 = 1; loggerReportLoop(); } void testCloseness() { doGyroCalibration(); turnSensor.start(); led1 = 1; while(1) { updateReckoner(); float magn = magnitude(); led3 = magn < (1 << 5); led4 = magn < (1 << 7); } } void showOrientationWithLeds34() { led3 = reckoner.cosv > 0; led4 = reckoner.sinv > 0; } void testTurnInPlace() { doGyroCalibration(); turnSensor.start(); led1 = 1; while(!button1DefinitelyPressed()) { updateReckoner(); showOrientationWithLeds34(); } led2 = 1; Pacer motorUpdatePacer(10000); Timer timer; timer.start(); motorsSpeedSet(-300, 300); while(timer.read_ms() < 4000) { updateReckoner(); showOrientationWithLeds34(); } timer.reset(); float integral = 0; while (timer.read_ms() < 4000) { if (motorUpdatePacer.pace()) { int16_t rotationSpeed; float s = (float)reckoner.sinv / (1 << RECKONER_LOG_UNIT); 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); } } infiniteReckonerReportLoop(); } // This also tests the LineTracker by printing out a lot of data from it. void testLineFollowing() { doGyroCalibration(); turnSensor.start(); led1 = 1; while(!button1DefinitelyPressed()) { updateReckoner(); } led2 = 1; Pacer reportPacer(200000); loadLineCalibration(); uint16_t loopCount = 0; while(1) { updateReckoner(); bool lineVisiblePrevious = lineTracker.getLineVisible(); lineTracker.read(); updateMotorsToFollowLine(); loopCount += 1; if (lineVisiblePrevious != lineTracker.getLineVisible()) { pc.printf("%5d ! %1d %4d | %5d %5d | %4d %4d %4d\r\n", loopCount, lineTracker.getLineVisible(), lineTracker.getLinePosition(), motorLeftSpeed, motorRightSpeed, lineTracker.calibratedValues[0], lineTracker.calibratedValues[1], lineTracker.calibratedValues[2] ); } if (reportPacer.pace()) { pc.printf("%5d %1d %4d | %5d %5d | %4d %4d %4d\r\n", loopCount, lineTracker.getLineVisible(), lineTracker.getLinePosition(), motorLeftSpeed, motorRightSpeed, lineTracker.calibratedValues[0], lineTracker.calibratedValues[1], lineTracker.calibratedValues[2] ); } } } void testDriveHome() { doGyroCalibration(); turnSensor.start(); led1 = 1; while(!button1DefinitelyPressed()) { updateReckoner(); } setLeds(1, 0, 1, 0); driveHomeAlmost(); //setLeds(0, 1, 1, 0); //finalSettleIn(); setLeds(1, 1, 1, 1); infiniteReckonerReportLoop(); } void testFinalSettleIn() { doGyroCalibration(); turnSensor.start(); led1 = 1; while(!button1DefinitelyPressed()) { updateReckoner(); } finalSettleIn(); infiniteReckonerReportLoop(); } void testButtons() { led1 = 1; while(!button1DefinitelyReleased()); while(!button1DefinitelyPressed()); led2 = 1; while(!button1DefinitelyReleased()); while(!button1DefinitelyPressed()); led3 = 1; while(!button1DefinitelyReleased()); while(!button1DefinitelyPressed()); led4 = 1; while(1){}; } void testReckoner() { doGyroCalibration(); turnSensor.start(); led1 = 1; Pacer reportPacer(100000); while(1) { updateReckoner(); led1 = reckoner.x > 0; led2 = reckoner.y > 0; showOrientationWithLeds34(); if (reportPacer.pace()) { pc.printf("%6d %6d %11d %11d | %8d %8d %10f\r\n", reckoner.cosv, reckoner.sinv, reckoner.x, reckoner.y, encoderLeft.getCount(), encoderRight.getCount(), determinant()); } } } void testTurnSensor() { pc.printf("Test turn sensor\r\n"); doGyroCalibration(); led1 = 1; //Pacer reportPacer(200000); // 0.2 s Pacer reportPacer(10000000); // 10 s Timer timer; timer.start(); turnSensor.start(); while(1) { turnSensor.update(); if (reportPacer.pace()) { pc.printf("%u, %d, %d\r\n", timer.read_ms(), turnSensor.getAngleDegrees(), turnSensor.getAngleMillidegrees()); } } } void testL3gAndShowAverage() { wait_ms(2000); Pacer reportPacer(750000); Pacer readingPacer(2000); int32_t total = 0; int32_t readingCount = 0; while(1) { if (readingPacer.pace()) { int32_t result = l3gZAvailable(); if (result == 1) { int32_t gz = l3gZRead(); if (gz < -500000) { pc.printf("l3gZRead error: %d\n", gz); } else { total += gz; readingCount += 1; } } else if (result != 0) { pc.printf("l3gZAvailable error: %d\n", result); } } if (reportPacer.pace()) { float average = (float)total / readingCount; pc.printf("%d, %d, %f\r\n", total, readingCount, average); } } // Gyro calibration results get hardcoded into TurnSensor::update() // for now until we figure out something better. } void testL3g() { Pacer reportPacer(750000); Timer timer; timer.start(); int32_t gz = 0; bool reportedReading = false; while(1) { int32_t result = l3gZAvailable(); if (result == 1) { gz = l3gZRead(); reportedReading = false; if (gz > 100 || gz < -100) { pc.printf("%u, %d\r\n", timer.read_us(), gz); reportedReading = true; } } else if (result != 0) { pc.printf("l3gZAvailable error: %d\n", result); } if (reportPacer.pace() && !reportedReading) { pc.printf("%u, %d\r\n", timer.read_us(), gz); reportedReading = true; } } } void testLineSensors() { led1 = 1; Pacer reportPacer(100000); Pacer clearStatsPacer(2000000); uint16_t min[LINE_SENSOR_COUNT]; uint16_t max[LINE_SENSOR_COUNT]; bool const printBarGraph = true; while (1) { if (clearStatsPacer.pace()) { for(uint8_t i = 0; i < LINE_SENSOR_COUNT; i++) { min[i] = 0xFFFF; max[i] = 0; } } uint16_t values[3]; readSensors(values); for(uint8_t i = 0; i < LINE_SENSOR_COUNT; i++) { if (values[i] > max[i]){ max[i] = values[i]; } if (values[i] < min[i]){ min[i] = values[i]; } } if (reportPacer.pace()) { if (printBarGraph) { pc.printf("\x1B[0;0H"); // VT100 command for "go to 0,0" printBar("L", values[0]); printBar("M", values[1]); printBar("R", values[2]); pc.printf("%4d %4d \r\n", min[0], max[0]); pc.printf("%4d %4d \r\n", min[1], max[1]); pc.printf("%4d %4d \r\n", min[2], max[2]); } else { pc.printf("%8d %8d %8d\r\n", values[0], values[1], values[2]); } } } } // Values from David's office Values from dev lab, // in the day time, 2014-02-27: 2014-02-27: // # calmin calmax // 0 34872 59726 0 40617 60222 // 1 29335 60110 1 36937 61198 // 2 23845 58446 2 33848 58862 void testCalibrate() { Timer timer; timer.start(); Pacer reportPacer(200000); bool doneCalibrating = false; led1 = 1; while(1) { lineTracker.read(); if(!doneCalibrating) { lineTracker.updateCalibration(); } led3 = doneCalibrating; led4 = lineTracker.getLineVisible(); if (button1DefinitelyPressed()) { doneCalibrating = true; } if (reportPacer.pace()) { pc.printf("\x1B[0;0H"); // VT100 command for "go to 0,0" for(uint8_t s = 0; s < LINE_SENSOR_COUNT; s++) { pc.printf("%-2d %5d %5d %5d\r\n", s, lineTracker.calibratedMinimum[s], lineTracker.rawValues[s], lineTracker.calibratedMaximum[s]); } } } } void testMotorSpeed() { led1 = 1; motorsSpeedSet(400, 400); wait_ms(4000); uint32_t left = encoderLeft.getCount(); uint32_t right = encoderRight.getCount(); motorsSpeedSet(0, 0); Pacer reportPacer(500000); while (1) { if (reportPacer.pace()) { led2 = 1; pc.printf("%8d %8d\r\n", left, right); led2 = 0; } } } void testEncoders() { Pacer reportPacer(500000); led1 = 1; while(1) { while(encoderBuffer.hasEvents()) { PololuEncoderEvent event = encoderBuffer.readEvent(); } if(reportPacer.pace()) { led2 = 1; pc.printf("%8d %8d\r\n", encoderLeft.getCount(), encoderRight.getCount()); led2 = 0; } } } void testMotors() { led1 = 1; led2 = 0; led3 = 0; while(1) { motorsSpeedSet(0, 0); led2 = 0; led3 = 0; wait(2); motorsSpeedSet(300, 300); wait(2); motorsSpeedSet(-300, 300); wait(2); motorsSpeedSet(0, 0); led2 = 1; wait(2); motorsSpeedSet(600, 600); wait(2); motorsSpeedSet(0, 0); led3 = 1; wait(2); motorsSpeedSet(1200, 1200); wait(2); } } void infiniteReckonerReportLoop() { Pacer reportPacer(200000); while(1) { showOrientationWithLeds34(); if(reportPacer.pace()) { pc.printf("%6d %6d %11d %11d | %11f %11f\r\n", reckoner.cosv, reckoner.sinv, reckoner.x, reckoner.y, determinant(), dotProduct()); } } } // with should be between 0 and 63 void printBar(const char * name, uint16_t result) { pc.printf("%-2s %5d |", name, result); uint16_t width = result >> 4; if (width > 63) { width = 63; } uint8_t i; for(i = 0; i < width; i++){ pc.putc('#'); } for(; i < 63; i++){ pc.putc(' '); } pc.putc('|'); pc.putc('\r'); pc.putc('\n'); }