Better turning - Paladins-of-St-Pauls/StudentRobotics GitHub Wiki
Better turning:
def turnXX(degrees, speed, b, c):
t = (math.fabs(degrees) - b) / c
p = math.copysign(25,degrees)
set_power(p,-p)
R.sleep(t)
def turn25(degrees):
if math.fabs(degrees) >= 1:
turnXX(degrees, 25, -0.5404, 85.86)
def turn50(degrees):
if math.fabs(degrees) >= 4:
turnXX(degrees, 50, -1.597, 174.9)
else:
turn25(degrees)
def turn75(degrees):
if math.fabs(degrees) >= 15:
turnXX(degrees, 75, -9.562, 269.5)
else:
turn50(degrees)
def turn100(degrees):
if math.fabs(degrees) >= 25:
turnXX(degrees, 100, -20.15, 345.2)
else:
turn75(degrees)
There are four turning speeds 25, 50, 75 and 100. For small angles the higher speeds will fall-back to the lower speeds. Trying to turn less than a degree does nothing.
For completeness: Below is the test code which generated the parameters. The robot was turned for a fixed time, a number times each and then averaged. a range of fixed times was measured and a linear/polynomial regression applied. All the the power curves were well fitted (R-value close to 1) to a line and the linear regression gives the turning degrees in the form of y = mx + c. where y is degrees and x is time. re-arranging allows us to calculate time from degrees and this is encoded into the four functions above [ x = (y-c)/m]. The errors are larger for small angles at high speeds, so the function s automatically fall-back to lower speeds for low degrees values.
def test_rotation(sleep_time, power, iterations=100, heading_iterations=1000):
heading = get_heading(heading_iterations)
values = []
for i in range (iterations):
set_power(power,-power)
R.sleep(sleep_time)
stop()
old_heading = heading
heading = get_heading(heading_iterations)
diff = heading - old_heading
values.append(diff)
#unwrap the values
if power > 0:
values = [v if v > 0 else v+360 for v in values]
else:
values = [v if v < 0 else v-360 for v in values]
mean = statistics.mean(values)
stddev = statistics.stdev(values,mean)
return mean,stddev
def test_speed(speed, times):
rotations=[]
rotation_stddev=[]
for t in times:
rot, stdev = test_rotation(t,speed,10)
rotations.append(rot)
rotation_stddev.append(stdev)
print(f"Speed = {speed}")
print(f"rot = {rotations}")
print(f"rot_stddev = {rotation_stddev}")
m1 = numpy.poly1d(numpy.polyfit(times,rotations,1))
r1 = r2_score(rotations, m1(times))
print(f"Poly 1 - \n{m1} - R2 {r1}")
m2 = numpy.poly1d(numpy.polyfit(times,rotations,2))
r2 = r2_score(rotations, m2(times))
print(f"Poly 2 - \n{m2} - R2 {r2}")
m3 = numpy.poly1d(numpy.polyfit(times,rotations,3))
r3 = r2_score(rotations, m3(times))
print(f"Poly 3 - \n{m3} - R2 {r3}")
#test_speed(25,times = [i/100 for i in range(10,420,10)])
#test_speed(50,times = [i/100 for i in range(10,210,10)])
#test_speed(75,times = [i/100 for i in range(10,140,10)])
# test_speed(-100,times = [i/100 for i in range(5,110,5)])
print(f"heading = {get_heading(1000)}")
turn25(90)
print(f"heading = {get_heading(1000)}")
stop()
turn25(-90)
print(f"heading = {get_heading(1000)}")
stop()
# 1| Speed = 25
# 1| rot = [8.611056938363816, 16.917486291430023, 25.683866664659938, 33.89283786381079, 43.167987271013686, 50.653640504636265, 60.388313139380244, 68.04093144842815, 76.9062739658241, 84.92048235516572, 93.78456432611742, 102.27454068937867, 111.19768732594328, 119.16481922417377, 127.22758291368265, 136.15406489394084, 146.53891446386857, 154.43993452564283, 161.26524668576525, 169.91256505402416, 179.9811847701115, 188.03020827286622, 199.72909706059806, 204.73622680058148, 212.55679541174453, 222.10436740668683, 230.0071474159407, 240.7208134717624, 249.15059161546688, 256.8749211345896, 265.39274274578804, 271.7871162693676, 284.74770300161174, 289.9069846910776, 301.69959294462006, 308.69980208310403, 315.44951758043015, 325.9979700696409, 335.9596244030879, 344.03962341550374, 351.5551055868723]
# 1| rot_stddev = [0.2400373183266203, 0.21468509796746332, 0.5405217540663744, 0.7314326475705935, 0.830570735575057, 0.8099487691165012, 0.6962928248252233, 1.3272889398981929, 0.9277775309681372, 1.3440972035356626, 1.6353312162157443, 1.7335077394693799, 1.6222364845837405, 2.091281351548341, 1.7982890955379238, 2.6009788771859363, 2.2495184582532883, 2.6606012372207473, 2.4863228993556534, 3.2332921191523316, 2.84138094246828, 2.9087358727144244, 2.9741887198194137, 3.2063142509166944, 2.2961979512262856, 3.328984985231786, 3.5003338630401597, 4.065791521460981, 5.042283321236324, 3.2724816593540655, 2.7053697155119165, 4.066736853283281, 5.042860466540345, 4.659768208133594, 5.545227734521423, 4.0084506514577996, 5.907686216103333, 5.558211550625043, 6.840390873747399, 4.872002939045366, 3.235634078841093]
# 1| Poly 1 -
# 1|
# 1| 85.86 x - 0.5404 - R2 0.9998938359590247
# 1| Poly 2 -
# 1| 2
# 1| 0.2151 x + 84.96 x + 0.1071 - R2 0.9999008528470951
# 1| Poly 3 -
# 1| 3 2
# 1| 0.02372 x + 0.0657 x + 85.21 x + 0.01289 - R2 0.9999009445251877
# 1| Speed = 50
# 1| rot = [10.656031633665624, 33.70079139883913, 51.83327486812532, 69.0816355704793, 86.86929349362146, 103.29207500545921, 121.63636156820407, 138.68676186036794, 157.46228086873887, 174.21742579883113, 190.88685239522923, 208.78285529590576, 227.14636464332597, 242.314015219528, 261.9840795991805, 276.469713643908, 296.04236316459105, 311.2138580323337, 331.91636832431936, 346.2323052598228]
# 1| rot_stddev = [0.15010070083705865, 0.6315586378038234, 1.3623314687517016, 0.5941580787843427, 0.9023765227733962, 1.420785799815714, 1.534468052876554, 1.9709924848995444, 2.273615351261811, 1.4988807202966674, 2.2107275626975245, 2.299765804500417, 2.174483465899358, 2.784756185389699, 1.9975333135407392, 3.957619252937889, 3.4291155760676446, 4.832307159127082, 3.150768160919408, 4.165535679959867]
# 1| Poly 1 -
# 1|
# 1| 174.9 x - 1.597 - R2 0.9997468940018551
# 1| Poly 2 -
# 1| 2
# 1| -2.98 x + 181.1 x - 3.892 - R2 0.999823544887042
# 1| Poly 3 -
# 1| 3 2
# 1| 3.783 x - 14.9 x + 191.4 x - 5.902 - R2 0.9998545920126505
# 1| Speed = 75
# 1| rot = [10.678169715549013, 39.31409291708206, 75.87947556579118, 102.03822450823856, 129.0757992274523, 153.56860043397677, 181.26149935405434, 207.14109999035483, 234.61949077665756, 258.95684221031576, 284.93796285055544, 311.80753731696376, 339.2819834844519]
# 1| rot_stddev = [0.2707797701035612, 0.44358309777959426, 0.7109026204248657, 0.7535731931957086, 1.53757799917694, 1.3881895969173659, 1.8574417290920893, 2.9443586520600666, 1.7859651116575679, 3.061500059214956, 2.6711907583532684, 3.7771654778220474, 2.675673556710328]
# 1| Poly 1 -
# 1|
# 1| 269.5 x - 9.562 - R2 0.9989220922285521
# 1| Poly 2 -
# 1| 2
# 1| -19.27 x + 296.5 x - 16.31 - R2 0.9994834921124794
# 1| Poly 3 -
# 1| 3 2
# 1| 44.24 x - 112.2 x + 350.5 x - 23.74 - R2 0.9997880052860028
# 1| Speed = 100
# 1| rot = [3.0613948016909918, 10.694687927481619, 22.853837201105684, 39.249720216946656, 64.02672659144424, 88.86106331008237, 107.09874545355767, 122.59223643546422, 139.22047835938392, 154.86309269512952, 170.38087010311511, 186.29173797322284, 205.97166723636093, 222.02509589130634, 238.09064078087576, 254.35566380086547, 273.38963433188854, 289.89930110194956, 306.3673020859921, 322.91379167221936, 341.7256769074499]
# 1| rot_stddev = [0.13320408865041528, 0.2201450230767953, 0.27164697878155386, 0.3182651093603595, 0.6525417451188676, 0.5733016622737074, 0.49234123969851895, 0.907047681778783, 0.7313386644254385, 0.8580016618152748, 1.338959299226537, 1.2593898819998726, 0.8786660352933884, 0.9313861275613292, 0.7570022118586921, 0.8395101245538629, 0.49728304930752026, 0.44467501626075145, 0.6339227796674156, 0.8198494983262747, 1.4563620211597537]
# 1| Poly 1 -
# 1|
# 1| 345.2 x - 20.15 - R2 0.9984635572278128
# 1| Poly 2 -
# 1| 2
# 1| -16.12 x + 362.9 x - 23.55 - R2 0.9986221943274536
# 1| Poly 3 -
# 1| 3 2
# 1| 5.866 x - 25.8 x + 367.3 x - 23.99 - R2 0.9986236523838807