int Car::steering_for_curvature(Angle theta_per_meter) {
static const LookupTable t(
{
{-85.1, 1929},
{-71.9, 1839},
{-58.2, 1794},
{-44.1,1759},
{-29.6, 1678},
{-14.8, 1599},
{0, 1521},
{14.8, 1461},
{29.6,1339},
{44.0,1306},
{58.2,1260},
{71.9,1175},
{85.1,1071}
});
return (int) t.lookup(theta_per_meter.degrees());
}
Angle Car::angle_for_steering(int str) {
static const LookupTable t(
{
{1071, 30},
{1175, 25},
{1260, 20},
{1306, 15},
{1339, 10},
{1461, 5},
{1521, 0},
{1599, -5},
{1678, -10},
{1759, -15},
{1794, -20},
{1839, -25},
{1929, -30}
});
return Angle::degrees(t.lookup(str));
}
int Car::steering_for_angle(Angle theta)
{
static const LookupTable t(
{
{-30, 1929},
{-25, 1839},
{-20, 1794},
{-15,1759},
{-10, 1678},
{-5, 1599},
{0, 1521},
{5, 1461},
{10,1339},
{15,1306},
{20,1260},
{25,1175},
{30,1071}
});
return (int) t.lookup(theta.degrees());
}
int Car::esc_for_velocity(double v)
{
static const LookupTable t(
{
{-2., 1200},
{-1., 1250},
{-.1, 1326},
{0.0, 1500},
{0.1, 1620},
{1.5, 1663},
{3, 1724},
{4, 1764},
{7, 1887},
{9.5,1895},
{20, 2000}
});
return t.lookup(v);
/* old, ts table
static const LookupTable t(
{
{-2., 1200},
{-1., 1250},
{0.0, 1500},
{0.1, 1645},
{0.34, 1659},
{0.85, 1679},
{1.2, 1699},
{1.71, 1719},
{1.88, 1739},
{2.22, 1759},
{2.6, 1779},
{3.0, 1799},
{14.0, 2000}
});
return t.lookup(v);
*/
}