pi.py

import serial
import time
from math import *
from src.Mouse import Mouse
import io
from multiprocessing import Process
import asyncio


# position, orientation robot, distance robot-target, position target, vitesse moteur
x = 0
y = 0
ort = 0
d = 0
posT = [0, 0]
Vmoy = 0

# position départ, position target VIOLET / JAUNE
startPosV = [0, 0]
posTV = [0, 0]
startPosJ = [0, 0]
posTJ = [0, 10]

# Seuils (distance a d avant arret + distance a d avant ralentir)
dBfrStop = 1
dBfrDecrSpeed = 10

# truc pour le serial
ser = serial.Serial(
    port='/dev/ttyUSB0',
    baudrate=9600,
    timeout=1
    # parity=serial.PARITY_ODD,
    # stopbits=serial.STOPBITS_TWO,
    # bytesize=serial.SEVENBITS
)


async def init():
    print('init start')
    canGo = False
    verifyCover = False
	# ser.write(b"LIGHT\n")
    team("violet")
    goto(posT[0], posT[1])
#     while (verifyCover == False):
#        a = 1+1  # pour pa un while vide
#        #possibilité de définition de l'équipe
#        #qqc pour qu'il commence a verifier si il est couvert
#    """ while (canGo == False):
#        canGo = checkCover()  # qui renvoi donc true si c pa couvert
#    ser.write("ELEC") """""" 
#    goto(posT[0], posT[1]) """


# definition des variables de départ en fonction de l'équipe
def team(team):
    if (team == "violet"):
        x = startPosV[0]
        y = startPosV[1]
        posT = posTV
    if (team == "jaune"):
        x = startPosJ[0]
        y = startPosJ[1]
        posT = posTJ


def goto(xt, yt):
    s = getS(xt, yt)
    d = getD(xt, yt)
    Vmoy = 200  # change la vitesse de rotation
    ort = getGyro()
    #tant qu'il n'est pas bien orienté, il tourne (lol, tu voulais qu'il recule?)
    while (abs(ort - s) > 1):
        if (0 < ort-s <= pi | ort-s < -pi):
            setMotors(0.8*Vmoy, 1.2*Vmoy)
        else:
            setMotors(1.2 * Vmoy, 0.8 * Vmoy)
        ort = getGyro()
    time.sleep(1)
    mouse.reset()
    dBfrStop = d - 1
    dBfrDecrSpeed = d - 10
    while (mouse.getPosition()[1] > dBfrStop):
        #et en plus, tant qu'il est assez loin, il va vite
        if (mouse.getPosition()[1] > dBfrDecrSpeed):
            Vmoy = 200
        else:
            Vmoy = 120
        setMotors(Vmoy, Vmoy)

#definir vitesse moteur
def setMotors(Vd, Vg):
    ser.write(b"TWICE#", Vg, "#", Vd, '\n')


def checkCover():
    ser.write(b"ISCOVER\n")
    return()  # true si c pa couvert

#obtenir distance robo/target


def getD(xt, yt):
    return(sqrt((xt-x)*(xt-x)+(yt-y)*(yt-y)))
#obtenir orientation


def getS(xt, yt):
    return(atan2(yt-y, xt-x))
#gyro


def getGyro():
    ser.write(b"ANGLEZ\n")
    time.sleep(0.1)
    return(ser.read(1))

def elec():
    while(True):
        ser.write(b"ELEC\n")
        time.sleep(2)


#ser.write(b"TWICE#200#100\n")
elec()
print('r: ', ser.readline())

# while(True):
#     ser.write(b"ANGLEZ\n")
#     print('r: ', ser.readline())
#     time.sleep(0.2)

mouse = Mouse()

loop = asyncio.get_event_loop()
loop.run_until_complete(asyncio.gather(mouse.start(), init()))
loop.close()