<commit_before>import logging
import logging.config
import sys
import threading
import traceback
import math
import time

import serial
import os

from amberdriver.common.message_handler import MessageHandler
from amberdriver.null.null import NullController
from amberdriver.roboclaw import roboclaw_pb2
from amberdriver.roboclaw.roboclaw import Roboclaw
from amberdriver.tools import serial_port, config


__author__ = 'paoolo'

pwd = os.path.dirname(os.path.abspath(__file__))
logging.config.fileConfig('%s/roboclaw.ini' % pwd)
config.add_config_ini('%s/roboclaw.ini' % pwd)

LOGGER_NAME = 'RoboclawController'

SERIAL_PORT = str(config.ROBOCLAW_SERIAL_PORT)
BAUD_RATE = int(config.ROBOCLAW_BAUD_RATE)

REAR_RC_ADDRESS = int(config.ROBOCLAW_REAR_RC_ADDRESS)
FRONT_RC_ADDRESS = int(config.ROBOCLAW_FRONT_RC_ADDRESS)

MOTORS_MAX_QPPS = int(config.ROBOCLAW_MOTORS_MAX_QPPS)
MOTORS_P_CONST = int(config.ROBOCLAW_P_CONST)
MOTORS_I_CONST = int(config.ROBOCLAW_I_CONST)
MOTORS_D_CONST = int(config.ROBOCLAW_D_CONST)

WHEEL_RADIUS = float(config.ROBOCLAW_WHEEL_RADIUS)
PULSES_PER_REVOLUTION = float(config.ROBOCLAW_PULSES_PER_REVOLUTION)

STOP_IDLE_TIMEOUT = float(config.ROBOCLAW_STOP_IDLE_TIMEOUT)
RESET_IDLE_TIMEOUT = float(config.ROBOCLAW_RESET_IDLE_TIMEOUT)

BATTERY_MONITOR_INTERVAL = float(config.ROBOCLAW_BATTERY_MONITOR_INTERVAL)
ERROR_MONITOR_INTERVAL = float(config.ROBOCLAW_ERROR_MONITOR_INTERVAL)
CRITICAL_READ_REPEATS = int(config.ROBOCLAW_CRITICAL_READ_REPEATS)

RESET_DELAY = float(config.ROBOCLAW_RESET_DELAY)
RESET_GPIO_PATH = str(config.ROBOCLAW_RESET_GPIO_PATH)

TEMPERATURE_MONITOR_INTERVAL = float(config.ROBOCLAW_TEMPERATURE_MONITOR_INTERVAL)
TEMPERATURE_CRITICAL = float(config.ROBOCLAW_TEMPERATURE_CRITICAL)
TEMPERATURE_DROP = float(config.ROBOCLAW_TEMPERATURE_DROP)

TIMEOUT = 0.3


class RoboclawController(MessageHandler):
    def __init__(self, pipe_in, pipe_out, driver):
        MessageHandler.__init__(self, pipe_in, pipe_out)
        self.__driver = driver
        self.__logger = logging.getLogger(LOGGER_NAME)

    def handle_data_message(self, header, message):
        if message.HasExtension(roboclaw_pb2.currentSpeedRequest):
            self.__handle_current_speed_request(header, message)

        elif message.HasExtension(roboclaw_pb2.motorsCommand):
            self.__handle_motors_command(header, message)

        else:
            self.__logger.warning('No request in message')

    @MessageHandler.handle_and_response
    def __handle_current_speed_request(self, received_header, received_message, response_header, response_message):
        self.__logger.debug('Get current speed')

        front_left, front_right, rear_left, rear_right = self.__driver.get_measured_speeds()

        current_speed = response_message.Extensions[roboclaw_pb2.currentSpeed]
        current_speed.frontLeftSpeed = int(front_left)
        current_speed.frontRightSpeed = int(front_right)
        current_speed.rearLeftSpeed = int(rear_left)
        current_speed.rearRightSpeed = int(rear_right)

        return response_header, response_message

    def __handle_motors_command(self, _, message):
        self.__logger.debug('Set speed')

        front_left = message.Extensions[roboclaw_pb2.motorsCommand].frontLeftSpeed
        front_right = message.Extensions[roboclaw_pb2.motorsCommand].frontRightSpeed
        rear_left = message.Extensions[roboclaw_pb2.motorsCommand].rearLeftSpeed
        rear_right = message.Extensions[roboclaw_pb2.motorsCommand].rearRightSpeed

        self.__driver.set_speeds(front_left, front_right, rear_left, rear_right)

    def handle_subscribe_message(self, header, message):
        self.__logger.debug('Subscribe action for %s', str(header.clientIDs))

    def handle_unsubscribe_message(self, header, message):
        self.__logger.debug('Unsubscribe action for %s', str(header.clientIDs))

    def handle_client_died_message(self, client_id):
        self.__logger.info('Client %d died, stop!', client_id)
        self.__driver.stop()


