98 lines
2.1 KiB
Python
98 lines
2.1 KiB
Python
import machine
|
|
import utime, sys
|
|
from stm32_ssd1306 import SSD1306, SSD1306_I2C
|
|
from stm32_alphabot_v2 import AlphaBot_v2
|
|
import neopixel
|
|
import _thread
|
|
import os
|
|
import buzzer
|
|
|
|
import binascii
|
|
import uasyncio as asyncio
|
|
import RobotBleServer
|
|
|
|
from Interpreteur import StartCPU
|
|
|
|
motorSpeedFactor = 50
|
|
motorDCompensation = 1.04
|
|
|
|
alphabot = AlphaBot_v2()
|
|
oled = SSD1306_I2C(128, 64, alphabot.i2c)
|
|
|
|
oled.fill(0)
|
|
oled.show()
|
|
|
|
|
|
def motorCallback(motG, motD):
|
|
sMotG = 1 - ((motG >> 2) & 0b10)
|
|
sMotD = 1 - ((motD >> 2) & 0b10)
|
|
motG = ((motG & 0b0111) * sMotG) * motorSpeedFactor / 7
|
|
motD = (((motD & 0b0111) * sMotD) * motorSpeedFactor / 7) * motorDCompensation
|
|
print("Mot G :", motG)
|
|
print("Mot D :", motD)
|
|
alphabot.setMotors(left=motG, right=motD)
|
|
|
|
def registerCallback(register, value):
|
|
print(f"Register R{register} changed to {value}")
|
|
pass
|
|
|
|
|
|
|
|
# to know COM port used when connected on PC:
|
|
# python -m serial.tools.list_ports
|
|
|
|
# in this example, robot will send back to PC the checksum of each message received
|
|
|
|
|
|
robotName = 'Nogard'
|
|
toSend = []
|
|
|
|
def onMsgToRobot(data:str|bytes):
|
|
"""Function to call when a message sent by PC is received
|
|
:param data: message received"""
|
|
checksum = binascii.crc32(data)
|
|
print('received', data, '=>', checksum)
|
|
print(data)
|
|
|
|
StartCPU(data, motorCallback, registerCallback)
|
|
alphabot.stop()
|
|
|
|
|
|
async def robotMainTask(bleConnection):
|
|
"""Main function for robot activities
|
|
:param bleConnection: object to check BLE connection and send messages"""
|
|
while True:
|
|
await asyncio.sleep(0.1)
|
|
#print('connection', bleConnection.connection)
|
|
if not bleConnection.connection: continue
|
|
if toSend == []: continue
|
|
while not toSend == []:
|
|
data = toSend.pop(0)
|
|
bleConnection.sendMessage(data)
|
|
print('sent', data)
|
|
|
|
# Run tasks
|
|
async def main():
|
|
print('Start main')
|
|
bleConnection = RobotBleServer.RobotBleServer(robotName=robotName, onMsgReceived=onMsgToRobot)
|
|
asyncio.create_task(robotMainTask(bleConnection))
|
|
await bleConnection.communicationTask()
|
|
|
|
asyncio.run(main())
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|