double findObject(double leftPulse, double rightPulse){
// one edge is global_u1_time, other is global_u2_time
// perform law of cosines, let u1 = a, u2 = b, and base = c
double leftLength = convertToDistance(leftPulse * timerPeriod);
double rightLength = convertToDistance(rightPulse * timerPeriod);
double c = sideLength;
double preAngleA = (rightLength * rightLength + c * c - leftLength * leftLength) / (2 * rightLength * c);
double angleA = acos(preAngleA) * 180 / pi;
double preAngleB = (leftLength * leftLength + c * c - rightLength * rightLength) / (2 * leftLength * c);
double angleB = acos(preAngleB) * 180 / pi;
double angleDiff = angleA - angleB;
// find distance to the object
double baseDistance = leftLength * (sin(angleA * pi / 180));
// if we are too close, then we need to backup/turn
if (baseDistance > 40)
{
}
// if the angles are less than 5 degrees apart consider it straight ahead
if (fabs(angleDiff) <= 5 ){
return 45; // simulate turning 45 degrees
}
else {
return angleDiff; // angle diff will represent how much it needs to turn
}
// still needs conditions if facing the fridge/etc
// need to find the distance to the object
}
// Returns the distance in centimeters from a pin.
unsigned int long_range_distance(unsigned char pin) {
unsigned int distVoltage = analog_read(pin);
unsigned int distValue = convertToDistance(distVoltage);
}
