def __init__(self):
self.i2c = I2C(2)
self.imu = BNO055(self.i2c)
#init position yaw, roll, pitch
self.init_yaw, self.init_roll, self.init_pitch = self.tare()
self.yaw = self.roll = self.pitch = 0
def __init__(self,
i2c: machine.I2C,
address: int = 40,
crystal=True,
sign: tuple = (0, 0, 0)):
"""Initialize the BNO055 from a given micropython machine.I2C connection
object, I2C device address, and an orientation sign integer 3-tuple.
"""
self.bno = BNO055(i2c, address=address, crystal=crystal, sign=sign)
def __init__(self, event_loop):
self.set_default(write=False)
super(VisualizerOrchestrator, self).__init__(event_loop)
bno055 = BNO055()
visualizer = Visualizer(enabled=True)
self.add_nodes(bno055, visualizer)
self.subscribe(bno055, visualizer, visualizer.bno055_tag)
def __init__(self):
GPIO.setwarnings(False)
#self.microgps = MicropyGPS(9,'dd')# MicroGPSオブジェクトを生成する。
self.bno055 = BNO055()
self.rightMotor = motor(19,26,5,6,13)
self.leftMotor = motor(8,7,16,20,12)
self.servomotor = servomotor(18)
self.bno055.setupBno()
self.LoRa = LoRa.LoRa()
GPIO.setmode(GPIO.BCM) #GPIOの設定
def __init__(self, refresh_rate=0.01):
# Initialize IMU sensor nd left/right wheel encoders
bno055 = BNO055(PERIOD_IMU, self, Sensor.KEY_BNO055)
left_wenc = Wheel_Encoder(False, PERIOD_WENC, self,
Sensor.KEY_WENC_LEFT)
right_wenc = Wheel_Encoder(True, PERIOD_WENC, self,
Sensor.KEY_WENC_RIGHT)
# Add all sensors
self.sensors = {
Sensor.KEY_BNO055: bno055,
Sensor.KEY_WENC_LEFT: left_wenc,
Sensor.KEY_WENC_RIGHT: right_wenc
}
# Set all sensors to "not ready" state
self.sensor_states = {
Sensor.KEY_BNO055: False,
Sensor.KEY_WENC_LEFT: False,
Sensor.KEY_WENC_RIGHT: False
}
# Set overall state to "not ready" until all individual sensors are ready
self.sensor_manager_state = False
# Stores latest measurement from each sensor in a dict
self.sensor_data = {
Sensor.KEY_BNO055: [],
Sensor.KEY_WENC_LEFT: [],
Sensor.KEY_WENC_RIGHT: []
}
self.sensor_recording_strings = {}
# Saves state to indicate if the sensors are current reading measurements
self.is_sensing = False
# Indicates if measurement readings will be printed out as they are being read
self.output_mode = {
Sensor.KEY_BNO055: True,
Sensor.KEY_WENC_LEFT: True,
Sensor.KEY_WENC_RIGHT: True
}
# DEPRECATED
self.refresh_rate = refresh_rate
# Observers that will receive measurement readings from sensors
self.observers = []
def soccer_walk(kondo, steps):
i2c = I2C(2)
imu = BNO055(i2c)
PROPORTION = 7
z,a1=kondo.rcb4.getUserParameter(19)
pyb.delay(100)
z,b1=kondo.rcb4.getUserParameter(20)
pyb.delay(100)
a=kondo.rcb4.setUserParameter(19,0)
pyb.delay(100)
a=kondo.rcb4.setUserParameter(20,0)
pyb.delay(100)
clock = time.clock()
yaw1, roll, pitch = imu.euler()
if yaw1 > 180: yaw1 = yaw1-360
# 22 фрейма исходящий шаг, 32 фрейма цикл, 22 фрейма заключительный шаг.
while( kondo.rcb4.getMotionPlayNum() !=0) : pyb.delay(1000)
a = kondo.rcb4.setUserParameter(1,0)
pyb.delay(100)
a = kondo.rcb4.setUserCounter( 1, steps )
pyb.delay(100)
a = kondo.rcb4.motionPlay(8)
clock.tick()
tim1 = 220+220+320*steps
while (clock.avg() < tim1):
pyb.delay(200)
yaw, roll, pitch = imu.euler()
if yaw > 180: yaw = yaw-360
correction = int(PROPORTION * ( yaw1 - yaw ))
if correction > 800 :
correction = 800
if correction < -800 :
correction = -800
a = kondo.rcb4.setUserParameter(1,correction)
pyb.delay(100)
a=kondo.rcb4.setUserParameter(19,a1)
pyb.delay(100)
a=kondo.rcb4.setUserParameter(20,b1)
pyb.delay(100)
def __init__(self, uart, pwm_freq):
self.uart = uart
self.i_state = array('i', [0] * len(INAMES))
self.f_state = array('f', [0] * len(FNAMES))
# controller timer
self.controller_timer = Timer(1)
# motor power control
self.nstby = Pin('NSTBY', mode=Pin.OUT_PP)
self.nstby.value(0)
# motors
motor_pwm_timer = Timer(8, freq=pwm_freq)
self.motor1 = TB6612(
motor_pwm_timer.channel(3, Timer.PWM_INVERTED, pin=Pin('PWM_A')),
Pin('AIN1', mode=Pin.OUT_PP), Pin('AIN2', mode=Pin.OUT_PP))
self.motor2 = TB6612(
motor_pwm_timer.channel(1, Timer.PWM_INVERTED, pin=Pin('PWM_B')),
Pin('BIN1', mode=Pin.OUT_PP), Pin('BIN2', mode=Pin.OUT_PP))
# encoders
self.enc1 = init_encoder(4, 'ENC_A1', 'ENC_A2', Pin.AF2_TIM4)
self.enc2 = init_encoder(3, 'ENC_B1', 'ENC_B2', Pin.AF2_TIM3)
# gyro
i2c = I2C(1, freq=400_000)
imu = BNO055(i2c, crystal=True, transpose=(2, 0, 1), sign=(0, 0, 1))
imu.mode(NDOF_FMC_OFF_MODE)
sleep_ms(800)
imu.iget(QUAT_DATA)
self.imu = imu
# pid controllers
self.pid = [
PID(1, 7, 50, 0, -100, 100),
PID(1, 7, 50, 0, -100, 100),
]
def __init__(self, i2c: machine.I2C, sign: tuple = (0, 0, 0)):
"""Initialize the BNO055 from a given micropython machine.I2C connection
object and an orientation sign integer 3-tuple.
"""
self.bno = BNO055(i2c, sign=(0, 0, 0))
def __init__(self, unix=False, controller=True, imu=True):
self.unix = unix
# RCB4 controller init
if controller:
from kondo_controller import Rcb4BaseLib
from pyb import UART
self.kondo = Rcb4BaseLib()
_uart = UART(1, 115200, parity=0, timeout=1000)
self.kondo.open(_uart)
print(self.kondo.checkAcknowledge())
else:
self.kondo = None
print('no controller mode')
# imu init
if imu:
from machine import I2C
from bno055 import BNO055, AXIS_P7
i2c = I2C(2)
self.imu = BNO055(i2c)
else:
self.imu = None
print('no imu mode')
# loading motion dictionary
#self.motions = json.load("/motion/data_motion.json")
self.motion_to_apply = None
self.motions = {
"Soccer_WALK_FF": {
"id":
8,
"time": (lambda c1, u1: 230 * c1 + 440),
"shift_x":
(lambda c1, u1: 4.3 / math.sin(u1 * 0.0140625 * math.pi / 180.)
* math.sin(u1 * 0.028125 * c1 * math.pi / 180.) / 100.),
"shift_y": (lambda c1, u1: -4.3 / math.sin(
u1 * 0.0140625 * math.pi / 180.) / 100 + 4.3 / math.sin(
u1 * 0.0140625 * math.pi / 180.) * math.cos(
u1 * 0.028125 * c1 * math.pi / 180.) / 100.),
"shift_turn": (lambda c1, u1: u1 * 0.028125)
},
"Soccer_Turn": {
"id": 15,
"time": (lambda c1, u1: 220 * c1 + 80),
"shift_x": (lambda c1, u1: 0),
"shift_y": (lambda c1, u1: 0),
"shift_turn": (lambda c1, u1: -1 * u1 * 0.12 * c1)
},
"Soccer_Side_Step_Left": {
"id": 11,
"time": (lambda c1, u1: 180 * c1 + 360),
"shift_x": (lambda c1, u1: 0),
"shift_y": (lambda c1, u1: 11. * c1 / 100.),
"shift_turn": (lambda c1, u1: 0)
},
"Soccer_Side_Step_Right": {
"id": 10,
"time": (lambda c1, u1: 180 * c1 + 360),
"shift_x": (lambda c1, u1: 0),
"shift_y": (lambda c1, u1: -11. * c1 / 100.),
"shift_turn": (lambda c1, u1: 0)
},
"Soccer_Small_Step_Left": {
"id": 13,
"time": (lambda c1, u1: 1250 * c1 + 150),
"shift_x": (lambda c1, u1: 0),
"shift_y": (lambda c1, u1: 3.3 * c1 / 100.),
"shift_turn": (lambda c1, u1: 0)
},
"Soccer_Small_Step_Right": {
"id": 12,
"time": (lambda c1, u1: 1250 * c1 + 150),
"shift_x": (lambda c1, u1: 0),
"shift_y": (lambda c1, u1: -3.3 * c1 / 100.),
"shift_turn": (lambda c1, u1: 0)
},
"Soccer_Take_Around_Left": {
"id":
17,
"time": (lambda c1, u1: 1250 * c1 + 150),
"shift_x":
(lambda c1, u1: 1.65 / math.sin(6.3 * math.pi / 180) *
(1 - math.cos(12.6 * c1 * math.pi / 180)) / 100.),
"shift_y": (lambda c1, u1: 1.65 / math.sin(6.3 * math.pi / 180)
* math.sin(12.6 * c1 * math.pi / 180) / 100.),
"shift_turn": (lambda c1, u1: 12.6 * c1)
},
"Soccer_Take_Around_Right": {
"id":
16,
"time": (lambda c1, u1: 1250 * c1 + 150),
"shift_x":
(lambda c1, u1: 1.65 / math.sin(6.3 * math.pi / 180) *
(1 - math.cos(12.6 * c1 * math.pi / 180)) / 100.),
"shift_y":
(lambda c1, u1: -1.65 / math.sin(6.3 * math.pi / 180) * math.
sin(12.6 * c1 * math.pi / 180) / 100.),
"shift_turn": (lambda c1, u1: -12.6 * c1)
},
"Soccer_Kick_Forward_Left_leg": {
"id": 19,
"time": (lambda c1, u1: 1000),
"shift_x": (lambda c1, u1: 0),
"shift_y": (lambda c1, u1: 0),
"shift_turn": (lambda c1, u1: 0)
},
"Soccer_Kick_Forward_Right_leg": {
"id": 18,
"time": (lambda c1, u1: 1000),
"shift_x": (lambda c1, u1: 0),
"shift_y": (lambda c1, u1: 0),
"shift_turn": (lambda c1, u1: 0)
},
"Soccer_HomePosition": {
"id": 1,
"time": (lambda c1, u1: 1000),
"shift_x": (lambda c1, u1: 0),
"shift_y": (lambda c1, u1: 0),
"shift_turn": (lambda c1, u1: 0)
},
"Soccer_Get_Ready": {
"id": 2,
"time": (lambda c1, u1: 1000),
"shift_x": (lambda c1, u1: 0),
"shift_y": (lambda c1, u1: 0),
"shift_turn": (lambda c1, u1: 0)
},
"Free": {
"id": 4,
"time": (lambda c1, u1: 1000),
"shift_x": (lambda c1, u1: 0),
"shift_y": (lambda c1, u1: 0),
"shift_turn": (lambda c1, u1: 0)
},
"Move_Head": {
"id": 112,
"time": (lambda c1, u1: 600),
"shift_x": (lambda c1, u1: 0),
"shift_y": (lambda c1, u1: 0),
"shift_turn": (lambda c1, u1: 0)
},
"Empty": {
"id": 100,
"time": (lambda c1, u1: 0),
"shift_x": (lambda c1, u1: 0),
"shift_y": (lambda c1, u1: 0),
"shift_turn": (lambda c1, u1: 0)
}
}
# timer for motions
self._motion_duration = 0.0
self._motion_start_time = 0.0
# head init
with open("motion/head_motion.json", "r") as f:
self.head_motion_states = json.loads(f.read())
self.head_state_num = len(self.head_motion_states)
self.head_enabled = True
self.head_pan = 0
self.head_tilt = 0
self.head_state = -1
# get config params
with open("motion/config.json", "r") as f:
self.motion_config = json.loads(f.read())
# walk threshold
self.walk_threshold = self.motion_config['walk_threshold']
self.max_blind_distance = self.motion_config['max_blind_distance']
self.step_len = self.motion_config['step_len']
# acceptable error in degrees
self.angle_error_treshold = self.motion_config[
'angle_error_treshold'] * math.pi / 180.
# This module must export the following functions # fill(color) Fill the buffer with a color # line(xs, ys, xe, ye, color) Draw a line to the buffer # show() Display result # Also dimension bound variable. from ssd1351_16bit import SSD1351 as SSD _HEIGHT = const(128) # SSD1351 variant in use # IMU driver for test_imu only. With other IMU's the fusion module # may be used for quaternion output. # https://github.com/micropython-IMU/micropython-fusion from bno055 import BNO055 # Initialise IMU i2c = I2C(2) imu = BNO055(i2c) # Export color constants WHITE = SSD.rgb(255, 255, 255) GREY = SSD.rgb(100, 100, 100) GREEN = SSD.rgb(0, 255, 0) BLUE = SSD.rgb(0, 0, 255) RED = SSD.rgb(255, 0, 0) YELLOW = SSD.rgb(255, 255, 0) CYAN = SSD.rgb(0, 255, 255) # Initialise display # Monkey patch size of square viewing area. No. of pixels for a change of 1.0 # Viewing area is 128*128 DIMENSION = 64