void locationListTest(void) {
initLocationList(&locationList, (Location(*)[]) &locationListArray, LOCATION_LIST_ARRAY_TEST);
Location* locationA;
Location* locationB;
Location* locationC;
Location* locationD;
Location* locationE;
Location* tmpLocation;
// Location* tmpLocation2;
// Emtpy Test
bool isEmpty = isEmptyLocationList(&locationList);
TEST_ASSERT_TRUE(isEmpty);
locationA = addNamedLocation(&locationList, "A", 20, 20);
locationB = addNamedLocation(&locationList, "B", 10, 30);
locationC = addNamedLocation(&locationList, "C", -1, 2);
locationD = addNamedLocation(&locationList, "D", -100, 200);
locationE = addNamedLocation(&locationList, "E", -10, -50);
isEmpty = isEmptyLocationList(&locationList);
TEST_ASSERT_FALSE(isEmpty);
// getLocationCount
unsigned locationCount = getLocationCount(&locationList);
TEST_ASSERT_EQUAL(5, locationCount);
// findLocationByName
tmpLocation = findLocationByStringName(&locationList, "C");
TEST_ASSERT_EQUAL(locationC, tmpLocation);
// locationEquals
bool actual = locationEquals(locationA, locationA);
TEST_ASSERT_TRUE(actual);
actual = locationEquals(locationA, locationC);
TEST_ASSERT_FALSE(actual);
// getNearestLocation
tmpLocation = getNearestLocation(&locationList, -120, 180);
TEST_ASSERT_EQUAL(locationD, tmpLocation);
}
void Trinbot::update()
{
int d_left_odom = left_odom - last_left_odom;
int d_right_odom = right_odom - last_right_odom;
last_left_odom = left_odom;
last_right_odom = right_odom;
int dt = millis() - last_time;
last_time = millis();
double wheel_difference = (d_left_odom-d_right_odom)/(2.0*TICKS_PER_CM);
double dP = (d_left_odom + d_right_odom)/(2.0*TICKS_PER_CM); //[cm]
//float dtheta = 180.0*((left_odom-right_odom)/WHEEL_SEPARATION)/PI; //[degrees]
// double dtheta = (180.0*atan2(wheel_difference,WHEEL_SEPARATION)/PI); //[degrees]
double dtheta = wheel_difference/TICKS_PER_DEG; //[degrees]
float velocity = dP*1000.0/dt; // [cm/s] average of the velocities of the wheels
float heading = get_current_heading() + dtheta; // [cm/s] small angle assumption
heading += 720;
int numfits = heading/360;
heading -= numfits*360.0;
xPos += dP*cos(heading*PI/180.0);
yPos += dP*sin(heading*PI/180.0);
Serial.print("xPos: ");
Serial.print(xPos,DEC);
Serial.print(" yPos: ");
Serial.print(yPos,DEC);
Serial.print(" theta: ");
Serial.print(heading,DEC);
int closestloc = getNearestLocation();
Serial.print(" closestLoc: ");
Serial.print(locations[closestloc].name);
Serial.println(" ");
set_current_velocity(velocity);
set_current_heading(heading);
}
Parameters ParameterFile::getParameters(int iTime, const Location& iLocation, bool iAllowNearestNeighbour) const {
if(iTime < 0) {
std::stringstream ss;
ss << "Could not load parameters for time " << iTime;
Util::error(ss.str());
}
int time = iTime;
if(!isTimeDependent())
time = 0;
if(time > mMaxTime) {
std::stringstream ss;
ss << "Could not load parameters for time " << time << " (max " << mMaxTime << ")";
Util::error(ss.str());
}
if(mParameters.size() == 0)
return Parameters();
// Find the right location to use
Location loc = iLocation;
if(iAllowNearestNeighbour) {
bool found = getNearestLocation(time, iLocation, loc);
if(!found)
return Parameters();
}
// Find the right time to use
LocationParameters::const_iterator it = mParameters.find(loc);
if(it == mParameters.end())
return Parameters();
const std::vector<Parameters>& timeParameters = it->second;
if(timeParameters.size() > time)
return timeParameters[time];
else {
return Parameters();
}
}