def __init__(self):
z_pin = 18
angle_one_pin = 13
angle_two_pin = 12
self.z_motor = Motor(z_pin)
self.shoulder_motor = Motor(angle_one_pin, 20 * pi / 180)
self.elbow_motor = Motor(angle_two_pin, 70 * pi / 180)
self.shoulder_motor.change_angle(-1)
self.elbow_motor.change_angle(-1)
def initMotors():
speed = 100
m1 = Motor(1)
m1.setSpeedBrushless(speed)
m1.go()
m2 = Motor(2)
m2.setSpeedBrushless(speed)
m2.go()
m3 = Motor(3)
m3.setSpeedBrushless(speed)
m3.go()
m4 = Motor(4)
m4.setSpeedBrushless(speed)
m4.go()
#time.sleep(0.5)
time.sleep(5)
#####
# shutdown the motors
#####
speed = 0
m1.setSpeedBrushless(speed)
m1.go()
m2.setSpeedBrushless(speed)
m2.go()
m3.setSpeedBrushless(speed)
m3.go()
m4.setSpeedBrushless(speed)
m4.go()
from machine import Pin
import blesync_server
import blesync_uart.server
from motors import Motor
import pinout
from steering import Steering
motor_r = Motor(pinout.MOTOR_1A, pinout.MOTOR_2A, pinout.MOTOR_12EN)
motor_l = Motor(pinout.MOTOR_3A, pinout.MOTOR_4A, pinout.MOTOR_34EN)
steering = Steering(motor_l, motor_r)
built_in_led = Pin(2, Pin.OUT)
@blesync_uart.server.UARTService.on_message
def on_message(service, conn_handle, message):
if message == b'!B516':
steering.center(1000)
elif message == b'!B507':
steering.center(0)
elif message == b'!B615':
steering.center(-1000)
elif message == b'!B606':
steering.center(0)
elif message == b'!B813':
steering.right(1000)
elif message == b'!B804':
steering.right(0)
elif message == b'!B714':
steering.left(1000)
from __future__ import division
from time import sleep, time
from threading import Thread
import Adafruit_PCA9685
import RPi.GPIO as GPIO
pwm = Adafruit_PCA9685.PCA9685()
import configuration
from motors import Motor
from servo import Servo
servo = Servo(GPIO, configuration.servo_pin, 180)
motor_f_r = Motor(configuration.motor_f_r, pwm, GPIO)
motor_f_l = Motor(configuration.motor_f_l, pwm, GPIO)
motor_b_r = Motor(configuration.motor_b_r, pwm, GPIO)
motor_b_l = Motor(configuration.motor_b_l, pwm, GPIO)
current_milli_time = lambda: int(round(time() * 1000))
import matplotlib.pyplot as plt
import random
def sign(num):
return -1 if num < 0 else 1
class PID(Thread):
from pyPS4Controller.controller import Controller
from motors import Motor
Motor_1 = Motor(3, 5)
Motor_2 = Motor(7, 8)
#from ROADENT.App import Motor_1, Motor_2
class MyController(Controller):
def __init__(self, **kwargs):
Controller.__init__(self, **kwargs)
def on_up_arrow_press(self):
print("Forward")
Motor_1.clockwise()
Motor_2.clockwise()
def on_up_down_arrow_release(self):
print("Stop")
Motor_1.stop()
Motor_2.stop()
def on_down_arrow_press(self):
Motor_1.c_clockwise()
Motor_2.c_clockwise()
def on_right_arrow_press(self):
print("turn right")
Motor_2.clockwise()
Motor_1.c_clockwise()
def __init__(self):
"""
This method creates a tortoise. It initializes the sensors, the variables that control the random motion and creates a file with the PID of the process which has created the tortoise so that the watchdog (a background process) can stops the motors and LEDs in case the user process finishes because of an error. The tortoise is created uncalibrated.
The reasons of termination of a user process could be because of normal termination
or because of an error (exceptions, ctrl-c, ...). When an error happens, the motors
and may be still on. In this case, the motors and LEDs should be turned off
for the battery not to drain.
The solution implemented is to have a background process (a watchdog) running
continously. This process checks if the user process doesn't exist anymore (termination).
If it doesn't, it stops the motors, switches off the LEDs and cleans up all the pins.
In order to identy that the user script has finished, a file with the name [PID].pid is
created in the folder ~/.tortoise_pids/, where [PID] is the PID of the user process.
Regarding calibration, the purpose was to avoid calibration everytime the tortoise object is created.
However, this hasn't been implemented yet. The idea could be to save the light
values in a file and read that file when creating the tortoise object.
Light conditions could have changed, so this should be done carefully.
At the moment, the tortoise object is created without calibration. If the users
want to use the light sensors, they need will need to execute the calibrateLight
function before using those sensors.
"""
# Variables that control the calibration of the light sensors
global isLightCalibrated
global lowerBoundLight
global upperBoundLight
isLightCalibrated = False
lowerBoundLight = 0
upperBoundLight = 0
# --- Variables that control the calibration of the light sensors ---
# No warnings from the GPIO library
GPIO.setwarnings(False)
# Variables that control the random motion
self.lastRandomCommand = None
self.timesSameRandomCommandExecuted = 0
self.numberRepeatsRandomCommand = -1
self.lastRandomStepsWheelA = None
self.lastRandomStepsWheelB = None
self.lastRandomDirection = None
# --- Variables that control the random motion ---
# Setting the motors, sensors and actuators
# Pin numbers of the motors
motorPins = [13, 6, 5, 7, 20, 10, 9, 11]
self.A = Motor(motorPins[0], motorPins[1], motorPins[2], motorPins[3])
self.B = Motor(motorPins[4], motorPins[5], motorPins[6], motorPins[7])
self.sensors = Sensors()
self.actuators = Actuators()
# Position 1 of the light sensors area in the PCB assigned to pin 17
self.sensors.setSensor(enums.SensorType.light, 1, 17)
# Position 2 of the light sensors area in the PCB assigned to pin 4
self.sensors.setSensor(enums.SensorType.light, 2, 4)
# Position 1 of the touch sensors area in the PCB assigned to pin 3
self.sensors.setSensor(enums.SensorType.emergencyStop, 1, 3)
# Position 2 of the touch sensors area in the PCB assigned to pin 27
self.sensors.setSensor(enums.SensorType.touch, 2, 27)
# Position 3 of the touch sensors area in the PCB assigned to pin 2
self.sensors.setSensor(enums.SensorType.touch, 3, 2)
# Position 4 of the touch sensors area in the PCB assigned to pin 18
self.sensors.setSensor(enums.SensorType.touch, 4, 18)
# Position 1 of the proximity sensors area in the PCB assigned to pin 19
self.sensors.setSensor(enums.SensorType.proximity, 1, 19)
# Position 2 of the proximity sensors area in the PCB assigned to pin 21
self.sensors.setSensor(enums.SensorType.proximity, 2, 21)
# Position 3 of the proximity sensors area in the PCB assigned to pin 22
self.sensors.setSensor(enums.SensorType.proximity, 3, 22)
# Position 4 of the proximity sensors area in the PCB assigned to pin 26
self.sensors.setSensor(enums.SensorType.proximity, 4, 26)
# Positions of the LEDs area in the PCB assigned to pins 8, 16, 25, 12
ledPins = [8, 16, 25, 12]
self.actuators.initActuator(enums.ActuatorType.led, 1, ledPins[0])
self.actuators.initActuator(enums.ActuatorType.led, 2, ledPins[1])
self.actuators.initActuator(enums.ActuatorType.led, 3, ledPins[2])
self.actuators.initActuator(enums.ActuatorType.led, 4, ledPins[3])
# --- Setting the motors, sensors and actuators ---
# Times pressed the touch sensor for the latching behavour
self.lastTouch = [-1,-1,-1]
# Minimum milliseconds to send to the motors as delay
self.minDelayMotors = 2
# Creation of a file with the PID of the process
# PID of process
pid = os.getpid()
# ~/.tortoise_pids/
directory = os.path.expanduser("~") + "/.tortoise_pids/"
# Filename: [PID].pid
f = open(directory + str(pid) + ".pid", "w")
# First line: motor pins
f.write(str(motorPins[0]) + " " + str(motorPins[1]) + " " + str(motorPins[2]) + " " + str(motorPins[3]) + " " + str(motorPins[4]) + " " + str(motorPins[5]) + " " + str(motorPins[6]) + " " + str(motorPins[7]) + "\n")
# Second line: LED pins
f.write(str(ledPins[0]) + " " + str(ledPins[1]) + " " + str(ledPins[2]) + " " + str(ledPins[3]) + "\n")
f.close()
# --- Creation of a file with the PID of the process ---
# Waiting for the user to press the e-stop button
self.state = enums.State.paused
messages.printMessage('greetings')
while self.getSensorData(enums.SensorType.emergencyStop, 4) == 0:
time.sleep(0.1)
messages.printMessage('running')
self.state = enums.State.running
############################ #instantiate the BT controller bt = BT_Controller(z_d) bt.run() #instantiate IMU #TODO see how to import C interface to python imu = IMU() #MyKalman=KalmanFilter(....) # dynamical model instance dyn_model = Dynamical_Model() #instantiate motors and put them together in a list motor1 = Motor(1) motor2 = Motor(2) motor3 = Motor(3) motor4 = Motor(4) motors = [motor1, motor2, motor3, motor4] # instantiate PID controllers rollPID = PID(Kp, Kd, Ki) # Kp, Kd, Ki pitchPID = PID(Kp, Kd, Ki) yawPID = PID(0, 0, 0) zPID = PID(1, 0, 0) #xposPID=PID(-0.09, -0.1, 0) #yposPID=PID(-0.09, -0.1, 0) # test variables frequencies = []
def __init__(self):
self.lmotor = Motor(4, 0x09, 1)
self.rmotor = Motor(4, 0x09, 2)
self.camera = Camera()
self.sensor = Sensor()
from bot_serial import SerialWrapper
from motors import Motor
from recorder import CVCaptureRecorder
if __name__ == '__main__':
parser = argparse.ArgumentParser(
description='Control the potato beer delivery system via an arduino.')
parser.add_argument('dev',
type=str,
help='serial device the arduino is connected to')
args = parser.parse_args()
recorder = CVCaptureRecorder()
serial = SerialWrapper(dev=args.dev, speed=57600)
speed_motor = Motor(serial, arduino_command="S")
direction_motor = Motor(serial, arduino_command="D")
api = Api(recorder=recorder)
api.demonize(port=8000)
while True:
# api events
recorder.update_frame_rate()
recorder.handle_frame()
with api.lock:
for event, kwargs in api.events:
if event == "set":
direction_motor.set(kwargs["position"])
if event == "set_speed":
speed_motor.set(kwargs["position"])
GPIO.setup("P8_4", GPIO.OUT)
GPIO.output("P8_4", GPIO.HIGH)
GPIO.setup("P8_11", GPIO.OUT)
GPIO.output("P8_11", GPIO.HIGH)
GPIO.setup("P9_15", GPIO.OUT)
GPIO.output("P9_15", GPIO.HIGH)
GPIO.setup("P9_23", GPIO.OUT)
GPIO.output("P9_23", GPIO.HIGH)
##############################
speed = 0
m1 = Motor(1)
m1.setSpeed(speed)
m1.go()
m2 = Motor(2)
m2.setSpeed(speed * (-1))
m2.go()
m3 = Motor(3)
m3.setSpeed(speed)
m3.go()
m4 = Motor(4)
m4.setSpeed(speed * (-1))
m4.go()
#!/usr/bin/env micropython
"""Development/debug test program."""
from motors import Motor, Mechanism
from sensors import Touch
from time import sleep
# Set up devices
gripper_motor = Motor('outA', gear_ratio=12)
gripper_switch = Touch('in1')
# Define and reset mechanism
targets = {'open': 0, 'closed': 110, 'up': 400, 'reset': 430}
default_speed = 50
gripper = Mechanism(gripper_motor, targets, default_speed, gripper_switch)
# Go to predefined absolute target
gripper.go_to_target('closed')
sleep(1)
gripper.go_to_target('open')
The call to this method is:
python text_motor.py [speed = 10] [time = 5]
'''
from motors import Motor
import Adafruit_BBIO.GPIO as GPIO
import time
import os
import sys
speed = 10
if len(sys.argv) > 1:
speed = int(sys.argv[1])
m1 = Motor(1);
m1.setSpeedBrushless(speed)
m1.go()
m2 = Motor(2);
m2.setSpeedBrushless(speed);
m2.go()
m3 = Motor(3);
m3.setSpeedBrushless(speed);
m3.go()
m4 = Motor(4);
m4.setSpeedBrushless(speed);
m4.go()
