Skip to content

Instantly share code, notes, and snippets.

@flyudvik

flyudvik/chikibriki.py

Last active November 21, 2016 10:16
Show Gist options
  • Save flyudvik/bf9fb1eda88a6681a8d80185328ae11b to your computer and use it in GitHub Desktop.
Save flyudvik/bf9fb1eda88a6681a8d80185328ae11b to your computer and use it in GitHub Desktop.
Download ZIP
really shitti figurine drawer
Raw
chikibriki.py
#!/usr/bin/python
# импортирование необходимых модулей
from BrickPi import *
import time
import sys
import logging
################### CUSTOMIZABLE ###########################
# Все что может изменить пользователь. Это глобальные константы
FORMAT = "[%(asctime)s] %(message)s"
LONG_PRESS_NUMBER = 10
CLOCKWISE = 90
COUNTERCLOCKWISE = -90
MOTOR_LEFT = PORT_C
MOTOR_RIGHT = PORT_B
DEFAULT_SPEED = 200
DEFAULT_TIMEOUT = 3
ERROR_TIMEOUT = 10000
BUTTON = PORT_1
GYRO = PORT_4
################### /CUSTOMIZABLE ##########################
# установка логгирования
logging.basicConfig(format=FORMAT, level=logging.INFO)
log = logging.getLogger("squares")
# Запуск сетапа (инициализация подсистемы BrickPi)
log.info("Start of BrickPi setup")
BrickPiSetup()
log.info("end of BrickPi setup")
# Включение двигателей
BrickPi.MotorEnable[MOTOR_LEFT] = 1
BrickPi.MotorEnable[MOTOR_RIGHT] = 1
# Установка таймаута. Если робот не получает в течение этого времени новую команду, он выключается
BrickPi.Timeout = ERROR_TIMEOUT
BrickPiSetTimeout()
# включение сенсоров с указанием их типа
BrickPi.SensorType[BUTTON] = TYPE_SENSOR_EV3_TOUCH_DEBOUNCE
BrickPi.SensorType[GYRO] = TYPE_SENSOR_EV3_GYRO_M0
# инициализация сенсоров
log.info("Start of BrickPiSetupSensors")
BrickPiSetupSensors()
log.info("Start of BrickPiSetupSensors")
class StopException(Exception):
"""Пользовательское исключение"""
pass
def check_interrupt_presses(presses):
# функция для проверки количество сообщений о нажатии на кнопку.
# нужна для исключения интерференции и случайных сообщений об ошибке.
# платим за это более долгим нажатием на кнопку.
# При достижении нужного количества, выбрасываем исключение.
if presses >= LONG_PRESS_NUMBER:
log.debug("There were %d presses before release" % presses)
raise StopException
def dispatch_button_interrupt(presses=0):
""" anti interference hack. Still dirty tho.
If accidental presses exist, then increase LONG_PRESS_NUMBER"""
# Проверяем количество сообщений о нажатии
check_interrupt_presses(presses)
result = BrickPiUpdateValues()
if not result:
# если result == 0, то данные обновились успешно, поэтому выбираем данные
if BrickPi.Sensor[BUTTON] == 1:
return presses + 1
return 0
def timeout_with_dispatch_interrupt(timeout=DEFAULT_TIMEOUT):
presses = 0
ot = time.time() # получаем время старта
while time.time() - ot < timeout: # проверяем нажатие на кнопку пока не пройдет timeout секунд
presses = dispatch_button_interrupt(presses)
def forward(speed=DEFAULT_SPEED, timeout=DEFAULT_TIMEOUT):
log.info("Going forward")
BrickPi.MotorSpeed[MOTOR_RIGHT] = speed # установка скорости от -255 до 255
BrickPi.MotorSpeed[MOTOR_LEFT] = speed
timeout_with_dispatch_interrupt(timeout) # вызываем функцию, которая работает timeout секунду
# или пока не нажмут на кнопку
def backward(speed=DEFAULT_SPEED, timeout=DEFAULT_TIMEOUT):
log.info("calling backward")
# ехать назад, что и вперед только с отрицательным вектором скорости
forward(-speed, timeout)
def rotate(speed=DEFAULT_SPEED, angle=90, old_angle=0):
log.info("rotating from %d" % old_angle)
# функция разворота
assert angle % 360 != 0 # убрать развороты на 360 градусов. Это тупо
right = speed if angle > 0 else -speed # установка скорости левого и правого моторов
left = speed if angle < 0 else -speed # согласно углу поворота.
# Если поворот направо, логичнее всего запустить левый мотор вперед, а правый назад
BrickPi.MotorSpeed[MOTOR_RIGHT] = right
BrickPi.MotorSpeed[MOTOR_LEFT] = left
log.debug("... motor speed l=%d r=%d" % (left, right))
presses = 0
while True: # поворачиваем бесконечно, пока не случится определенное событие
check_interrupt_presses(presses) # проверить количество сигналов нажатия на кнопку
result = BrickPiUpdateValues() # проверить на успешность выполнения функции BrickPiUpdateValues()
if not result:
if BrickPi.Sensor[BUTTON]:
presses += 1
else:
presses = 0
new_angle = BrickPi.Sensor[GYRO] # получаем абсолютный угол поворота, относительно старта
delta_angle = abs(new_angle) - abs(old_angle) # считаем на сколько градусов повернулся робот
log.debug("new_angle = %d start_angle = %d delta_angle = %d" % (new_angle, old_angle, delta_angle))
if abs(delta_angle) > 360 or abs(new_angle) < 10 or \
(not (new_angle > 0 and old_angle > 0 or new_angle < 0 and old_angle < 0) and old_angle != 0):
# interference
# данное условие проверят на интерференцию. Гироскоп может выдавать неверные результаты,
# поэтому игнорируем их
continue
if abs(angle) - 3 <= abs(delta_angle) <= abs(angle) + 3 or abs(delta_angle) >= abs(angle):
log.debug("stopping rotating. angle %s reached " % str(delta_angle))
log.info("rotated from %d to %d for %d" % (old_angle, new_angle, delta_angle))
# если робот почти или уже достиг нужного угла, то завершить функцию поворота
# вернув абсолютный угол новой позиции
return new_angle
def circle(speed_left=DEFAULT_SPEED, speed_right=DEFAULT_SPEED/2, run_time=3, start_angle=0):
log.info("circle")
BrickPi.MotorSpeed[MOTOR_RIGHT] = speed_right
BrickPi.MotorSpeed[MOTOR_LEFT] = speed_left
presses = 0
while True:
check_interrupt_presses(presses)
result = BrickPiUpdateValues()
if not result:
if BrickPi.Sensor[BUTTON]:
presses += 1
else:
presses = 0
new_angle = BrickPi.Sensor[GYRO]
delta_angle = abs(new_angle) - abs(start_angle)
log.debug("Rotated for %d degrees" % delta_angle)
if 10 > abs(delta_angle) > 400 or abs(new_angle) < 10:
continue
if 350 <= abs(delta_angle) <= 370:
log.debug("Stopping circle. new angle = %d" % new_angle)
return new_angle
time.sleep(0.001)
def square(speed=DEFAULT_SPEED, time_forward=DEFAULT_TIMEOUT,
rotate_speed=DEFAULT_SPEED, direction=CLOCKWISE, start_angle=0):
log.info("Started square figurine")
# функция для прохождения пути по квадрату.
angle = start_angle # получаем начальный угол из аргументов
for _ in xrange(4): # цикл. сделать следующее действие 4 раза
forward(speed, time_forward) # идем вперед с определенной скоростью и определенное время
angle = rotate(rotate_speed, direction, angle) # поворачиваем от старого угла angle на указанный угол direction
# с определенной скоростью
log.info("Ended square figurine")
return angle # возвращаем абсолютный угол новой позиции
def polygon(number_of_edges=3, speed=DEFAULT_SPEED, time_forward=DEFAULT_TIMEOUT,
rotate_speed=DEFAULT_SPEED, direction=CLOCKWISE,
start_angle=0):
log.info("Start polygon with %d edges!!!" % number_of_edges)
angle = start_angle
rotate_for = int(360 / number_of_edges) # считаем угол для правильного многоугольника, по умолчанию - треугольник
rotate_for = rotate_for if direction is CLOCKWISE else -rotate_for # указываем знак угла,
# если поворачиваем на по часовой или противчасовой стрелки
for _ in xrange(number_of_edges): # цикл. выполняем следующее действие number_of_edges раз
forward(speed, time_forward) # идём вперед
angle = rotate(rotate_speed, rotate_for, angle) # поворачиваем на 360/количество углов
log.info("End polygon with %d edges!!!" % number_of_edges)
return angle
if __name__ == '__main__': # условие, при котором код ниже выполнится, если вызвать этот скрипт напрямую
try:
angle = 0 # начальный абсолютный угол всегда ноль
#################### user script
while True:
# angle = square(255, start_angle=angle, rotate_speed=150)
# forward()
angle = circle(start_angle=angle) # вызвать функцию прохождения по квадрату
# forward()
# backward()
###### можете писать любой свой код между этими блоками
except StopException: # обработка нашего исключения.
log.info("Stopped by pressing button")
except KeyboardInterrupt: # работу скрипта завершили
log.info("Stopped by user")
finally: # выполнить этот блок при завершении работы
log.info("Stopping motors")
BrickPi.MotorSpeed[MOTOR_LEFT] = 0
BrickPi.MotorSpeed[MOTOR_RIGHT] = 0
BrickPiUpdateValues()
time.sleep(0.1)
Sign up for free to join this conversation on GitHub. Already have an account? Sign in to comment