113 lines
4.0 KiB
Python
113 lines
4.0 KiB
Python
from math import cos, sin, pi, atan2, tan, dist, radians
|
|
|
|
class Robot:
|
|
# position
|
|
pos=(0.1,1)
|
|
# orientation
|
|
rot=-pi/2
|
|
# velocite
|
|
v=0
|
|
# velocite angulaire
|
|
angv=0
|
|
# velocite des roues (non angulaire)
|
|
wv=[0,0]
|
|
# velocite max
|
|
wmv=0.1
|
|
# espace entre les deux roues
|
|
l=0.1
|
|
# time
|
|
t = 0
|
|
# pins level
|
|
pins = [0]*16
|
|
# acceleration maximal avant de perdre l'adhérance
|
|
maxRotAccel = 10
|
|
# pins reliés au moteurs (bw_l, fw_l, bw_r, fw_r)
|
|
motors_pins = ((9,10),(12,11))
|
|
# position du spot lumineux
|
|
light_pos = (9, 1.5)
|
|
# ecart entre les deux photores
|
|
photores_l = 0.07
|
|
# longeur du séparateur entre les photorésistances
|
|
photores_sep_l = 0.6
|
|
# constantes de résistance des photoresistances
|
|
PHOTORES_LIGHT = 100
|
|
PHOTORES_SHADOW = 20000
|
|
PHOTORES_BRIDGE = 1000
|
|
pr_l_pin = 0 # A0 voir adapter_ard.h
|
|
pr_r_pin = 1 # A1
|
|
# rayon de mesure des capteurs ultrason
|
|
ultr_sense_max = 4
|
|
ultr_sense_rays = (pi/2, -pi/2)
|
|
ultr_sense_scatter = radians(6) # TODO: check
|
|
dists = [None] * len(ultr_sense_rays)
|
|
tr = [0]*len(ultr_sense_rays)
|
|
ci = [0]*len(tr)
|
|
|
|
def __init__(self, laby):
|
|
self.laby = laby
|
|
self.pr_max_ang = tan((self.photores_l/2)/self.photores_sep_l)
|
|
self.t = 0
|
|
self.dt = -1
|
|
self.angle2spot = 0
|
|
|
|
def update(self):
|
|
if(self.dt == -1):
|
|
return
|
|
self.update_ultr_sense()
|
|
self.compute_light()
|
|
print("***********", self.t, "****", self.dt)
|
|
# https://robotics.stackexchange.com/questions/106/what-is-a-suitable-model-for-two-wheeled-robots
|
|
self.v = 1/2*(self.wv[0]+self.wv[1])
|
|
self.angv = 1/self.l*(self.wv[1]-self.wv[0])
|
|
self.rot += self.angv*self.dt
|
|
self.pos = (self.pos[0]+self.v*sin(self.rot)*self.dt,self.pos[1]+self.v*cos(self.rot)*self.dt)
|
|
|
|
def compute_light(self):
|
|
a2s = atan2(self.light_pos[1]-self.pos[1],self.light_pos[0]-self.pos[0])
|
|
self.angle2spot = pi/2-a2s-self.rot%(2*pi)
|
|
if abs(self.angle2spot) > pi/2:
|
|
self.pr_l = self.PHOTORES_SHADOW
|
|
self.pr_r = self.PHOTORES_SHADOW
|
|
elif abs(self.angle2spot) < self.pr_max_ang:
|
|
self.pr_l = self.PHOTORES_LIGHT
|
|
self.pr_r = self.PHOTORES_LIGHT
|
|
else:
|
|
if self.angle2spot > 0:
|
|
self.pr_l = self.PHOTORES_LIGHT
|
|
self.pr_r = self.PHOTORES_SHADOW
|
|
else:
|
|
self.pr_l = self.PHOTORES_SHADOW
|
|
self.pr_r = self.PHOTORES_LIGHT
|
|
|
|
self.pins[self.pr_l_pin] = int(1024 * self.pr_l/(self.pr_l+self.PHOTORES_BRIDGE))
|
|
self.pins[self.pr_r_pin] = int(1024 * self.pr_r/(self.pr_r+self.PHOTORES_BRIDGE))
|
|
|
|
|
|
def analogWrite(self, pin, dc):
|
|
print(f">> set pin {pin} to {dc}")
|
|
speedDelta = (self.pins[pin] - dc) / self.dt
|
|
#assert(speedDelta < self.maxRotAccel)
|
|
self.pins[pin] = dc
|
|
for ind, mp in enumerate(self.motors_pins):
|
|
assert(bool(mp[0] > 0) ^ bool(mp[1] > 0), "deux pins moteur allumés en même temps")
|
|
self.wv[ind] = (-self.pins[mp[0]]+self.pins[mp[1]])/255*self.wmv
|
|
|
|
def digitalWrite(self, pin, level):
|
|
self.analogWrite(pin, level*255)
|
|
|
|
def setTime(self, t):
|
|
self.dt = (t/1000000 - self.t)
|
|
self.t = t/1000000
|
|
assert(self.dt < 0.1)
|
|
|
|
def update_ultr_sense(self):
|
|
for ind,r in enumerate(self.ultr_sense_rays):
|
|
#tr = (self.pos[0]+self.ultr_sense_max*sin(r+self.rot), self.pos[1]+self.ultr_sense_max*cos(r+self.rot))
|
|
#self.tr[ind] = tr
|
|
self.ci[ind] = self.laby.get_closest_wall_dist(self.pos, r+self.rot, self.ultr_sense_max, self.ultr_sense_scatter)
|
|
#self.ci[ind] = self.laby.get_closest_wall_dist((self.pos, tr))
|
|
if self.ci[ind]:
|
|
self.dists[ind] = dist(self.pos, self.ci[ind])
|
|
else:
|
|
self.dists[ind] = None
|