int main(void) {
init();
serial_putString("Starting up...\r\n");
/*
* Create a job description for a blinking LED
*/
job_t blinkyLedJob = {
.activationTime = timer_currentTime(),
.jobFunction = blinkyLed
};
/*
* Create a job description that will check the state of the buttons
*/
job_t checkButtonsJob = {
.activationTime = timer_currentTime(),
.jobFunction = checkButtons
};
job_t checkSerialJob = {
.activationTime = timer_currentTime(),
.jobFunction = checkUart
};
job_t sendI2CJob = {
.activationTime = timer_currentTime(),
.jobFunction = sendi2c
};
// Add the jobs into the jobs controller
jobs_add(&blinkyLedJob);
jobs_add(&checkButtonsJob);
jobs_add(&checkSerialJob);
jobs_add(&sendI2CJob);
while (1) {
board_update(); // Run the low level operations (buttons, uart...)
jobs_update(); // Run the jobs
// Go into sleep mode
//board_idle();
}
return 0;
}
/**
* 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
}
