int main(void) { lcd_init(); timer_init(); while(1) { unsigned pulse_width = ping_read(); unsigned long distance = time2dist(pulse_width); lprintf("Delta: %d \nDistance: %d \nOverflows: %d", pulse_width, distance, getOverflow()); wait_ms(300); } }
int main(void) { //lprintf("%d", 4); init_ADC(); init_timer3(); init_timer(); init_USART(calcUBRR(BAUD)); char * str = "Degrees\tIR Distance (cm)\tSonar Distance (cm)\n\n\r"; move_servo(0); char degrees = 0; while(1) { //send_pulse(); Transmit_String(str); sprintf(str, "%d\t%d\t%d\n\r", degrees, ir_distance(), time2dist(ping_read())); move_servo(degrees); degrees += 2; if(degrees > 180) break; } }
/** * Takes 180 data points from the ping sensor, converts all values into cm, and puts all objects into the given container * @param objects an array of objects used to store any data collected by sweepScan */ void sweepScan(Object objects[]) { int degrees = 0; int objectCount = 0; //number of objects already scanned int prevDetectionStatus = 0; //previous state of object detection move_servo(0); //move servo to starting position wait_ms(1000); //wait for initializations to finish int finalValuesCalculated = 1; while(degrees < 180) { //for one full rotation move_servo(degrees); //sweep servo degrees++; //increments of 2 degrees ping_read(delta); //take ping sensor data wait_ms(10); //wait for return pulse int foundDelta = delta; pingDistance = timeToDist(foundDelta); //convert ping data to cm quantization = avgSensorResults(); //read from ADC channel 2 (IR sensor) IRdistance = 2364.5*(pow(quantization, -0.888)); //convert quantization to distance in cm int objectDetected = 0; //whether or not an object is being detected //int distDifference = abs(IRdistance-pingDistance); //determine the absolute value of the difference between sensor values if (IRdistance < 85 && prevDetectionStatus == 0) { //if an object is near and was not previously being detected objectDetected = 1; //an object is near Object scannedObject; //create a new object struct scannedObject.degreePosition = degrees; //set degrees to current servo position scannedObject.scannedDegrees = 1; //currently been scanned for one degree scannedObject.cmDistance = 0; //distance according to ping sensor scannedObject.isValid = 1; objects[objectCount] = scannedObject; //add object to objects array objectCount++; //increment object count by 1 prevDetectionStatus = objectDetected; finalValuesCalculated = 0; } else if (IRdistance < 150 && prevDetectionStatus == 1) { //if an object is still being detected objectDetected = 1; } else if (IRdistance > 150 && prevDetectionStatus == 1) { //if a large change in IRdistance has occurred (noise) but an object was being scanned objectDetected = 1; //ignore IRdistance and continue scanning object prevDetectionStatus = 0; //if the large gap persists, assume object is no longer being scanned } if (objectDetected) { //if currently scanning an object objects[objectCount-1].scannedDegrees++; //increase number of degrees scanned for each servo rotation objects[objectCount-1].cmDistance += pingDistance; } if (objectDetected == 0 && finalValuesCalculated == 0) { //if the object is no longer being detected, perform final calculations objects[objectCount-1].cmDistance = (objects[objectCount-1].cmDistance/objects[objectCount-1].scannedDegrees); objects[objectCount-1].cmWidth = ((2*objects[objectCount-1].cmDistance) * tan(((objects[objectCount-1].scannedDegrees)*3.14)/360)); //calculate width using angular diameter formula finalValuesCalculated = 1; } //lprintf("Objects: %d\nDegrees: %d\nWidth: %d", objectCount, objects[objectCount-1].scannedDegrees, objects[objectCount-1].cmWidth); //FOR DEBUG ONLY /*char toPrint[31]; //contains string to pass to putty toPrint[0] = ' '; sprintf(toPrint, "%d %d %lu %d\n\r", degrees, IRdistance, pingDistance, objectDetected); serial_putString(toPrint, 29); //send string to putty for debug */ } }
/** * Takes 180 data measurements from the ping sensor, converts them to cm values, and determines the smallest object's index, width, and distance. * */ void scanSmallestObj() { int degrees = 0; Object objects[10]; //holds scanned objects for later analysis int objectCount = 0; //number of objects already scanned int prevDetectionStatus = 0; //previous state of object detection move_servo(0); //move servo to starting position wait_ms(1500); //wait for initializations to finish int finalValuesCalculated = 1; while(degrees < 180) { //for one full rotation move_servo(degrees); //sweep servo wait_ms(10); degrees++; //increments of 2 degrees ping_read(delta); //take ping sensor data wait_ms(50); //wait for return pulse int foundDelta = delta; pingDistance = timeToDist(foundDelta); //convert ping data to cm quantization = avgSensorResults(); //read from ADC channel 2 (IR sensor) IRdistance = 2364.5*(pow(quantization, -0.888)); //convert quantization to distance in cm int objectDetected = 0; //whether or not an object is being detected //int distDifference = abs(IRdistance-pingDistance); //determine the absolute value of the difference between sensor values if (IRdistance < 85 && prevDetectionStatus == 0) { //if an object is near and was not previously being detected objectDetected = 1; //an object is near Object scannedObject; //create a new object struct scannedObject.degreePosition = degrees; //set degrees to current servo position scannedObject.scannedDegrees = 1; //currently been scanned for one degree scannedObject.cmDistance = 0; //distance according to ping sensor objects[objectCount] = scannedObject; //add object to objects array objectCount++; //increment object count by 1 prevDetectionStatus = objectDetected; finalValuesCalculated = 0; } else if (IRdistance < 150 && prevDetectionStatus == 1) { //if an object is still being detected objectDetected = 1; } else if (IRdistance > 150 && prevDetectionStatus == 1) { //if a large change in IRdistance has occurred (noise) but an object was being scanned objectDetected = 1; //ignore IRdistance and continue scanning object prevDetectionStatus = 0; //if the large gap persists, assume object is no longer being scanned } if (objectDetected) { //if currently scanning an object objects[objectCount-1].scannedDegrees++; //increase number of degrees scanned for each servo rotation objects[objectCount-1].cmDistance += pingDistance; } if (objectDetected == 0 && finalValuesCalculated == 0) { //if the object is no longer being detected, perform final calculations objects[objectCount-1].cmDistance = (objects[objectCount-1].cmDistance/objects[objectCount-1].scannedDegrees); objects[objectCount-1].cmWidth = ((2*objects[objectCount-1].cmDistance) * tan(((objects[objectCount-1].scannedDegrees)*3.14)/360)); //calculate width using angular diameter formula finalValuesCalculated = 1; } lprintf("Objects: %d\nDegrees: %d\nWidth: %d", objectCount, objects[objectCount-1].scannedDegrees, objects[objectCount-1].cmWidth); //FOR DEBUG ONLY char toPrint[31]; //contains string to pass to putty toPrint[0] = ' '; sprintf(toPrint, "%d %d %lu %d\n\r", degrees, IRdistance, pingDistance, objectDetected); serial_putString(toPrint, 29); //send string to putty } int smallestWidth = 1023; //used to determine smallest object int index = 0; //current object index int removedObjects = 0; //running count of objects thrown out due to size or distance int prevRemovedObjects = 0; //objects removed prior to indicated index int totalRemovedObjects = 0; int loopRuns = 0; for (int i = 0; i < objectCount; i++) { if (objects[i].cmWidth <= 3 || objects[i].cmDistance > 100) { //if the object is very small or very far away, throw it out removedObjects++; loopRuns++; totalRemovedObjects++; } else if ((objects[i].cmWidth < smallestWidth) && objects[i].cmWidth > 3) { //check if current object has new smallest width smallestWidth = objects[i].cmWidth; //replace smallest width with new value index = i; //lock onto index prevRemovedObjects = removedObjects; //use for index compensation before final print statement removedObjects = 0; //reset value for previous removed objects } } lprintf("Index: %d of %d\nDist (cm): %d\nAngular width: %d\nWidth (cm): %d\n", (index-prevRemovedObjects+1), (objectCount-removedObjects+prevRemovedObjects), objects[index].cmDistance, objects[index].scannedDegrees, objects[index].cmWidth); //final results move_servo(objects[index].degreePosition); //point to smallest object }