Racing Bots on Pi Pico in MicroPython

race_bots.py

# Race car bots

from machine import Pin,SPI,PWM,ADC, Timer, reset, soft_reset
import framebuf, gc
import time, array, _thread
from time import sleep, ticks_us, ticks_diff, ticks_ms
from lcd_1_8 import LCD_1inch8
from random import randint
from sys import exit
from math import sin,cos,pi, radians
from uctypes import addressof
from car_bitmap import car_bitmap
from tiles5 import tiles
from race_texture import map_texture

MAXSCREEN_X = const(160)
MAXSCREEN_Y = const(128)
map_width = 6
map_height = 6
MAP_WIDTH = const(6)
MAP_HEIGHT = const(6)
MARGIN = const(50)
NUM_CARS = const(10)
NUM_PARM = const(6)
SCALE = const(12)
X     = const(0)
Y     = const(1)
DEG   = const(2)
SPEED = const(3)
MAP_X = const(4)
MAP_Y = const(5)

dark_grey  = const(0x4711) #pico_display.create_pen(20, 40, 60)
black      = const(0)      #pico_display.create_pen(0, 0, 0)
dark_green = const(0x0024) #pico_display.create_pen(32, 128, 0)
lt_blue    = const(0x3e44) #pico_display.create_pen(66, 133, 244)
lt_yellow  = const(0x33ff) #pico_display.create_pen(255, 229, 153)
lt_green   = const(0xe007) #pico_display.create_pen(0, 255, 0)
brown      = const(0xe079) #pico_display.create_pen(120, 63, 4)
white      = const(0xffff) #pico_display.create_pen(255, 255, 255)
sky_blue   = const(0x1f66) #pico_display.create_pen(96, 192, 255)
blue       = const(0x1f00)
red        = const(0xf8)
lt_brown   = const(0x52de)

colors = array.array('H',(black,dark_grey,red,white,black,black))
colors2 = array.array('H',(black,black,black,black,red,black,black,black,blue,black,black,black,black,black,black, sky_blue,blue,black))
colors3 = array.array('H',(white,black,red,lt_green, sky_blue,dark_green,dark_grey,sky_blue,blue,white,white,white,white,lt_green,dark_green, sky_blue,blue,black))

INTERP0_ACCUM0     = const(0x080)
INTERP0_ACCUM1     = const(0x084)
INTERP0_BASE0      = const(0x088)
INTERP0_BASE1      = const(0x08c)
INTERP0_BASE2      = const(0x090)
INTERP0_POP_FULL   = const(0x09c)
INTERP0_CTRL_LANE0 = const(0x0ac)
INTERP0_CTRL_LANE1 = const(0x0b0)

INTERP1_ACCUM0     = const(0x0c0)
INTERP1_ACCUM1     = const(0x0c4)
INTERP1_BASE0      = const(0x0c8)
INTERP1_BASE1      = const(0x0cc)
INTERP1_BASE2      = const(0x0d0)
INTERP1_POP_FULL   = const(0x0dc)
INTERP1_CTRL_LANE0 = const(0x0ec)
INTERP1_CTRL_LANE1 = const(0x0f0)

