############################################################################ # CoderBot, a didactical programmable robot. # Copyright (C) 2014, 2015 Roberto Previtera # # This program is free software; you can redistribute it and/or modify # it under the terms of the GNU General Public License as published by # the Free Software Foundation; either version 2 of the License, or # (at your option) any later version. # # This program is distributed in the hope that it will be useful, # but WITHOUT ANY WARRANTY; without even the implied warranty of # MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the # GNU General Public License for more details. # # You should have received a copy of the GNU General Public License along # with this program; if not, write to the Free Software Foundation, Inc., # 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA. ############################################################################ import os import sys import time from math import copysign import logging import pigpio import sonar from hw import mpu from rotary_encoder.wheelsaxel import WheelsAxel # GPIO class GPIO_CODERBOT_V_4(): # motors PIN_MOTOR_ENABLE = 22 PIN_LEFT_FORWARD = 25 PIN_LEFT_BACKWARD = 24 PIN_RIGHT_FORWARD = 4 PIN_RIGHT_BACKWARD = 17 PIN_PUSHBUTTON = 11 # servo PIN_SERVO_1 = 9 PIN_SERVO_2 = 10 # sonar PIN_SONAR_1_TRIGGER = 18 PIN_SONAR_1_ECHO = 7 PIN_SONAR_2_TRIGGER = 18 PIN_SONAR_2_ECHO = 8 PIN_SONAR_3_TRIGGER = 18 PIN_SONAR_3_ECHO = 23 PIN_SONAR_4_TRIGGER = 18 PIN_SONAR_4_ECHO = 13 # encoder PIN_ENCODER_LEFT_A = 14 PIN_ENCODER_LEFT_B = 6 PIN_ENCODER_RIGHT_A = 15 PIN_ENCODER_RIGHT_B = 12 HAS_ENCODER = False class GPIO_CODERBOT_V_5(): # motors PIN_MOTOR_ENABLE = None #22 PIN_LEFT_FORWARD = 17 #25 PIN_LEFT_BACKWARD = 18 # 24 PIN_RIGHT_FORWARD = 22 # 4 PIN_RIGHT_BACKWARD = 23 #17 PIN_PUSHBUTTON = 16 #11 # servo PIN_SERVO_1 = 19 #9 PIN_SERVO_2 = 26 #10 # sonar PIN_SONAR_1_TRIGGER = 5 #18 PIN_SONAR_1_ECHO = 27 #7 PIN_SONAR_2_TRIGGER = 5 #18 PIN_SONAR_2_ECHO = 6 #8 PIN_SONAR_3_TRIGGER = 5 #18 PIN_SONAR_3_ECHO = 12 #23 PIN_SONAR_4_TRIGGER = 5 #18 PIN_SONAR_4_ECHO = 13 #23 # encoder PIN_ENCODER_LEFT_A = 14 PIN_ENCODER_LEFT_B = 15 #6 PIN_ENCODER_RIGHT_A = 24 #15 PIN_ENCODER_RIGHT_B = 25 #12 HAS_ENCODER = True # PWM PWM_FREQUENCY = 100 #Hz PWM_RANGE = 100 #0-100 HW_VERSIONS = { "4": GPIO_CODERBOT_V_4(), "5": GPIO_CODERBOT_V_5() } class CoderBot(object): # pylint: disable=too-many-instance-attributes def __init__(self, motor_trim_factor=1.0, motor_min_power=0, motor_max_power=100, hw_version="5", pid_params=(0.8, 0.1, 0.01, 200, 0.01)): try: self._mpu = mpu.AccelGyroMag() logging.info("MPU available") except: logging.info("MPU not available") self.GPIOS = HW_VERSIONS.get(hw_version, GPIO_CODERBOT_V_5()) self._pin_out = [self.GPIOS.PIN_LEFT_FORWARD, self.GPIOS.PIN_RIGHT_FORWARD, self.GPIOS.PIN_LEFT_BACKWARD, self.GPIOS.PIN_RIGHT_BACKWARD, self.GPIOS.PIN_SERVO_1, self.GPIOS.PIN_SERVO_2] self.pi = pigpio.pi('localhost') self.pi.set_mode(self.GPIOS.PIN_PUSHBUTTON, pigpio.INPUT) self._cb = dict() self._cb_last_tick = dict() self._cb_elapse = dict() self._encoder = self.GPIOS.HAS_ENCODER self._motor_trim_factor = motor_trim_factor self._motor_min_power = motor_min_power self._motor_max_power = motor_max_power self._twin_motors_enc = WheelsAxel( self.pi, enable_pin=self.GPIOS.PIN_MOTOR_ENABLE, left_forward_pin=self.GPIOS.PIN_LEFT_FORWARD, left_backward_pin=self.GPIOS.PIN_LEFT_BACKWARD, left_encoder_feedback_pin_A=self.GPIOS.PIN_ENCODER_LEFT_A, left_encoder_feedback_pin_B=self.GPIOS.PIN_ENCODER_LEFT_B, right_forward_pin=self.GPIOS.PIN_RIGHT_FORWARD, right_backward_pin=self.GPIOS.PIN_RIGHT_BACKWARD, right_encoder_feedback_pin_A=self.GPIOS.PIN_ENCODER_RIGHT_A, right_encoder_feedback_pin_B=self.GPIOS.PIN_ENCODER_RIGHT_B, pid_params=pid_params) self.motor_control = self._dc_enc_motor self._cb1 = self.pi.callback(self.GPIOS.PIN_PUSHBUTTON, pigpio.EITHER_EDGE, self._cb_button) for pin in self._pin_out: self.pi.set_PWM_frequency(pin, PWM_FREQUENCY) self.pi.set_PWM_range(pin, PWM_RANGE) self.sonar = [sonar.Sonar(self.pi, self.GPIOS.PIN_SONAR_1_TRIGGER, self.GPIOS.PIN_SONAR_1_ECHO), sonar.Sonar(self.pi, self.GPIOS.PIN_SONAR_2_TRIGGER, self.GPIOS.PIN_SONAR_2_ECHO), sonar.Sonar(self.pi, self.GPIOS.PIN_SONAR_3_TRIGGER, self.GPIOS.PIN_SONAR_3_ECHO), sonar.Sonar(self.pi, self.GPIOS.PIN_SONAR_4_TRIGGER, self.GPIOS.PIN_SONAR_4_ECHO)] self._servos = [self.GPIOS.PIN_SERVO_1, self.GPIOS.PIN_SERVO_2] self.stop() the_bot = None def exit(self): self._cb1.cancel() if self._encoder: self._twin_motors_enc.cancel_callback() for s in self.sonar: s.cancel() @classmethod def get_instance(cls, motor_trim_factor=1.0, motor_max_power=100, motor_min_power=0, hw_version="5", pid_params=(0.8, 0.1, 0.01, 200, 0.01), from_defaults=True): if not cls.the_bot: if from_defaults: raise ValueError("incorrect CoderBot initialisation") cls.the_bot = CoderBot(motor_trim_factor=motor_trim_factor, motor_max_power= motor_max_power, motor_min_power=motor_min_power, hw_version=hw_version, pid_params=pid_params) return cls.the_bot def get_motor_power(self, speed): return int(copysign(min(max(((self._motor_max_power - self._motor_min_power) * abs(speed) / 100) + self._motor_min_power, self._motor_min_power), self._motor_max_power), speed)) def move(self, speed=100, elapse=None, distance=None): speed_left = speed * self._motor_trim_factor speed_right = speed / self._motor_trim_factor self.motor_control(speed_left=speed_left, speed_right=speed_right, time_elapse=elapse, target_distance=distance) def turn(self, speed=100, elapse=None, distance=None): speed_left = speed * self._motor_trim_factor speed_right = -speed / self._motor_trim_factor self.motor_control(speed_left=speed_left, speed_right=speed_right, time_elapse=elapse, target_distance=distance) def turn_angle(self, speed=100, angle=0): z = self._mpu.get_gyro()[2] self.turn(speed, elapse=0) while abs(z - self._mpu.get_gyro()[2]) < angle: time.sleep(0.05) logging.info(self._mpu.get_gyro()[2]) self.stop() def forward(self, speed=100, elapse=None, distance=None): self.move(speed=speed, elapse=elapse, distance=distance) def backward(self, speed=100, elapse=None, distance=None): self.move(speed=-speed, elapse=elapse, distance=distance) def left(self, speed=100, elapse=-1): self.turn(speed=-speed, elapse=elapse) def right(self, speed=100, elapse=-1): self.turn(speed=speed, elapse=elapse) def servo(self, servo, angle): self._servo_control(self._servos[servo], angle) def get_sonar_distance(self, sonar_id=0): return self.sonar[sonar_id].get_distance() def get_mpu_accel(self, axis=None): acc = self._mpu.get_acc() if axis is None: return acc else: return int(acc[axis]*100.0)/100.0 def get_mpu_gyro(self, axis=None): gyro = self._mpu.get_gyro() if axis is None: return gyro else: return int(gyro[axis]*100.0)/200.0 def get_mpu_heading(self): hdg = self._mpu.get_hdg() return int(hdg) def get_mpu_temp(self): temp = self._mpu.get_temp() return int(temp*100.0)/100.0 def _servo_control(self, pin, angle): duty = ((angle + 90) * 100 / 180) + 25 self.pi.set_PWM_range(pin, 1000) self.pi.set_PWM_frequency(pin, 50) self.pi.set_PWM_dutycycle(pin, duty) def _dc_enc_motor(self, speed_left=100, speed_right=100, time_elapse=None, target_distance=None): self._twin_motors_enc.control(power_left=self.get_motor_power(speed_left), power_right=self.get_motor_power(speed_right), time_elapse=time_elapse, target_distance=target_distance) def stop(self): self._twin_motors_enc.stop() def is_moving(self): return self._twin_motors_enc._is_moving # Distance travelled getter def distance(self): return self._twin_motors_enc.distance() # CoderBot velocity getter def speed(self): return self._twin_motors_enc.speed() # CoderBot direction getter def direction(self): return self._twin_motors_enc.speed() def set_callback(self, gpio, callback, elapse): self._cb_elapse[gpio] = elapse * 1000 self._cb[gpio] = callback self._cb_last_tick[gpio] = 0 def sleep(self, elapse): logging.debug("sleep: %s", str(elapse)) time.sleep(elapse) def _cb_button(self, gpio, level, tick): cb = self._cb.get(gpio) if cb: elapse = self._cb_elapse.get(gpio) if level == 0: self._cb_last_tick[gpio] = tick elif tick - self._cb_last_tick[gpio] > elapse: self._cb_last_tick[gpio] = tick logging.info("pushed: %d, %d", level, tick) cb()