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)