really shitti figurine drawer

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)