def to_mmps(val):
    return int(val * WHEEL_RADIUS * math.pi * 2.0 / PULSES_PER_REVOLUTION)


def to_qpps(val):
    rps = val / (WHEEL_RADIUS * math.pi * 2.0)
    return int(rps * PULSES_PER_REVOLUTION)


class RoboclawDriver(object):
    def __init__(self, front, rear):
        self.__controller = None

        self.__front, self.__rear = front, rear
        self.__roboclaw_lock = threading.Lock()

        self.__timeout_lock = threading.Lock()
        self.__motors_stop_timer_enabled, self.__battery_low = False, False
        self.__reset_time, self.__motors_stop_time = 0.0, 0.0

        self.__reset_gpio = open(RESET_GPIO_PATH, mode='w')

        self.__overheated = False
        self.__roboclaw_disabled = False

        self.__logger = logging.getLogger(LOGGER_NAME)

    def set_controller(self, controller):
        self.__controller = controller

    def get_measured_speeds(self):
        if self.__roboclaw_disabled:
            return 0, 0, 0, 0

        self.__roboclaw_lock.acquire()
        try:
            front_right, _ = self.__front.read_speed_m1()
            front_left, _ = self.__front.read_speed_m2()
            rear_right, _ = self.__rear.read_speed_m1()
            rear_left, _ = self.__rear.read_speed_m2()
        finally:
            self.__roboclaw_lock.release()

        front_left = to_mmps(front_left)
        front_right = to_mmps(front_right)
        rear_left = to_mmps(rear_left)
        rear_right = to_mmps(rear_right)

        return front_left, front_right, rear_left, rear_right

    def set_speeds(self, front_left, front_right, rear_left, rear_right):
        if self.__roboclaw_disabled:
            return

        front_left = to_qpps(front_left)
        front_right = to_qpps(front_right)
        rear_left = to_qpps(rear_left)
        rear_right = to_qpps(rear_right)

        self.__reset_timeouts()

        self.__roboclaw_lock.acquire()
        try:
            self.__front.drive_m1_with_signed_speed(front_right)
            self.__front.drive_m2_with_signed_speed(front_left)
            self.__rear.drive_m1_with_signed_speed(rear_right)
            self.__rear.drive_m2_with_signed_speed(rear_left)
        finally:
            self.__roboclaw_lock.release()

    def stop(self):
        self.__roboclaw_lock.acquire()
        try:
            self.__front.drive_mixed_with_signed_speed(0, 0)
            self.__rear.drive_mixed_with_signed_speed(0, 0)
        finally:
            self.__roboclaw_lock.release()

    def __reset(self):
        self.__reset_gpio.write(str('\0'))
        time.sleep(0.5)
        self.__reset_gpio.write(str('\1'))

    def setup(self):
        self.__front.set_pid_constants_m1(MOTORS_P_CONST, MOTORS_I_CONST, MOTORS_D_CONST, MOTORS_MAX_QPPS)
        self.__front.set_pid_constants_m2(MOTORS_P_CONST, MOTORS_I_CONST, MOTORS_D_CONST, MOTORS_MAX_QPPS)
        self.__rear.set_pid_constants_m1(MOTORS_P_CONST, MOTORS_I_CONST, MOTORS_D_CONST, MOTORS_MAX_QPPS)
        self.__rear.set_pid_constants_m2(MOTORS_P_CONST, MOTORS_I_CONST, MOTORS_D_CONST, MOTORS_MAX_QPPS)

        self.__front.set_m1_encoder_mode(0)<commit_msg>Fix reset command for GPIO<commit_after>import logging
import logging.config
import sys
import threading
import traceback
import math
import time

import serial
import os

from amberdriver.common.message_handler import MessageHandler
from amberdriver.null.null import NullController
from amberdriver.roboclaw import roboclaw_pb2
from amberdriver.roboclaw.roboclaw import Roboclaw
from amberdriver.tools import serial_port, config