@micropython.viper
def interp_setup(map_width_bits:int, map_height_bits:int, uv_fractional_bits:int, tile_width_bits:int, tile_height_bits:int):
                           #                                             texture width
    sio=ptr32(0xd000_0000) #      shift                           raw   mask MSB    LSB
    sio[INTERP0_CTRL_LANE0//4] =    16                | 1<<18 |  (map_width_bits-1)<<10                  | 0<<5
    sio[INTERP0_CTRL_LANE1//4] =    16-map_width_bits | 1<<18 |  (map_width_bits+map_height_bits-1)<<10  | map_width_bits<<5
 
    sio[INTERP1_CTRL_LANE0//4] =    16-4              | 1<<18 |  3<<10  | 0<<5 #tile
    sio[INTERP1_CTRL_LANE1//4] =    16-8              | 1<<18 |  7<<10  | 4<<5
    du = 65536 // 16
    dv = 0    
    sio[INTERP0_BASE0//4]  = du
    sio[INTERP0_BASE1//4]  = dv                    
    sio[INTERP1_BASE0//4]  = du
    sio[INTERP1_BASE1//4]  = dv


SCREEN_CTL = const(0)
COLORS_CTL = const(4)
TILES_CTL  = const(8)
MAP_CTL    = const(12)
CAR_CTL    = const(16)
SIO_CTL    = const(20)

@micropython.asm_thumb
def draw_track_asm(r0):
    ldr(r1,[r0,CAR_CTL])
    ldr(r1,[r1,MAP_X*4])     # r1 : u = car_ptr[MAP_X]
    mov(r2,0)                # r2 : y
    label(LOOP_Y)
    ldr(r3,[r0,CAR_CTL])
    ldr(r3,[r3,MAP_Y*4])     # car_ptr[MAP_Y]
    asr(r3,r3,SCALE)         # car_ptr[MAP_Y]>>SCALE
    add(r3,r3,r2)            # car_ptr[MAP_Y]>>SCALE)+y
    lsl(r3,r3,12)            # r3 : v = ((car_ptr[MAP_Y]>>SCALE)+y)<<12
    ldr(r4,[r0,SIO_CTL])     # 0xd000_0080
    str(r1,[r4,0])           # sio[INTERP0_ACCUM0//4] = u
    str(r1,[r4,0x40])        # sio[INTERP1_ACCUM0//4] = u
    str(r3,[r4,0x4])         # sio[INTERP0_ACCUM1//4] = v
    str(r3,[r4,0x44])        # sio[INTERP1_ACCUM1//4] = v
    mov(r5,MAXSCREEN_X)
    mul(r5,r2)               # r5 : y_offset = y * MAXSCREEN_X
    mov(r6,0)                # r6 : x
    label(LOOP_X)
    ldr(r4,[r0,SIO_CTL])     # 0xd000_0080
    ldr(r7,[r4,0x1c])        # t = sio[INTERP0_POP_FULL//4] # map
    ldr(r3,[r0,MAP_CTL])     # map_ptr
    add(r3,r3,r7)            # [t]
    ldrb(r3,[r3,0])          # map_ptr[t]  
    lsl(r3,r3,8)             # map_ptr[t]<<8
    ldr(r7,[r4,0x5c])        # c = sio[INTERP1_POP_FULL//4] # tile
    add(r3,r3,r7)            # c+(map_ptr[t]<<8)
    ldr(r7,[r0,TILES_CTL])   # tiles_ptr
    add(r3,r3,r7)
    ldrb(r7,[r3,0])          # tiles_ptr[c+(map_ptr[t]<<8)]
    add(r7,r7,r7)            # double for ldrh
    ldr(r3,[r0,COLORS_CTL])  # colors_ptr
    add(r3,r3,r7)            #
    ldrh(r3,[r3,0])          # r3 : pixel = colors_ptr[tiles_ptr[c+(map_ptr[t]<<8)]]
    ldr(r7,[r0,SCREEN_CTL])  #
    add(r4,r5,r6)            # y_offset+x
    add(r4,r4,r4)            # double for strh
    add(r7,r7,r4)            # output[y_offset+x]   
    strh(r3,[r7,0])          # output[y_offset+x] = pixel
    add(r6,r6,1)
    cmp(r6,MAXSCREEN_X)
    blt(LOOP_X)
    add(r2,r2,1)
    cmp(r2,MAXSCREEN_Y)
    blt(LOOP_Y)   
    label(EXIT)
    

@micropython.viper
def draw_track():
    output     = ptr16(LCD.buffer)
    colors_ptr = ptr16(colors3)
    tiles_ptr  = ptr8(tiles)
    map_ptr    = ptr8(map_texture)
    sio        = ptr32(0xd000_0000)
    car_ptr    = ptr32(CAR)
    u  = car_ptr[MAP_X]  
    for y in range(1,MAXSCREEN_Y): 
        v = ((car_ptr[MAP_Y]>>SCALE)+y)<<12
        sio[INTERP0_ACCUM0//4] = u
        sio[INTERP1_ACCUM0//4] = u
        sio[INTERP0_ACCUM1//4] = v
        sio[INTERP1_ACCUM1//4] = v
        y_offset = y * MAXSCREEN_X
        for x in range(0,MAXSCREEN_X):
            t = sio[INTERP0_POP_FULL//4] # map
            c = sio[INTERP1_POP_FULL//4] # tile
            pixel = colors_ptr[tiles_ptr[c+(map_ptr[t]<<8)]]
            output[y_offset+x] = pixel

def init_isincos():           
    global isin,icos
    isin=array.array('i',range(0,360))
    icos=array.array('i',range(0,360))
    for i in range(0,360):
        isin[i]=int(sin(radians(i))*(1<<SCALE))
        icos[i]=int(cos(radians(i))*(1<<SCALE))
    

def init_joystick():
    global POT_X,POT_Y,POT_X_ZERO,POT_Y_ZERO
    POT_X = machine.ADC(27)
    POT_Y = machine.ADC(26)
    POT_X_ZERO = 0
    POT_Y_ZERO = 0
    for i in range(1000):
        POT_X_ZERO += POT_X.read_u16() 
        POT_Y_ZERO += POT_Y.read_u16()
    POT_X_ZERO = POT_X_ZERO//1000
    POT_Y_ZERO = POT_Y_ZERO//1000
    pot_scale = 12 

def init_car():
    global CAR,car_bitmap1
    CAR = array.array('i',())
    for i in range(NUM_CARS):
        CAR.append(randint(65,85)<<SCALE)   # X
        CAR.append(randint(55,65)<<SCALE)   # Y 
        CAR.append(0)                       # DEG
        CAR.append(+NUM_CARS-(i>>1))        # SPEED
        CAR.append(i<<15)                   # MAP_X
        CAR.append((7+ i % 4)<<16)          # MAP_Y
               
    car_bitmap1 = car_bitmap[:]
    for i in range(len(map_texture)):
        t=map_texture[i]
        if t == 0:
            map_texture[i] = 4
        else:
            map_texture[i] = t-1         
    for y in range(16):
        for x in range(16):
            car_bitmap[y*16+(15-x)] = car_bitmap1[x*16+y] # rotate bitmap
          
@micropython.viper
def autopilot():
    map_data = ptr8(map_texture)
    sine     = ptr32(isin)
    cosine   = ptr32(icos)
    screen   = ptr16(LCD.buffer)
    car_ptr  = ptr32(CAR)
    for i in range(NUM_CARS):
        #if i == 0 : continue
        index = i * NUM_PARM
        rot = car_ptr[DEG+index]
        speed = car_ptr[SPEED+index]
        inc_right = speed
        inc_left = speed
        rot_right = rot + 60
        rot_left = rot - 60
        if rot_right > 359: rot_right -= 360
        if rot_left < 0   : rot_left += 360        
        x   = car_ptr[MAP_X+index]+(5<<16)
        y   = car_ptr[MAP_Y+index]+(4<<16)                
        step = 120_000 # 120_000
        x_right = x + ((step * cosine[rot_right])>>SCALE)
        y_right = y + ((step * sine[rot_right])>>SCALE)
        x_left  = x + ((step * cosine[rot_left])>>SCALE)
        y_left  = y + ((step * sine[rot_left])>>SCALE)           
        x_c = (x>>16) & 63
        y_c = (y>>16) & 63
        x_r = (x_right>>16) & 63
        y_r = (y_right>>16) & 63
        x_l = (x_left>>16) & 63
        y_l = (y_left>>16) & 63        
        center = map_data[y_c*64+x_c]   
        right  = map_data[y_r*64+x_r]
        left   = map_data[y_l*64+x_l]       
        if left  == 4: rot -= inc_left
        if right == 4: rot += inc_right          
        if rot > 359: rot -= 360
        if rot < 0  : rot += 360    
        car_ptr[DEG+index] = rot
   
@micropython.viper 
def rot_viper(deg:int, width:int, height:int,source:ptr16,dest:ptr16): # 0-360 degrees, pixel width and height of bitmap
    sin=ptr32(isin)
    cos=ptr32(icos)
    offset_x = width//2
    offset_y = height//2
    cos_rad = cos[deg]
    sin_rad = sin[deg]
    for i in range(16*16):
        dest[i]=0
    y=height-1
    while y>=0: 
        x=width-1
        while x>=0:
            i=y*height+x
            color=source[i]
            if color or 1:
                adjusted_x = (x - offset_x)
                adjusted_y = (y - offset_y)
                qx = offset_x 
                qx+= (cos_rad * adjusted_x)>>SCALE
                qx+= (sin_rad * adjusted_y)>>SCALE
                if qx>=0 and qx<width:                
                    qy = offset_y
                    qy+= (sin_rad * adjusted_x)>>SCALE
                    qy-= (cos_rad * adjusted_y)>>SCALE
                    if qy>=0 and qy<height:
                        i = qy * width + qx
                        dest[i]=color
            x-=1
        y-=1

def read_joystick():
    global CAR
    pot_scale = 13
    x_inc = int(POT_X.read_u16() - POT_X_ZERO)>>pot_scale
    y_inc = int(POT_Y.read_u16() - POT_Y_ZERO)>>pot_scale
    if int(abs(x_inc))<2:
        x_inc=0
    if int(abs(y_inc))<2:
        y_inc=0
    if x_inc==0 and y_inc==0: return
    CAR[DEG] -= x_inc
    if CAR[DEG] > 359: CAR[DEG] -= 360
    if CAR[DEG] < 0  : CAR[DEG] += 360
    CAR[SPEED] -= y_inc
    if CAR[SPEED] > 10 : CAR[SPEED] = 10
    if CAR[SPEED] < 0  : CAR[SPEED] = 0    

@micropython.viper
def calc_move():
    car_ptr = ptr32(CAR)
    sin_ptr = ptr32(isin)
    cos_ptr = ptr32(icos)
    map_data = ptr8(map_texture)
    for i in range(NUM_CARS):
        index = i * NUM_PARM
        x = car_ptr[MAP_X+index]+(5<<16)
        y = car_ptr[MAP_Y+index]+(4<<16)
        x_c = (x>>16) & 63
        y_c = (y>>16) & 63       
        center = map_data[y_c*64+x_c]
        if center == 4:
            speed = car_ptr[SPEED+index]
        else:
            speed = car_ptr[SPEED+index]>>1    
        #if i>0: car_ptr[SPEED+index] = int(randint(19,21))
        x_inc = (cos_ptr[car_ptr[DEG+index]]*speed)>>2
        y_inc = (sin_ptr[car_ptr[DEG+index]]*speed)>>2
        new_x = car_ptr[MAP_X+index] + (x_inc<<1)
        new_y = car_ptr[MAP_Y+index] + (y_inc<<1)
        x1 = new_x
        y1 = new_y
        width = 1<<14
        height = 1<<14
        collision = 0
        for j in range(NUM_CARS):
            if i == j: continue # same car
            index_j = j * NUM_PARM
            x2 = car_ptr[MAP_X+index_j]
            y2 = car_ptr[MAP_Y+index_j]
            if x1 < x2 + width and x1 + width > x2 and y1 < y2 + height and y1 + height > y2:
                collision = 1
                break
        if collision == 0 :
            car_ptr[MAP_X+index] += x_inc<<1
            car_ptr[MAP_Y+index] += y_inc<<1
            if int(randint(0,5)) == 1:
                car_ptr[DEG+index] += int(randint(-1,1))*10
        else:
            car_ptr[DEG+index] += int(randint(-1,1))*20
            

@micropython.viper
def draw_car(fps):
    screen_ptr = ptr16(LCD.buffer)
    color_ptr  = ptr16(colors)
    color3_ptr  = ptr16(colors3)
    car_ptr2   = ptr16(car_bitmap1)
    car_pos_ptr= ptr32(CAR)
    width = 16
    height = 16
    car1_x = car_pos_ptr[MAP_X+0*NUM_PARM]
    car1_y = car_pos_ptr[MAP_Y+0*NUM_PARM]
    for i in range(NUM_CARS):
        index = i * NUM_PARM
        rot_viper(car_pos_ptr[DEG+index],16,16,car_bitmap,car_bitmap1)
        car_x = ((car_pos_ptr[MAP_X+index]-car1_x)>>12)+75
        car_y = ((car_pos_ptr[MAP_Y+index]-car1_y)>>12)+60
        for y in range(16):
            for x in range(16):
                car_pos = 16*y+x
                color = car_ptr2[car_pos]
                if color == 0xe027: color = color3_ptr[i]  # change car color
                if color:
                    x1 = x+car_x
                    y1 = y+car_y
                    if x1<160 and y1<128 and x1>=0 and y1>=0:
                        screen_ptr[160*y1+x1]= color
    #LCD.text(str(car_pos_ptr[MAP_X]>>16),30,0,0xff)
    #LCD.text(str(car_pos_ptr[MAP_Y]>>16),60,0,0xff)
    LCD.text(str(fps),0,0,0xff)
    LCD.show()
    LCD.fill(0)

@micropython.viper
def draw_tiles():
    screen_ptr = ptr16(LCD.buffer)
    tiles_ptr = ptr8(tiles)
    map_ptr = ptr8(map_texture)
    color_ptr = ptr16(colors3)
    car_pos_ptr= ptr32(CAR)
    map_x = car_pos_ptr[MAP_X]>>SCALE
    map_y = car_pos_ptr[MAP_Y]>>SCALE
    for ym in range(8):
        for xm in range(10):
            for y in range(16):
                for x in range(16):
                    addr = y*160+x + (ym*160*16 + xm*16)
                    t = (y*16+x)+map_ptr[(map_y+ym)*64+(map_x+xm)]*256
                    screen_ptr[addr] = color_ptr[tiles_ptr[t]]

#@micropython.viper
def main():
    global EXIT
    fps = 0
    old_deg = CAR[DEG]
    joystick_ticks = 0
    calc_move_ticks = 0
    debug_ticks = 0
    autopilot_ticks = 0
    while not EXIT:
        gticks = ticks_us()
        if int(ticks_diff(ticks_ms(),joystick_ticks)) > 1: 
            joystick_ticks = int(ticks_ms())
            read_joystick()
            if old_deg != CAR[DEG]:
                old_deg = CAR[DEG]
                #rot_viper(CAR[DEG],16,16,car_bitmap,car_bitmap1)
        if int(ticks_diff(ticks_ms(),calc_move_ticks)) > 30:   
            calc_move_ticks = int(ticks_ms())
            calc_move()
        if int(ticks_diff(ticks_ms(),debug_ticks)) > 1000:   
            debug_ticks = int(ticks_ms())
        #draw_track()
        #draw_tiles()
        if int(ticks_diff(ticks_ms(),autopilot_ticks)) > 30:  #50 
            autopilot_ticks = int(ticks_ms())    
            autopilot()
        draw_track_asm(track_ctl)
        draw_car(fps)
        fps = 1_000_000//int(ticks_diff(ticks_us(),gticks))
       
if __name__=='__main__':
    EXIT = 0
    interp_setup(MAP_WIDTH, MAP_HEIGHT, 16, 4, 4)
    init_joystick()
    init_isincos()
    init_car()
    FIRE = Pin(22, Pin.IN, Pin.PULL_UP)   
    POWER = Pin(16, machine.Pin.OUT)
    BUZZ = machine.PWM(Pin(17, machine.Pin.OUT))
    pwm = PWM(Pin(13))
    pwm.freq(1000)
    pwm.duty_u16(0x7fff)#max 65535    
    machine.freq(200_000_000)
    LCD = LCD_1inch8()
    machine.mem32[0x40008048] = 1<<11 # enable peri_ctrl clock
    track_ctl = array.array('I',(addressof(LCD.buffer),addressof(colors3),addressof(tiles),
                                 addressof(map_texture),addressof(CAR),0xd000_0080))
    gc.collect()
    print(gc.mem_free())
    main()
    draw_tiles()