最近、RaspberryPiを使ったロボットキットが安くなっていたので、Kumanロボットカーを買ってみた。

このロボットは、RaspberryPi 3B用との記載があったが、余っていたRaspberryPi 2で組み立て＆ソフトウェアの作成を行ってみた。

ソフトウェアのリポジトリは、こちら。

Joystickを情報を取るために、pygame を使ってみる。pygameを使うと、LinuxでもWindowsでも動作可能だった。

#! /usr/bin/env python
# -*- coding: utf-8 -*-

from __future__ import print_function
import pygame
from pygame.locals import *

#
#
def setup_joystick():
  pygame.joystick.init()
  try:
    j = pygame.joystick.Joystick(0) # create a joystick instance
    j.init() # init instance
    print( 'Joystick name: ' , j.get_name() )
    print( 'Num of buttons : ' , str(j.get_numbuttons()) )
    print( 'Num of sticks  : ' , str(j.get_numaxes()) )
    return j

  except pygame.error:
    print( 'Joystick not found.' )
    return None
    

#
# main function
def main(joy):
  loop_flag=True

  pygame.init()

  while loop_flag:
    pygame.time.wait(10)

    for e in pygame.event.get():
      if e.type == pygame.locals.JOYAXISMOTION: # Analogue Stick
        v=joy.get_axis(1) * 200
        lv = rv = -v
#        if abs(v) < 50 : rv=lv=0
#        elif abs(v) < 150 : rv=lv= -75*np.sign(v)
#        else : rv=lv= -150*np.sign(v)

        tsp=joy.get_axis(3) 
        if tsp < 0 :
          if rv > 0:
            rv += tsp*abs(rv)*2
          elif rv <0:
            rv -= tsp*abs(rv)*2
        if tsp > 0 :
          if lv > 0:
            lv -= tsp*abs(lv)*2
          elif lv <0:
            lv += tsp*abs(lv)*2

        print ( int(lv), int(rv) )


      elif e.type == pygame.locals.JOYHATMOTION: # cross key 
        hatval=joy.get_hat(0)
        if hatval == (0,0):
          pygame.time.set_timer(e.type, 0)
        else:
          pygame.time.set_timer(e.type, 60)
        print(hatval)
        

      elif e.type == pygame.locals.JOYBUTTONDOWN: # push a button
        print( str(e.button)+' down' )
        
      elif e.type == pygame.locals.JOYBUTTONUP: # release a button
        print( str(e.button)+' up' )
        if e.button == 0: loop_flag=False

      else:
        print( e )

if __name__ == '__main__':
  j=setup_joystick()
  if j :
    main(j)