#'fernsteuerung-GY521-espnow-sender.py' - 2022-12-12
import network, espnow, time
from machine import Pin, SoftI2C

i2c = SoftI2C(sda=Pin(21), scl=Pin(22), freq=10000)

GY521_addr=0x68    # 0x68 = 104

i2c.start()
i2c.writeto(GY521_addr, bytearray([107, 0]))
i2c.stop()

def bytes_toint(byte1, byte2):
    wert = byte1 * 256 + byte2
    if wert>32768:
        wert = wert - 65536
    return wert

def sende_rohdaten():
    nachricht = kennung + "," + str(cnt) + "," +  daten
    print("Sende: "+ nachricht)
    e.send(peer, bytes(nachricht,""))
    
def sende_fahrbefehle():
    if abs(ac_z)<2000:
        print("Halt")
        eingabe="h"
    elif  ac_z>2000:
        if abs(ac_y)<1000:
            print("Vorwaerts")
            if ac_z<3000:
               print(" - 1. Gang")
               eingabe="f 666"
            elif ac_z<5000:
               print(" - 2. Gang")
               eingabe="f 888"
            else:
               print(" - 3. Gang")
               eingabe="f 1023"
        elif ac_y>1000:
            print("Linkskurve")
            eingabe="links"
        else:
            print("Rechtskurve")
            eingabe="rechts"
    else:
        print("Rueckwaerts")
        if ac_z>-3000:
            print(" - 1. Gang")
            eingabe="f -666"
        else:    
            print(" - 2. Gang")
            eingabe="f -888"
    nachricht = kennung + "," + str(cnt) + "," +  eingabe
    print("Sende: "+ nachricht)
    e.send(peer, bytes(nachricht,""))

wlan_sta = network.WLAN(network.STA_IF)
wlan_sta.active(True)

e = espnow.ESPNow()
e.active(True)

peer = b'\xff\xff\xff\xff\xff\xff'
e.add_peer(peer)

kennung = "RoboCar-max"
print("\n", kennung)
print(" an :", peer)
cnt = 0  # Zähler für gesendete Packete
while True:
    data = i2c.readfrom_mem(GY521_addr,0x3B,14)
    ac_x  = bytes_toint(data[0], data[1])
    ac_y  = bytes_toint(data[2], data[3])
    ac_z  = bytes_toint(data[4], data[5])
    
    gy_x  = bytes_toint(data[8], data[9])
    gy_y  = bytes_toint(data[10], data[11])
    gy_z  = bytes_toint(data[12], data[13])
    
    daten = f'{ac_x} {ac_y} {ac_z} {gy_x} {ac_y} {ac_z}'
    print(daten)
    #sende_rohdaten()
    sende_fahrbefehle()
    time.sleep(1.1)
    
    cnt+=1;