__author__ = 'paoolo'

pwd = os.path.dirname(os.path.abspath(__file__))
logging.config.fileConfig('%s/roboclaw.ini' % pwd)
config.add_config_ini('%s/roboclaw.ini' % pwd)

LOGGER_NAME = 'RoboclawController'

SERIAL_PORT = str(config.ROBOCLAW_SERIAL_PORT)
BAUD_RATE = int(config.ROBOCLAW_BAUD_RATE)

REAR_RC_ADDRESS = int(config.ROBOCLAW_REAR_RC_ADDRESS)
FRONT_RC_ADDRESS = int(config.ROBOCLAW_FRONT_RC_ADDRESS)

MOTORS_MAX_QPPS = int(config.ROBOCLAW_MOTORS_MAX_QPPS)
MOTORS_P_CONST = int(config.ROBOCLAW_P_CONST)
MOTORS_I_CONST = int(config.ROBOCLAW_I_CONST)
MOTORS_D_CONST = int(config.ROBOCLAW_D_CONST)

WHEEL_RADIUS = float(config.ROBOCLAW_WHEEL_RADIUS)
PULSES_PER_REVOLUTION = float(config.ROBOCLAW_PULSES_PER_REVOLUTION)

STOP_IDLE_TIMEOUT = float(config.ROBOCLAW_STOP_IDLE_TIMEOUT)
RESET_IDLE_TIMEOUT = float(config.ROBOCLAW_RESET_IDLE_TIMEOUT)

BATTERY_MONITOR_INTERVAL = float(config.ROBOCLAW_BATTERY_MONITOR_INTERVAL)
ERROR_MONITOR_INTERVAL = float(config.ROBOCLAW_ERROR_MONITOR_INTERVAL)
CRITICAL_READ_REPEATS = int(config.ROBOCLAW_CRITICAL_READ_REPEATS)

RESET_DELAY = float(config.ROBOCLAW_RESET_DELAY)
RESET_GPIO_PATH = str(config.ROBOCLAW_RESET_GPIO_PATH)

TEMPERATURE_MONITOR_INTERVAL = float(config.ROBOCLAW_TEMPERATURE_MONITOR_INTERVAL)
TEMPERATURE_CRITICAL = float(config.ROBOCLAW_TEMPERATURE_CRITICAL)
TEMPERATURE_DROP = float(config.ROBOCLAW_TEMPERATURE_DROP)

TIMEOUT = 0.3


class RoboclawController(MessageHandler):
    def __init__(self, pipe_in, pipe_out, driver):
        MessageHandler.__init__(self, pipe_in, pipe_out)
        self.__driver = driver
        self.__logger = logging.getLogger(LOGGER_NAME)

    def handle_data_message(self, header, message):
        if message.HasExtension(roboclaw_pb2.currentSpeedRequest):
            self.__handle_current_speed_request(header, message)

        elif message.HasExtension(roboclaw_pb2.motorsCommand):
            self.__handle_motors_command(header, message)

        else:
            self.__logger.warning('No request in message')

    @MessageHandler.handle_and_response
    def __handle_current_speed_request(self, received_header, received_message, response_header, response_message):
        self.__logger.debug('Get current speed')

        front_left, front_right, rear_left, rear_right = self.__driver.get_measured_speeds()

        current_speed = response_message.Extensions[roboclaw_pb2.currentSpeed]
        current_speed.frontLeftSpeed = int(front_left)
        current_speed.frontRightSpeed = int(front_right)
        current_speed.rearLeftSpeed = int(rear_left)
        current_speed.rearRightSpeed = int(rear_right)

        return response_header, response_message

    def __handle_motors_command(self, _, message):
        self.__logger.debug('Set speed')

        front_left = message.Extensions[roboclaw_pb2.motorsCommand].frontLeftSpeed
        front_right = message.Extensions[roboclaw_pb2.motorsCommand].frontRightSpeed
        rear_left = message.Extensions[roboclaw_pb2.motorsCommand].rearLeftSpeed
        rear_right = message.Extensions[roboclaw_pb2.motorsCommand].rearRightSpeed

        self.__driver.set_speeds(front_left, front_right, rear_left, rear_right)

    def handle_subscribe_message(self, header, message):
        self.__logger.debug('Subscribe action for %s', str(header.clientIDs))

    def handle_unsubscribe_message(self, header, message):
        self.__logger.debug('Unsubscribe action for %s', str(header.clientIDs))

    def handle_client_died_message(self, client_id):
        self.__logger.info('Client %d died, stop!', client_id)
        self.__driver.stop()


