Skip to content

Instantly share code, notes, and snippets.

@calebreister
Created April 3, 2014 17:05
Show Gist options
  • Save calebreister/9958562 to your computer and use it in GitHub Desktop.
Save calebreister/9958562 to your computer and use it in GitHub Desktop.
This could be useful for some people involved in FIRST robotics, but I posted it in order to get it logic-checked.
//earlier in the program I run this to initialize the array
for (int i = 0; i < SONIC_SAMPLE * 2; i++)
sonicRead();
//Averages and returns the inch reading of the ultrasonic sensor
//Updates sonicHotZone
SonicData robot::sonicRead()
{
SonicData out;
out.avg = 0;
//shift sonicLog data
sonicLog[SONIC_SAMPLE - 1] = sonicLog[SONIC_SAMPLE - 2];//shift last value
for (int i = 1; i <= SONIC_SAMPLE - 2; i++)//shift mid values
sonicLog[i] = sonicLog[i - 1];
//add new 1st value
long double localAvg = 0;
long double localSonicLog[10];
for (int i = 0; i < 10; i++)
{
localSonicLog[i] = sonic->GetVoltage() / VOLTS_INCH;
localAvg += sonicData[i];
feed();
}
localAvg /= 10;
sonicLog[0] = localAvg;
//get the average of sonicLog
for(int i = 0; i < SONIC_SAMPLE; i++)
out.avg += sonicLog[i];
out.avg /= SONIC_SAMPLE;
//Check for hot zone
if (out.avg >= TOO_CLOSE
&& out.avg <= TOO_FAR)
out.hotZone = true;
else
out.hotZone = false;
return out;
}
//Ultrasonic Sensor
const int TOO_FAR = 120;
const int TOO_CLOSE = 60;
const double VOLTS_INCH = 0.009765625;//5/512
const int SONIC_SAMPLE = 10;
//Package for ultrasonic sensor data
//Datatype returned by sonicRead function
struct SonicData
{
long double avg;//average value over SONIC_SAMPLE time
bool hotZone;//whether or not the robot is in the hot zone
//HOT ZONE IS SET BY TOO_FAR and TOO_CLOSE
};
Sign up for free to join this conversation on GitHub. Already have an account? Sign in to comment