43 lines
1.2 KiB
Python
43 lines
1.2 KiB
Python
from math import cos, sin, pi
|
|
|
|
class Robot:
|
|
# position
|
|
pos=(0,1.5)
|
|
# orientation
|
|
rot=pi/2
|
|
# velocite
|
|
v=0
|
|
# velocite angulaire
|
|
angv=0
|
|
# velocite des roues (non angulaire)
|
|
wv=(0.1,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
|
|
|
|
def __init__(self, dt):
|
|
self.dt = dt
|
|
|
|
def update(self):
|
|
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)
|
|
print(self.pins)
|
|
|
|
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
|
|
|
|
def digitalWrite(self, pin, level):
|
|
self.analogWrite(pin, level*255)
|
|
|