def to_mmps(val):
    return int(val * WHEEL_RADIUS * math.pi * 2.0 / PULSES_PER_REVOLUTION)


def to_qpps(val):
    rps = val / (WHEEL_RADIUS * math.pi * 2.0)
    return int(rps * PULSES_PER_REVOLUTION)


class RoboclawDriver(object):
    def __init__(self, front, rear):
        self.__controller = None

        self.__front, self.__rear = front, rear
        self.__roboclaw_lock = threading.Lock()

        self.__timeout_lock = threading.Lock()
        self.__motors_stop_timer_enabled, self.__battery_low = False, False
        self.__reset_time, self.__motors_stop_time = 0.0, 0.0

        self.__reset_gpio = open(RESET_GPIO_PATH, mode='w')

        self.__overheated = False
        self.__roboclaw_disabled = False

        self.__logger = logging.getLogger(LOGGER_NAME)

    def set_controller(self, controller):
        self.__controller = controller

    def get_measured_speeds(self):
        if self.__roboclaw_disabled:
            return 0, 0, 0, 0

        self.__roboclaw_lock.acquire()
        try:
            front_right, _ = self.__front.read_speed_m1()
            front_left, _ = self.__front.read_speed_m2()
            rear_right, _ = self.__rear.read_speed_m1()
            rear_left, _ = self.__rear.read_speed_m2()
        finally:
            self.__roboclaw_lock.release()

        front_left = to_mmps(front_left)
        front_right = to_mmps(front_right)
        rear_left = to_mmps(rear_left)
        rear_right = to_mmps(rear_right)

        return front_left, front_right, rear_left, rear_right

    def set_speeds(self, front_left, front_right, rear_left, rear_right):
        if self.__roboclaw_disabled:
            return

        front_left = to_qpps(front_left)
        front_right = to_qpps(front_right)
        rear_left = to_qpps(rear_left)
        rear_right = to_qpps(rear_right)

        self.__reset_timeouts()

        self.__roboclaw_lock.acquire()
        try:
            self.__front.drive_m1_with_signed_speed(front_right)
            self.__front.drive_m2_with_signed_speed(front_left)
            self.__rear.drive_m1_with_signed_speed(rear_right)
            self.__rear.drive_m2_with_signed_speed(rear_left)
        finally:
            self.__roboclaw_lock.release()

    def stop(self):
        self.__roboclaw_lock.acquire()
        try:
            self.__front.drive_mixed_with_signed_speed(0, 0)
            self.__rear.drive_mixed_with_signed_speed(0, 0)
        finally:
            self.__roboclaw_lock.release()

    def __reset(self):
        self.__reset_gpio.write('0')
        time.sleep(0.5)
        self.__reset_gpio.write('1')

    def setup(self):
        self.__front.set_pid_constants_m1(MOTORS_P_CONST, MOTORS_I_CONST, MOTORS_D_CONST, MOTORS_MAX_QPPS)
        self.__front.set_pid_constants_m2(MOTORS_P_CONST, MOTORS_I_CONST, MOTORS_D_CONST, MOTORS_MAX_QPPS)
        self.__rear.set_pid_constants_m1(MOTORS_P_CONST, MOTORS_I_CONST, MOTORS_D_CONST, MOTORS_MAX_QPPS)
        self.__rear.set_pid_constants_m2(MOTORS_P_CONST, MOTORS_I_CONST, MOTORS_D_CONST, MOTORS_MAX_QPPS)

        self.__front.set_m1_encoder_mode(0)
{'fraction_non_alphanumeric': 0.0826724682433855, 'fraction_numerical': 0.006175871991016914, 'mean_word_length': 3.942767950052029, 'pattern_counts': {'":': 0, '<': 3, '<?xml version=': 0, '>': 3, 'https://': 0, 'lorem ipsum': 0, 'www.': 0, 'xml': 0}, 'pii_count': 0, 'substrings_counts': 0, 'word_list_counts': {'cursed_substrings.json': 0, 'profanity_word_list.json': 0, 'sexual_word_list.json': 0, 'zh_pornsignals.json': 0}}
{'dir': 'git-commits-cleaned', 'id': '521522', 'n_tokens_mistral': 5017, 'n_tokens_neox': 4927, 'n_words': 691}