class RoboArm(object):
def __init__(self):
self.joints = [
PWMJoint(
5, BASE_MIN_ANGLE, BASE_MIN_PWM, BASE_MAX_ANGLE,
BASE_MAX_PWM, BASE_COF1, BASE_COF0),
PWMJoint(
6, SHOULDER_MIN_ANGLE, SHOULDER_MIN_PWM, SHOULDER_MAX_ANGLE,
SHOULDER_MAX_PWM, SHOULDER_COF1, SHOULDER_COF0),
PWMJoint(
7, ELBOW_MIN_ANGLE, ELBOW_MIN_PWM, ELBOW_MAX_ANGLE,
ELBOW_MAX_PWM, ELBOW_COF1, ELBOW_COF0),
DyMiJoint(
1, WRIST_MIN_ANGLE, WRIST_MIN_PWM, WRIST_MAX_ANGLE,
WRIST_MAX_PWM, WRIST_COF1, WRIST_COF0)]
self.motor = Motor(4)
def move_arm(self, out_pos):
self.motor.set_vel(out_pos.pos7)
arm_pos = [0.] * 6
for i in range(0, 4):
legal, arm_pos[i] = joints[i].get_pos(arm_pose[i])
if not legal:
rospy.logerr('angle over bound for joint %d' % i)
joints[i].set_pos(arm_pos[i])
def close(self):
for joint in self.joints:
joint.close()
def main():
log_file = "main_log.log"
create_timed_rotating_log("log/" + log_file)
logger = logging.getLogger("BasicLogger")
logger.info("----- Starting Logging Session -----")
config = configparser.ConfigParser()
config.read('config.ini')
# How to use the config values. REMOVE when done with setup of this!
print(config.sections())
print('scale of this whole thing from config file is: ' + config['grid']['scale'])
# To get the values as integers:
i = int(config['grid']['max_position_z'])
print(i+2)
# subprocess.call("../gcodepull.sh", shell=True)
#opens the file named in the varibles file
length = range(FileOperator.OpenFile()- 3)
Motor.setup()
start = 2
for row in length:
# for the appropiated length each row is worked through
# and the needet steps are sent to the stepper motors
next_row = row + start
delta_step = FileOperator.NextMove(next_row)
corrected_coords = FileOperator.MoveCorrect(delta_step)
Motor.move(corrected_coords)
print('finished')
GPIO.cleanup()
def navigate():
'''
Function will call exploration() function to move robot, but will keep
track of previous moves
'''
# Test output
print("Navigating ...")
# Create motor and buzzer objects
motor = Motor()
buzzer = Buzzer()
# Beep to indicate begining of navigate step
buzzer.play(4)
try:
# Enter the exploration loop
for i in xrange(params.p['MAX_ITER']):
# Execute explore function and save results
explore(motor, buzzer)
# Wait between moves
time.sleep(params.p['WAIT_TIME'])
except Exception:
pass
finally:
motor.stop_bot()
def main():
Volvomotor=Motor(0,100,0,"Volvo","Stop")
state=raw_input("Motor state: ")
Volvomotor.changestate(state)
Volvomotor.accelerate()
print "Current speed: "+str(Volvomotor.rpm)+"rpm"
return 0
def __init__(self):
self._front = Motor(self._frontDriver)
self._rear = Motor(self._rearDriver)
self._front.setDirCw()
self._front.setSpeed(0)
self._rear.setDirCcw()
self._rear.setSpeed(0)
def __init__(self, config):
Thread.__init__(self)
self.angle = 0
self.K = 0.98
self.logDataSetBuffer = []
self.Kp = config.config.as_float('Kp')
self.Ki = config.config.as_float('Ki')
self.Kd = config.config.as_float('Kd')
self.set_point = config.config.as_float('set_point')
self.disable_motors = config.config.as_bool('disable_motors')
self.integral_error = 0
self.last_error = 0
self.motor_left = Motor("L", port_motor_left_pwm, port_motor_left_backward, port_motor_left_forward)
self.motor_right = Motor("R", port_motor_right_pwm, port_motor_right_backward, port_motor_right_forward)
self.accelerometer = MPU6050()
self.last_time = time()
self.logger = logging.getLogger(__name__)
self.setDaemon(True)
self.latest_sensor = 0
self.logger.info('Initialized ControlThread with the following settings')
self.logger.info('disable_motors={}'.format(self.disable_motors))
self.logger.info('set_point={:1.2f}'.format(self.set_point))
self.logger.info('Kp={:1.1f}'.format(self.Kp))
self.logger.info('Ki={:1.1f}'.format(self.Ki))
self.logger.info('Kd={:1.1f}'.format(self.Kd))
def __init__(self):
self.motor_front_left = Motor(MotorConductor.FRONT, MotorConductor.LEFT)
self.motor_front_right = Motor(MotorConductor.FRONT, MotorConductor.RIGHT)
for ndx, motor in enumerate(self.get_motors()):
motor.gear_ratio = Config.get_gear_ratio()
motor.wheel_diameter = Config.get_wheel_diameter()
motor.position_ratio = Config.get_position_ratio()[ndx]
motor.speed_ratio = Config.get_speed_ratio()[ndx]
class CarServer(object):
def __init__(self, motor_pins=(24, 25), servo_pin=0, port=2012):
self.port = port
# The motor and servo for driving
self.motor = Motor(*motor_pins)
self.servo = Servo(servo_pin)
# The most recent coordinates from the accelerameter
self.coords = (0, 0, 0)
# Whether or not to continue running the server
self._run = True
self.start()
def start(self):
""" Initialize and start threads. """
self._server_thread = threading.Thread(target=self._server_worker)
self._server_thread.start()
self._control_thread = threading.Thread(target=self._control_worker)
self._control_thread.start()
def stop(self):
""" Shut down server and control threads. """
self._run = False
def _server_worker(self):
HOST = '' # Symbolic name meaning all available interfaces
PORT = self.port
s = socket.socket(socket.AF_INET, socket.SOCK_STREAM)
s.bind((HOST, PORT))
s.listen(1)
conn, addr = s.accept()
print 'Connected by', addr
while self._run:
data = conn.recv(1024)
if data:
coords = data[1:].split(',')
x, y, z = [float(n) for n in coords]
self.coords = (x, y, z)
conn.sendall(data)
conn.close()
def _control_worker(self):
while self._run:
x, y, z = self.coords
forward_speed = -y/10
turning_power = (x+10)/20
self.motor.drive(forward_speed)
self.servo.set(turning_power)
def __init__(self, verbose=False):
self.verbose = verbose
capture = cv2.VideoCapture(0)
capture.set(cv2.CAP_PROP_FRAME_WIDTH, 960)
capture.set(cv2.CAP_PROP_FRAME_HEIGHT, 500)
capture.set(cv2.CAP_PROP_BUFFERSIZE, 3)
capture.set(cv2.CAP_PROP_FPS, 60)
self.__capture_manager = CaptureManager(capture, 10)
self.__io_manager = IOManager(IO_PINS, verbose)
self.__image_analyzer = ImageAnalyzer(self.__capture_manager.current_frame_color, verbose)
self.__coordinates_translator = CoordinatesTranslator(self.__image_analyzer, CONVEYOR_WIDTH)
self.__packager = Packager(TRAY_SIZE, PADDING, verbose)
self.__item_movement_motor = Motor(MOTOR_PINS['item_movement_control'],
MOTOR_PINS['item_movement_direction'],
ITEM_MOVEMENT_CONVEYOR_STEP_INTERVAL,
ITEM_MOVEMENT_CONVEYOR_STEP_LIMIT,
MOTOR_PINS['item_movement_limit_switch'],
0.15,
10,
0.15,
10,
self.__io_manager,
verbose)
self.__io_manager.protect_pin(MOTOR_PINS['item_movement_control'])
self.__io_manager.protect_pin(MOTOR_PINS['item_movement_direction'])
self.__io_manager.protect_pin(MOTOR_PINS['item_movement_limit_switch'])
self.__tray_movement_motor = Motor(MOTOR_PINS['tray_movement_control'],
MOTOR_PINS['tray_movement_direction'],
TRAY_MOVEMENT_CONVEYOR_STEP_INTERVAL,
None,
MOTOR_PINS['tray_movement_limit_switch'],
0.15,
10,
0.15,
10,
self.__io_manager,
verbose)
self.__io_manager.protect_pin(MOTOR_PINS['tray_movement_control'])
self.__io_manager.protect_pin(MOTOR_PINS['tray_movement_direction'])
self.__io_manager.protect_pin(MOTOR_PINS['tray_movement_limit_switch'])
self.current_loop_done = 1
self.__calibrate_motors()
class Frank:
def __init__(self):
print "Set mode BCM"
GPIO.setmode(GPIO.BCM)
self.motorX = Motor([6, 13, 19, 26])
#self.motorY = Motor([])
#self.sensor = Sensor(4)
self.setUpScreen()
def setUpScreen(self):
self.screen = curses.initscr()
# turn off input echoing
curses.noecho()
# respond to keys immediately (don't wait for enter)
curses.cbreak()
# map arrow keys to special values
self.screen.keypad(True)
def isArrowKey(self, char):
return char in [curses.KEY_RIGHT,curses.KEY_LEFT, curses.KEY_UP, curses.KEY_DOWN]
def start(self):
try:
char = self.screen.getch()
while True :
while self.isArrowKey():
if char == curses.KEY_RIGHT:
print 'right'
self.motorX.moveTo(Motor.RIGHT)
elif char == curses.KEY_LEFT:
print 'left '
self.motorX.moveTo(Motor.LEFT)
elif char == curses.KEY_UP :
print 'up'
elif char == curses.KEY_DOWN :
print 'down'
char = self.screen.getch()
if char == ord('q'):
self.cleanUp()
break
else:
char = self.screen.getch()
finally:
# shut down cleanly
curses.nocbreak(); self.screen.keypad(0); curses.echo()
curses.endwin()
def cleanUp(self):
GPIO.cleanup()
def add_motor(self, line):
"""
Add a new motor to the motor list
:param line: motor parameters line as string from config file
motor_number, motor_name, start_position, min_limit, max_limit
:return:
"""
line_list = line.split()
new_motor = Motor(line_list[1])
new_motor.motor_number = int(line_list[0])
new_motor.goal_position = int(line_list[2])
new_motor.min_limit = int(line_list[3])
new_motor.max_limit = int(line_list[4])
self.motor_list[line_list[1]] = new_motor
def __init__(self):
print "Set mode BCM"
config = json.load(open("config.json"))
GPIO.setmode(GPIO.BCM)
self.motorX = Motor(config["motor_x"]["pins"])
self.motorX.name = "X"
self.motorX.delay = config["motor_x"]["delay"]
self.motorX.steps_by_mm = config["motor_x"]["steps_by_mm"]
self.motorY = Motor(config["motor_y"]["pins"])
self.motorY.name = "Y"
self.motorY.delay = config["motor_y"]["delay"]
self.motorY.steps_by_mm = config["motor_y"]["steps_by_mm"]
self.laser = Laser(config["laser"]["pin"])
def handle_read(self):
data0 = self.recv(160);
if data0:
data = ConstBitStream(bytes=data0, length=160)
# print "All: %s" % data.bin
msize = data.read('intle:16')
mtype = data.read('intle:16')
mtime = data.read('intle:64')
# RA:
ant_pos = data.bitpos
ra = data.read('hex:32')
data.bitpos = ant_pos
ra_uint = data.read('uintle:32')
# DEC:
ant_pos = data.bitpos
dec = data.read('hex:32')
data.bitpos = ant_pos
dec_int = data.read('intle:32')
logging.debug("Size: %d, Type: %d, Time: %d, RA: %d (%s), DEC: %d (%s)" % (msize, mtype, mtime, ra_uint, ra, dec_int, dec))
(ra, dec, time) = coords.int_2_rads(ra_uint, dec_int, mtime)
x = transformar_coordenadas(dec, ra)
az,alt = x.get_azi_alt()
#instancia o motor de azimute nos pinos 12, 16, 20 e 21 do RPi
motor_az = Motor([31,33,35,37])
motor_az.rpm = 5
motor_az.mode = 2
motor_az.move_to(az-self.az_anterior)
self.az_anterior = az
#instancia o motor de azimute nos pinos 32, 36, 38 e 40 do RPi
motor_alt = Motor([32,36,38,40])
motor_alt.rpm = 5
motor_alt.mode = 2
motor_alt.move_to(alt-self.alt_anterior)
self.alt_anterior = alt
logging.debug("Azimute: %d, Altitude: %d" % (az,alt))
# envia as cordenadas para o Stellarium
self.act_pos(ra, dec)
def __init__(self, mode):
self.mode = mode
adafruitLoader = AdafruitLoader(self.mode)
self.pwm = adafruitLoader.getPwmModule()
self.bno = adafruitLoader.getBNO055Module()
if not self.bno.begin():
raise RuntimeError('Failed to initialize BNO055! Is the sensor connected?')
# Set frequency to 60hz, good for servos.
self.pwm.set_pwm_freq(60)
self.motor1 = Motor(0, self.pwm, 276,457)
self.motor2 = Motor(1, self.pwm, 276,457)
self.motor3 = Motor(14, self.pwm, 276,457)
self.motor4 = Motor(15, self.pwm, 276,457)
def __init__(self):
print "Set mode BCM"
GPIO.setmode(GPIO.BCM)
self.motorX = Motor([6, 13, 19, 26])
#self.motorY = Motor([])
#self.sensor = Sensor(4)
self.setUpScreen()
def __init__(self, debugMode = False):
GPIO.setmode(GPIO.BOARD)
GPIO.setwarnings(False)
if debugMode:
level = logging.DEBUG
else:
level = logging.CRITICAL
logging.basicConfig(stream=sys.stderr, level=level)
self.motor = Motor(GPIO, logging)
self.panandtilt = PanAndTilt(logging)
self.distance = Distance(GPIO, logging)
self.MoveDirectionsOptions = {
'fwd': self.motor.forward,
'stp': self.motor.stop,
'lft': self.motor.leftTurn,
'rgt': self.motor.rightTurn,
'bwd': self.motor.backward,
'lfm': self.motor.left,
'rgm': self.motor.right,
}
self.LookDirectionsOptions = {
'fnt': self.panandtilt.front,
'lft': self.panandtilt.left,
'rgt': self.panandtilt.right,
'up': self.panandtilt.up,
'dwn': self.panandtilt.down,
}
def go(min = 20, max = 50, n_steps = 5, zenith = 0, samp_rate = 4400, acc_len = 1, n_accs = 10,
port = '/dev/ttyUSB0', ip = '128.135.52.192', home=True, docal=True, indef=True):
'''
Main function that creates motor, spec, and hdf5 objects, calculates the computer's offset
from ntp time, and calls snap_and_move() in order to sweep the horn through a range of
elevations and write accumulations and metadata to disk. Closes file and creates a new file
after each return to zenith.
Inputs:
Step size in degrees, zenith angle in degrees, min and max angles in degrees, sample
rate in MHz, accumulation length in seconds, number of accumulations, /dev address of
motor controller, output filename, and ip address of roach board.
Outputs:
None, writes to disk and std out.
'''
m = Motor(port = port)
angles = np.sign(min)*scan_range(min, max, n_steps)
# Flag calibration data if not scanning indefinitely
if not indef:
calext='_cal'
else:
calext='_scan'
while True:
while True:
try:
s = Spec(ip = ip, samp_rate = samp_rate, acc_len = acc_len) #re-initialize roach
break
except Exception:
pass
dt = 0 #ts.offset()
fname = '/'.join(os.path.abspath(io.__file__).split('/')[:-2])\
+ '/output/' + ts.true_time(dt) + calext + '.h5'
if home:
#print('Homing')
m.abst(0)
m.home()
if docal:
move_and_snap(m, s, fname, zenith, CALIBRATOR_POSITION + zenith, acc_len, n_accs, dt)
for destination in tqdm.tqdm(angles, unit = 'steps'):
move_and_snap(m, s, fname, zenith, destination, acc_len, n_accs, dt)
if not indef:
break
def __init__(self, address, pin, maxthrottle=200, minthrottle=500, name="motor"):
self.name = name
self.address = address
self.server = ZMQServer(address)
self.motor = Motor(pin, minthrottle, maxthrottle)
self.motor.setmin()
self.quit = False
self.engage()
class MotorConductor:
FRONT = "front"
LEFT = "left"
RIGHT = "right"
def __init__(self):
self.motor_front_left = Motor(MotorConductor.FRONT, MotorConductor.LEFT)
self.motor_front_right = Motor(MotorConductor.FRONT, MotorConductor.RIGHT)
for ndx, motor in enumerate(self.get_motors()):
motor.gear_ratio = Config.get_gear_ratio()
motor.wheel_diameter = Config.get_wheel_diameter()
motor.position_ratio = Config.get_position_ratio()[ndx]
motor.speed_ratio = Config.get_speed_ratio()[ndx]
def get_motors(self):
return [self.motor_front_left, self.motor_front_right]
def set_velocity(self, linear_velocity, angular_velocity):
width = Config.get_robot_width()
# Calculate front motors velocity
# Apply linear speed
# On the left: substract arc velocity (rad * radius = arc)
# On the right: add arc velocity (rad * radius = arc)
# Considering radians increase counterclockwise and robot front is pointing toward x+
# right wheel needs to travel more distance (and thus go faster) when the robot turns counterclockwise
front_left_speed = linear_velocity - angular_velocity * width / 2.0
front_right_speed = linear_velocity + angular_velocity * width / 2.0
self.motor_front_left.set_velocity(front_left_speed)
self.motor_front_right.set_velocity(front_right_speed)
def measured_position(self):
pass
#Set the mode of the command
#int8 MODE_STOPPED=-1
#int8 MODE_VELOCITY=0 (DEFAULT)
#int8 MODE_POSITION=1
def set_motors_mode(self, mode):
for motor in self.get_motors():
motor.set_mode(mode)
def __init__(self):
self.logging = False
self.setDefaults()
self.interface = brickpi.Interface()
self.interface.initialize()
self.events = Events()
self.motorL = Motor(self.interface, self.events, 0)
self.motorR = Motor(self.interface, self.events, 1)
self.initMotorParams(self.motorL.motorParams)
self.initMotorParams(self.motorR.motorParams)
self.initConfig()
self.touchSensorL = PushSensor('left', self.interface, 0, self.events, brickpi.SensorType.SENSOR_TOUCH)
self.touchSensorR = PushSensor('right', self.interface, 1, self.events, brickpi.SensorType.SENSOR_TOUCH)
Bumper(self.events)
self.ultraSonic = UltraSonicSensor(self.interface, 2, self.events, brickpi.SensorType.SENSOR_ULTRASONIC)
self.setPID(self.pidk_p, self.pidk_i, self.pidk_d)
self.events.add(EventType.SENSOR_TOUCH, self.sensorAction)
def __init__(self, configuration):
"""Create a new instance of the clock class.
"""
self._configuration = configuration
self._logger = logging.getLogger(__name__)
self._motor = Motor(self._configuration)
self._blinker = Blinker(self._configuration)
self._display = Display(self._configuration)
self._database = Database(self._configuration)
self._consumer = Consumer(self._configuration, self.on_message)
def __init__(self, motor_pins=(24, 25), servo_pin=0, port=2012):
self.port = port
# The motor and servo for driving
self.motor = Motor(*motor_pins)
self.servo = Servo(servo_pin)
# The most recent coordinates from the accelerameter
self.coords = (0, 0, 0)
# Whether or not to continue running the server
self._run = True
self.start()
def __init__(self):
self.joints = [
PWMJoint(
5, BASE_MIN_ANGLE, BASE_MIN_PWM, BASE_MAX_ANGLE,
BASE_MAX_PWM, BASE_COF1, BASE_COF0),
PWMJoint(
6, SHOULDER_MIN_ANGLE, SHOULDER_MIN_PWM, SHOULDER_MAX_ANGLE,
SHOULDER_MAX_PWM, SHOULDER_COF1, SHOULDER_COF0),
PWMJoint(
7, ELBOW_MIN_ANGLE, ELBOW_MIN_PWM, ELBOW_MAX_ANGLE,
ELBOW_MAX_PWM, ELBOW_COF1, ELBOW_COF0),
DyMiJoint(
1, WRIST_MIN_ANGLE, WRIST_MIN_PWM, WRIST_MAX_ANGLE,
WRIST_MAX_PWM, WRIST_COF1, WRIST_COF0)]
self.motor = Motor(4)
def init_motor(self):
# Abkürzung
settings = self.config["MOTOR"]
# Serial-Config
port = settings["Port"]
baudrate = int(settings["Baudrate"])
serial_timeout = float(settings["SerialTimeout"])
self.motor_serial = Serial(port, baudrate, timeout=serial_timeout)
# Motor-Config
motor_name = settings["Name"]
motor_config = settings["Config"]
self.motor = Motor(self.motor_serial, motor_name)
self.motor.load_motor_config(motor_config)
self.motor.scaling = float(settings["Scaling"])
def __init__(self, name, ammo):
self.name = name
self.current_degree = 0
self.ammunition_count = ammo
self.degrees = range(0, 101, 10)
self.serial_connection = serial.Serial("/dev/ttyACM0", 9600)
self.seven_seg_one = Display(SDI=11, RCLK=12, SRCLK=13)
self.seven_seg_two = Display(SDI=33, RCLK=32, SRCLK=35)
self.x_axis = Servo(0, "X Axis", self)
self.y_axis = Servo(1, "Y Axis", self)
self.release= Servo(2, "Release", self)
self.sonic = Sonic(16, 18)
self.servos = [self.x_axis, self.y_axis]
self.motor = Motor(37, 38, 40)
def __init__(self, bus, adc_addr=0x48, motor_pin1=L293_1, motor_pin2=L293_2, motor_enable = STEREO_L293_ENABLE, dirmult=1, verbose=False):
Thread.__init__(self)
self.motor = Motor(pin1=motor_pin1, pin2=motor_pin2, enable = motor_enable)
self.motor.set_speed(0)
self.adc = ADS1015(bus, adc_addr)
self.adc.write_config(MUX_AIN0 | PGA_4V | MODE_CONT | DATA_1600 | COMP_MODE_TRAD | COMP_POL_LOW | COMP_NON_LAT | COMP_QUE_DISABLE)
self.dirmult = dirmult
self.setPoint = None
self.newSetPoint = False
self.moving = False
self.daemon = True
self.verbose = verbose
self.lastStopTime = time.time()
self.start()
class ZMQMotor:
def __init__(self, address, pin, maxthrottle=200, minthrottle=500, name="motor"):
self.name = name
self.address = address
self.server = ZMQServer(address)
self.motor = Motor(pin, minthrottle, maxthrottle)
self.motor.setmin()
self.quit = False
self.engage()
def engage(self):
self.mainloop()
def mainloop(self):
while not self.quit:
try:
message = self.server.receive()
self.process_msg(message)
self.reply(message)
time.sleep(1)
except ZMQError as e:
pass
self.destroy()
def process_msg(self, message):
match = THROTTLE_REGEX.match(message)
print "Received message: " + message
if message == "laputanmachine":
self.quit=True
elif match:
throttle = int(match.group('throttle'))
self.motor.set_throttle(throttle)
else:
print message
def reply(self, message):
self.server.reply(message)
def destroy(self):
self.motor.stop()
self.server.destroy()
# Author: David # Email: [email protected] # Created: 2016-04-08 05:38 # Last modified: 2016-04-08 05:43 # Filename: adjust.py # Description: import sys import time from motor import Motor try: index = int(sys.argv[1]) rot = True if sys.argv[2] == '1' else False num = int(sys.argv[3]) m = Motor() for i in xrange(num): m.adjust(index, rot) time.sleep(0.08) m.exit() except Exception, e: print 'ERROR:', e except KeyboardInterrupt, e: print 'EXIT.'
def addMotor(sid, data):
motors.append(Motor(pi, data['name'], data['pin'], config))
sendMotors()
import sys
sys.path.insert(1, './Library/scripts')
from RTIMUScripts import get_heading
from motor import Motor
from infraredSensor import InfraredSensor
from ultrasonic import Ultrasonic
motor = Motor()
right_ir_sensor = InfraredSensor(16)
left_ir_sensor = InfraredSensor(12)
right_front_ir_sensor = InfraredSensor(20)
left_front_ir_sensor = InfraredSensor(21)
ultrasonicSensor = Ultrasonic()
components = [
motor, right_ir_sensor, left_ir_sensor, right_front_ir_sensor,
left_front_ir_sensor, ultrasonicSensor
]
car_stopped = False
def normalizeDegrees(degrees):
if 0 <= degrees < 360:
return degrees
elif degrees < 0:
return degrees + 360
elif degrees >= 360:
return degrees - 360
#!/usr/bin/env python
import rospy
import RPi.GPIO as GPIO
from std_msgs.msg import Float32
from motor import Motor
if __name__ == "__main__":
power_motor = Motor("power_motor", 22, 17, 27)
class MotorMatrix(object):
# NOTE: this could a config file or whatever, coded in this class for now
# Todo: get pinouts decided
FRONT_LEFT_PIN = 12
FRONT_RIGHT_PIN = 18
BACK_LEFT_PIN = 13
BACK_RIGHT_PIN = 19
TRANSLATION_GAIN = 1 / 10
FL = 0
FR = 1
BL = 2
BR = 3
ALL = [FL, FR, BL, BR]
FRONT_ARRAY = [FL, FR]
BACK_ARRAY = [BL, BR]
LEFT_ARRAY = [FL, BL]
RIGHT_ARRAY = [FR, BR]
CLOCKWISE_ARRAY = [FR, BL] # Clockwise rotating motors
ANTICLOCKWISE_ARRAY = [FL, BR]
def __init__(self, motor_definition: MotorDefinition = None):
#NOTE: this is very static code, may be better to loop and calculuate these, or subclass to isolate motor
if not motor_definition:
self.front_left = Motor(MotorMatrix.FRONT_LEFT_PIN)
self.front_right = Motor(MotorMatrix.FRONT_RIGHT_PIN)
self.back_left = Motor(MotorMatrix.BACK_LEFT_PIN)
self.back_right = Motor(MotorMatrix.BACK_RIGHT_PIN)
else:
self.front_left = motor_definition.front_left
self.front_right = motor_definition.front_right
self.back_left = motor_definition.back_left
self.back_right = motor_definition.back_right
# Must be aligned with MotorMatrix.ALL
self.all = [
self.front_left,
self.front_right,
self.back_left,
self.back_right,
]
def start_your_engines(self):
for motor in self.all:
motor.start()
def set_platform_controls(self, rise_normalized: float,
forward_normalized: float,
roll_right_normalized: float,
yaw_clockwise_normalized: float):
buffer_speeds = [0.0, 0.0, 0.0, 0.0]
for motor_index in MotorMatrix.ALL:
buffer_speeds[motor_index] = rise_normalized
for motor_index in MotorMatrix.FRONT_ARRAY:
buffer_speeds[
motor_index] -= MotorMatrix.TRANSLATION_GAIN * forward_normalized
for motor_index in MotorMatrix.BACK_ARRAY:
buffer_speeds[
motor_index] += MotorMatrix.TRANSLATION_GAIN * forward_normalized
for motor_index in MotorMatrix.RIGHT_ARRAY:
buffer_speeds[
motor_index] -= MotorMatrix.TRANSLATION_GAIN * roll_right_normalized
for motor_index in MotorMatrix.LEFT_ARRAY:
buffer_speeds[
motor_index] += MotorMatrix.TRANSLATION_GAIN * roll_right_normalized
for motor_index in MotorMatrix.CLOCKWISE_ARRAY:
buffer_speeds[
motor_index] -= MotorMatrix.TRANSLATION_GAIN * yaw_clockwise_normalized
for motor_index in MotorMatrix.ANTICLOCKWISE_ARRAY:
buffer_speeds[
motor_index] += MotorMatrix.TRANSLATION_GAIN * yaw_clockwise_normalized
logger.critical("Buffer speeds FL {} FR {} BR {} BL {}".format(
buffer_speeds[0],
buffer_speeds[1],
buffer_speeds[2],
buffer_speeds[3],
))
for motor_index in MotorMatrix.ALL:
motor = self.all[motor_index]
desired_speed = buffer_speeds[motor_index]
desired_speed = min(1.0, max(-1.0, desired_speed))
translated_postive = desired_speed + 1
scaled_to_percent = (translated_postive / 2) * 100
logger.critical("Motor speed index {} value of {}".format(
motor_index, scaled_to_percent))
motor.update_speed(scaled_to_percent)
def direct_test(
self,
fl_normalized: float,
fr_normalized: float,
br_normalized: float,
bl_normalized: float,
):
self.front_left.update_speed(fl_normalized * 100)
self.front_right.update_speed(fr_normalized * 100)
self.back_right.update_speed(br_normalized * 100)
self.back_left.update_speed(bl_normalized * 100)
def cleanup(self):
for motor in self.all:
motor.stop()
Motor.cleanup_all_motors()
from m5stack import lcd
from machine import I2C, Pin
from motor import Motor
from time import sleep_ms
lcd.clear()
lcd.setCursor(0, 0)
lcd.setColor(lcd.WHITE)
lcd.print("Hello World!")
i2c = I2C(scl=Pin(22), sda=Pin(21))
m = Motor(i2c=i2c, address=0x0F)
while True:
m.setMotorSpeed(100, 100)
m.setDirection(0b0000)
sleep_ms(1000)
m.setDirection(0b0101)
sleep_ms(1000)
m.setDirection(0b1010)
sleep_ms(1000)
m.setDirection(0b1111)
sleep_ms(1000)
import curses
import RPi.GPIO as GPIO
import smbus
from motor import Motor
from stepper import StepperMotor
from i2cencoder import I2CEncoder
from ArmJoint import ArmJoint
try:
GPIO.setmode(GPIO.BCM)
bus = smbus.SMBus(1)
motor1 = Motor(6, 13)
motor2 = Motor(19, 26)
stepper1 = StepperMotor(21, 20)
encoderConnection = I2CEncoder(bus, 0x08)
jointAngleRatio = 54000 * 2 * -1 # 54000 measured position manually over 1/2th of full rotation
armJoint1 = ArmJoint(motor1, encoderConnection, 0, 180, jointAngleRatio)
armJoint2 = ArmJoint(motor2, encoderConnection, 1, 180, jointAngleRatio)
with stepper1, encoderConnection, armJoint1, armJoint2:
def updateScreen(screen):
screen.clear()
screen.addstr(1, 0, str(stepper1))
def main(io):
#GPIO 14,15,18,23; PIN 8,10,12,16
#GPIO 24,25, 8, 7; PIN 18,22,24,26
#GPIO 1 ,12,16,20; PIN 28,32,26,38
#GPIO 6 ,13,19,26; PIN 31,33,35,37 (Left turn)
#GPIO 10, 9,11, 5; PIN 19,21,23,29 (
#GPIO 17,27, 3, 4; PIN 11,13,5 ,7
#22 doesn't work
#rpi2
global rack_l, motor_l, rack_b, motor_b
global is_f_upright, is_b_upright, is_l_upright, is_r_upright
rack_l = RackMotor("rl", 6, 13, 19, 26, io)
motor_l = Motor("ml", 1, 21, 16, 20, io)
rack_b = RackMotor("rb", 10, 9, 11, 5, io)
motor_b = Motor("mb", 24, 25, 8, 7, io)
comms = serial_comm()
'''
rack_l.move_deg(L_RACK_OUT)
motor_l.move_deg(L_FWD)
motor_l.move_deg(L_FWD)
motor_l.move_deg(L_REV)
motor_l.move_deg(L_FWD)
motor_l.move_deg(L_REV)
motor_l.move_deg(L_REV)
rack_l.move_deg(L_RACK_IN)
return
'''
#solution = "L' F R F' B' F B".split(" ")
#solution = "B R' R L B' F B".split(" ")
#solution = "R' F' B R U' F' U' U' B' B' L D F U' U' F' F' D' D' F' L' L' F' F' B R' R' F".split()
solution = "F' F' B U' F' L' D F' B B L B B D F F U' D D R R U U R R F F U' R R B'".split(
)
solution = "F L B B L U B' D' L' B U' F F B B D R R D B B U' D D B B".split(
)
#solution = "U U' D D' L L' R R' B B' F F'".split()
for step in solution:
print(step)
if step == "L":
if not is_f_upright:
comms.write_serial(SRL_F_RACK_OUT)
comms.read_serial()
comms.write_serial(SRL_F_FWD)
comms.read_serial()
comms.write_serial(SRL_F_RACK_IN)
comms.read_serial()
is_f_upright = True
if not is_b_upright:
rack_b.move_set_power(B_RACK_OUT)
motor_b.move_deg(B_FWD)
rack_b.move_set_power(B_RACK_IN)
is_b_upright = True
motor_l.move_deg(L_FWD)
is_l_upright = not is_l_upright
elif step == "L'":
if not is_f_upright:
comms.write_serial(SRL_F_RACK_OUT)
comms.read_serial()
comms.write_serial(SRL_F_FWD)
comms.read_serial()
comms.write_serial(SRL_F_RACK_IN)
comms.read_serial()
is_f_upright = True
if not is_b_upright:
rack_b.move_set_power(B_RACK_OUT)
motor_b.move_deg(B_FWD)
rack_b.move_set_power(B_RACK_IN)
is_b_upright = True
motor_l.move_deg(L_REV)
is_l_upright = not is_l_upright
elif step == "R":
if not is_f_upright:
comms.write_serial(SRL_F_RACK_OUT)
comms.read_serial()
comms.write_serial(SRL_F_FWD)
comms.read_serial()
comms.write_serial(SRL_F_RACK_IN)
comms.read_serial()
is_f_upright = True
if not is_b_upright:
rack_b.move_set_power(B_RACK_OUT)
motor_b.move_deg(B_FWD)
rack_b.move_set_power(B_RACK_IN)
is_b_upright = True
comms.write_serial(SRL_R_FWD)
comms.read_serial()
is_r_upright = not is_r_upright
elif step == "R'":
if not is_f_upright:
comms.write_serial(SRL_F_RACK_OUT)
comms.read_serial()
comms.write_serial(SRL_F_FWD)
comms.read_serial()
comms.write_serial(SRL_F_RACK_IN)
comms.read_serial()
is_f_upright = True
if not is_b_upright:
rack_b.move_set_power(B_RACK_OUT)
motor_b.move_deg(B_FWD)
rack_b.move_set_power(B_RACK_IN)
is_b_upright = True
comms.write_serial(SRL_R_REV)
comms.read_serial()
is_r_upright = not is_r_upright
elif step == "F":
if not is_l_upright:
rack_l.move_set_power(L_RACK_OUT)
motor_l.move_deg(L_FWD)
rack_l.move_set_power(L_RACK_IN)
is_l_upright = True
if not is_r_upright:
comms.write_serial(SRL_R_RACK_OUT)
comms.read_serial()
comms.write_serial(SRL_R_FWD)
comms.read_serial()
comms.write_serial(SRL_R_RACK_IN)
comms.read_serial()
is_r_upright = True
comms.write_serial(SRL_F_FWD)
comms.read_serial()
is_f_upright = not is_f_upright
elif step == "F'":
if not is_l_upright:
rack_l.move_set_power(L_RACK_OUT)
motor_l.move_deg(L_FWD)
rack_l.move_set_power(L_RACK_IN)
is_l_upright = True
if not is_r_upright:
comms.write_serial(SRL_R_RACK_OUT)
comms.read_serial()
comms.write_serial(SRL_R_FWD)
comms.read_serial()
comms.write_serial(SRL_R_RACK_IN)
comms.read_serial()
is_r_upright = True
comms.write_serial(SRL_F_REV)
comms.read_serial()
is_f_upright = not is_f_upright
elif step == "B":
if not is_l_upright:
rack_l.move_set_power(L_RACK_OUT)
motor_l.move_deg(L_FWD)
rack_l.move_set_power(L_RACK_IN)
is_l_upright = True
if not is_r_upright:
comms.write_serial(SRL_R_RACK_OUT)
comms.read_serial()
comms.write_serial(SRL_R_FWD)
comms.read_serial()
comms.write_serial(SRL_R_RACK_IN)
comms.read_serial()
is_r_upright = True
motor_b.move_deg(B_FWD)
is_b_upright = not is_b_upright
elif step == "B'":
if not is_l_upright:
rack_l.move_set_power(L_RACK_OUT)
motor_l.move_deg(L_FWD)
rack_l.move_set_power(L_RACK_IN)
is_l_upright = True
if not is_r_upright:
comms.write_serial(SRL_R_RACK_OUT)
comms.read_serial()
comms.write_serial(SRL_R_FWD)
comms.read_serial()
comms.write_serial(SRL_R_RACK_IN)
comms.read_serial()
is_r_upright = True
motor_b.move_deg(B_REV)
is_b_upright = not is_b_upright
else: #U, U', D, D'
if (step == 'U' or step == "U'"):
is_up = True
elif (step == 'D' or step == "D'"):
is_up = False
else:
print('Flying kite')
raise Exception('flying kite')
if is_l_upright:
if not is_f_upright:
comms.write_serial(SRL_F_RACK_OUT)
comms.read_serial()
comms.write_serial(SRL_F_FWD)
comms.read_serial()
comms.write_serial(SRL_F_RACK_IN)
comms.read_serial()
is_f_upright = True
if not is_b_upright:
rack_b.move_set_power(B_RACK_OUT)
motor_b.move_deg(B_FWD)
rack_b.move_set_power(B_RACK_IN)
is_b_upright = True
rack_l.move_set_power(L_RACK_OUT)
motor_l.move_deg(L_FWD)
is_l_upright = False
rack_l.move_set_power(L_RACK_IN)
if is_r_upright:
if not is_f_upright:
comms.write_serial(SRL_F_RACK_OUT)
comms.read_serial()
comms.write_serial(SRL_F_FWD)
comms.read_serial()
comms.write_serial(SRL_F_RACK_IN)
comms.read_serial()
is_f_upright = True
if not is_b_upright:
rack_b.move_set_power(B_RACK_OUT)
motor_b.move_deg(B_FWD)
rack_b.move_set_power(B_RACK_IN)
is_b_upright = True
comms.write_serial(SRL_R_RACK_OUT)
comms.read_serial()
comms.write_serial(SRL_R_FWD)
comms.read_serial()
is_r_upright = False
comms.write_serial(SRL_R_RACK_IN)
comms.read_serial()
# retract front and back
comms.write_serial(SRL_F_RACK_OUT)
rack_b.move_set_power(B_RACK_OUT)
comms.read_serial()
# rotate cube's U to F claw
if is_up:
comms.write_serial(SRL_R_REV)
motor_l.move_deg(L_FWD)
comms.read_serial()
else:
comms.write_serial(SRL_R_FWD)
motor_l.move_deg(L_REV)
comms.read_serial()
# rotate using F claw
comms.write_serial(SRL_F_RACK_IN)
rack_b.move_set_power(B_RACK_IN)
comms.read_serial()
if step == "U" or step == "D":
comms.write_serial(SRL_F_FWD)
comms.read_serial()
elif step == "U'" or step == "D'":
comms.write_serial(SRL_F_REV)
comms.read_serial()
is_f_upright = not is_f_upright
comms.write_serial(SRL_F_RACK_OUT)
rack_b.move_set_power(B_RACK_OUT)
comms.read_serial()
if not is_f_upright: # make it upright to prevent collision when rotating back
comms.write_serial(SRL_F_FWD)
comms.read_serial()
is_f_upright = True
if not is_b_upright: # make it upright to prevent collision when rotating back
motor_b.move_deg(B_FWD)
is_b_upright = True
# rotate back
if is_up:
comms.write_serial(SRL_R_FWD)
motor_l.move_deg(L_REV)
comms.read_serial()
else:
comms.write_serial(SRL_R_REV)
motor_l.move_deg(L_FWD)
comms.read_serial()
# reengage motors
comms.write_serial(SRL_F_RACK_IN)
rack_b.move_set_power(B_RACK_IN)
comms.read_serial()
is_l_upright = False
is_r_upright = False
comms.write_serial(SRL_STOP)
return
'''
if result != "":
to_send = f"messaged about {result} received"
comms.write_serial(to_send)
print("sending confirmation")
'''
"""
import pigpio
import time
from pigpioFIFO import pigpioFIFO
from motor import Motor, Integrator, Command
from profiler import print_prof_data
motor = Motor(17)
integrator = Integrator(motor)
fifo = pigpioFIFO(2000, 0.1)
fifo.callBack = integrator.integrate
#Command(pos, vel)
integrator.g00(1600)
integrator.g00(16000)
time.sleep(1)
while fifo.pi.wave_tx_at() != 9999:
#print_prof_data()
time.sleep(1)
time.sleep(5)
fifo.pi.wave_tx_stop()
fifo.pi.stop()
def main():
# Local functions in main function
global nextLocationIndex
global locations
global robotMotor
botCentered = False
locationAccuracy = 0.000005
print("Setting devices...")
compass = Compass("/dev/ttyACM0")
robotMotor = Motor("/dev/ttyACM1")
try:
robotGps = RobotGps()
except OSError:
print("GPSD not started!")
exit(-1)
robotMotor.moveMotor('stop')
print("All device set!")
sleep(1)
robotMotor.moveMotor('stop')
# Set the bot to point at next location
while not (170 < compass.getCompassAngle()
and compass.getCompassAngle() < 180):
print(compass.getCompassAngle())
# os.system("clear")
botCentered = False
# Rotate Rover Once
# angle = compass.getCompassAngle()-100
# while compass.getCompassAngle()>angle or compass.getCompassAngle()<angle:
# print("Calibration!")
# roverMotor.moveMotor('left')
robotMotor.moveMotor("stop")
os.system("clear")
print("Rover centered!")
robotMotor.moveMotor('stop')
sleep(2)
# roverMotor.resetAllMotors()
print("Locations:", locations)
# input('Press anything to continue!')
# =========
# Main Loop
# =========
while nextLocationIndex < len(locations):
# roverMotor.resetAllMotors()
try:
currentLocation = robotMotor.getGpsData()
# print(roverGps)
except ValueError:
print("GPS not set")
continue
if currentLocation[0] == None:
print("GPS no location")
continue
if abs(currentLocation[0] -
locations[nextLocationIndex][0]) < locationAccuracy and abs(
currentLocation[1] -
locations[nextLocationIndex][1]) < locationAccuracy:
robotMotor.moveMotor("stop")
nextLocationIndex += 1
if nextLocationIndex >= len(locations):
break
print(locations)
print("Location Reached!", currentLocation)
print("Press any key to continue")
input()
# sleep(2)
# Center bot to north
botCentered = False
while not botCentered:
# os.system("clear")
if centerBot(compass, 0, robotGps, robotMotor, 20):
print()
botCentered = True
botCentered = False
print("Continue to location", locations[nextLocationIndex])
sleep(1)
# input()
continue
slope = getSlope(currentLocation)
# Move the rover to this slope
while not botCentered:
# os.system("clear")
slope = getSlope(currentLocation)
if centerBot(compass, slope, robotGps, robotMotor, 10):
botCentered = True
# sleep(0.5)
if not centerBot(compass, slope, robotGps, robotMotor, 10):
print()
botCentered = False
# # Move bot forward
# # os.system("clear")
try:
print("Move Forward",
robotGps.getGpsData(), locations[nextLocationIndex], slope,
compass.getCompassAngle())
# roverMotor.moveMotor('forward')
robotMotor.moveMotor('forward')
except ValueError:
print("Compass Value error")
robotMotor.moveMotor('stop')
print("Finished!")
from motor import Motor
from microbit import *
import music
m = Motor(pin0, pin12, pin13)
def count_ntimes(n):
for _ in range(n):
display.show(_)
sleep(1000)
def play_beep():
music.play(music.BA_DING, pin=pin8)
display.scroll("waiting...", wait=False)
while True:
if button_a.is_pressed():
m.start_forward(speed=500)
count_ntimes(5)
m.stop()
play_beep()
display.clear()
if button_b.is_pressed():
m.start_backward(speed=500)
count_ntimes(5)
m.stop()
play_beep()
display.clear()
def __init__(self,
name,
pinRightFwd,
pinRightRev,
pinLeftFwd,
pinLeftRev,
wheel_diameter=.065,
wheel_base=0.15,
left_max_rpm=200.0,
right_max_rpm=200.0,
frequency=20):
"""
Parameters
----------
name: str
The node name that will be used for this robot; defaults
to "wheelie"
pinRightFwd : int
The RaspPi GPIO pin that goes high to create forward motion
on the right wheel
pinRightRev : int
The RaspPi GPIO pin that goes high to create reverse motion
on the right wheel
pinLeftFwd : int
The RaspPi GPIO pin that goes high to create forward motion
on the right wheel
pinLeftRev : int
The RaspPi GPIO pin that goes high to create reverse motion
on the right wheel
wheel_diameter : float
The diameter of the wheels in meters
wheel_base : float
The distance between the center of the wheels in meters
left_max_rpm : float
The number of revolutions per minute made by the left motor
when running at 100% power
right_max_rpm : float
The number of revolutions per minute made by the left motor
when running at 100% power
frequency : int
The frequency in Hz used to control the PWM motors
"""
super().__init__(name)
self._frequency = frequency
self._left_max_rpm = left_max_rpm
self._right_max_rpm = right_max_rpm
self._wheel_diameter = wheel_diameter
self._wheel_base = wheel_base
self._rightWheel = Motor(pinRightFwd, pinRightRev, frequency)
self._leftWheel = Motor(pinLeftFwd, pinLeftRev, frequency)
self.speed = 0.0
self.spin = 0.0
self.close = 0.30 # start slowing down when this close
self.tooclose = 0.10 # no forward motion when this close
self.distance = 100.0
self._command_subscription = self.create_subscription(
String, 'command', self._command_callback, 10)
self._cmd_vel_subscription = self.create_subscription(
Twist, 'cmd_vel', self._cmd_vel_callback, 2)
self._joy_subscription = self.create_subscription(
Joy, 'joy', self._joy_callback, 5)
self._range_subscription = self.create_subscription(
Range, 'range', self._range_callback, 5)
from __future__ import print_function
#
# Sample demo for obstacle detector (od)
#
import sys
import time
import RPi.GPIO as GPIO
sys.path.append('..')
from ObstacleDetector import ObstacleDetector
from car_config import *
from motor import Motor
import time
left = Motor(LFP, LBP, LEP)
right = Motor(RFP, RBP, REP)
def stop(distance):
print("Distance is {0}".format(distance))
left.off()
right.off()
time.sleep(1)
left.forward(60)
right.backward(60)
time.sleep(1)
left.forward(50)
right.forward(50)
return
class Wheelie(Node):
'''Wheelie node suitable for a RPi robot with two PWM driven motors
Creates listener on /command to accept string-style commands.
Creates listener on /cmd_vel to accept twist messages.
Creates listener on /joy to accept xbox joystick input.
Attributes
----------
speed : float
Speed along the X axis in meters per second; positive is
forward and negative is backward
spin : float
Rotation about the pivot point in radians per second; positive
is clockwise when viewed from above (right spin)
Methods
-------
stop()
Stop all movement of the wheelie
'''
def __init__(self,
name,
pinRightFwd,
pinRightRev,
pinLeftFwd,
pinLeftRev,
wheel_diameter=.065,
wheel_base=0.15,
left_max_rpm=200.0,
right_max_rpm=200.0,
frequency=20):
"""
Parameters
----------
name: str
The node name that will be used for this robot; defaults
to "wheelie"
pinRightFwd : int
The RaspPi GPIO pin that goes high to create forward motion
on the right wheel
pinRightRev : int
The RaspPi GPIO pin that goes high to create reverse motion
on the right wheel
pinLeftFwd : int
The RaspPi GPIO pin that goes high to create forward motion
on the right wheel
pinLeftRev : int
The RaspPi GPIO pin that goes high to create reverse motion
on the right wheel
wheel_diameter : float
The diameter of the wheels in meters
wheel_base : float
The distance between the center of the wheels in meters
left_max_rpm : float
The number of revolutions per minute made by the left motor
when running at 100% power
right_max_rpm : float
The number of revolutions per minute made by the left motor
when running at 100% power
frequency : int
The frequency in Hz used to control the PWM motors
"""
super().__init__(name)
self._frequency = frequency
self._left_max_rpm = left_max_rpm
self._right_max_rpm = right_max_rpm
self._wheel_diameter = wheel_diameter
self._wheel_base = wheel_base
self._rightWheel = Motor(pinRightFwd, pinRightRev, frequency)
self._leftWheel = Motor(pinLeftFwd, pinLeftRev, frequency)
self.speed = 0.0
self.spin = 0.0
self.close = 0.30 # start slowing down when this close
self.tooclose = 0.10 # no forward motion when this close
self.distance = 100.0
self._command_subscription = self.create_subscription(
String, 'command', self._command_callback, 10)
self._cmd_vel_subscription = self.create_subscription(
Twist, 'cmd_vel', self._cmd_vel_callback, 2)
self._joy_subscription = self.create_subscription(
Joy, 'joy', self._joy_callback, 5)
self._range_subscription = self.create_subscription(
Range, 'range', self._range_callback, 5)
def stop(self):
self._leftWheel.stop()
self._rightWheel.stop()
def max_speed(self):
'''Speed in meters per second at maximum RPM'''
rpm = (self._left_max_rpm + self._right_max_rpm) / 2.0
mps = rpm * math.pi * self._wheel_diameter / 60.0
return mps
def max_twist(self):
'''Rotation in radians per second at maximum RPM'''
return self.max_speed() / self._wheel_diameter
def _forward(self, speed=100):
self._rightWheel.forward(speed)
self._leftWheel.forward(speed)
def _reverse(self, speed=100):
self._rightWheel.reverse(speed)
self._leftWheel.reverse(speed)
def _left(self, speed=100):
self._rightWheel.reverse(speed)
self._leftWheel.forward(speed)
def _right(self, speed=100):
self._rightWheel.forward(speed)
self._leftWheel.reverse(speed)
def _command_callback(self, msg):
command = msg.data
if command == 'forward':
self._forward()
elif command == 'reverse':
self._reverse()
elif command == 'left':
self._left()
elif command == 'right':
self._right()
elif command == 'stop':
self.stop()
else:
print('Unknown command, stopping instead')
self.stop()
def _joy_callback(self, msg):
'''Translate XBox buttons into speed and spin
Just use the left joystick (for now):
LSB left/right axes[0] +1 (left) to -1 (right)
LSB up/down axes[1] +1 (up) to -1 (back)
LB buttons[5] 1 pressed, 0 otherwise
'''
if abs(msg.axes[0]) > 0.10:
self.spin = msg.axes[0]
else:
self.spin = 0.0
if abs(msg.axes[1]) > 0.10:
self.speed = msg.axes[1]
else:
self.speed = 0.0
if msg.buttons[5] == 1:
self.speed = 0
self.spin = 0
self._set_motor_speeds()
def _cmd_vel_callback(self, msg):
self.speed = msg.linear.x
self.spin = msg.angular.z
self._set_motor_speeds()
def _range_callback(self, msg):
self.distance = msg.range
self._set_motor_speeds()
def _set_motor_speeds(self):
# TODO: inject a stop() if no speeds seen for a while
#
# Scary math ahead.
#
# First figure out the speed of each wheel based on spin: each wheel
# covers self._wheel_base meters in one radian, so the target speed
# for each wheel in meters per sec is spin (radians/sec) times
# wheel_base divided by wheel_diameter
#
right_twist_mps = self.spin * self._wheel_base / self._wheel_diameter
left_twist_mps = -1.0 * self.spin * \
self._wheel_base / self._wheel_diameter
#
# Now add in forward motion.
#
left_mps = self.speed + left_twist_mps
right_mps = self.speed + right_twist_mps
#
# Convert meters/sec into RPM: for each revolution, a wheel travels
# pi * diameter meters, and each minute has 60 seconds.
#
left_target_rpm = (left_mps * 60.0) / (math.pi * self._wheel_diameter)
right_target_rpm = (right_mps * 60.0) / \
(math.pi * self._wheel_diameter)
#
left_percentage = (left_target_rpm / self._left_max_rpm) * 100.0
right_percentage = (right_target_rpm / self._right_max_rpm) * 100.0
#
# clip to +- 100%
left_percentage = max(min(left_percentage, 100.0), -100.0)
right_percentage = max(min(right_percentage, 100.0), -100.0)
#
# Add in a governor to cap forward motion when we're about
# to collide with something (but still backwards motion)
governor = 1.0
if self.distance < self.tooclose:
governor = 0.0
elif self.distance < self.close:
governor = (self.distance - self.tooclose) / \
(self.close - self.tooclose)
if right_percentage > 0:
right_percentage *= governor
if left_percentage > 0:
left_percentage *= governor
#
self._rightWheel.run(right_percentage)
self._leftWheel.run(left_percentage)
class Car:
min_stop_sensor_dist = 25
"""Distance given in centimeters"""
def __init__(self, m1_forward, m1_backward, m2_forward, m2_backward,
m3_forward, m3_backward, m4_forward, m4_backward, front_trig,
front_echo, side_trig, side_echo, back_trig, back_echo):
GPIO.setmode(GPIO.BCM)
self.motor1 = Motor(m1_forward, m1_backward)
self.motor2 = Motor(m2_forward, m2_backward)
self.motor3 = Motor(m3_forward, m3_backward)
self.motor4 = Motor(m4_forward, m4_backward)
self.front_sensor = DistSensor(front_trig, front_echo)
self.side_sensor = DistSensor(side_trig, side_echo)
self.back_sensor = DistSensor(back_trig, back_echo)
self.running = False
# Pool size = number of distance sensors
dist_sensor_count = 3
self.pool = ThreadPool(dist_sensor_count)
def move_forward(self):
print('Moving the car forward ...')
self.motor1.move_forward()
self.motor2.move_forward()
self.motor3.move_forward()
self.motor4.move_forward()
def move_backward(self):
print('Moving the car backward ...')
self.motor1.move_backward()
self.motor2.move_backward()
self.motor3.move_backward()
self.motor4.move_backward()
def stop(self):
print('Stopping the car ...')
self.motor1.stop()
self.motor2.stop()
self.motor3.stop()
self.motor4.stop()
def read_distance(self, sensor):
return sensor.read_distance()
def read_distances(self):
# s1 = self.front_sensor.read_distance()
# s2 = self.side_sensor.read_distance()
# s3 = self.back_sensor.read_distance()
async_front = self.pool.apply_async(self.read_distance,
(self.front_sensor, ))
async_side = self.pool.apply_async(self.read_distance,
(self.side_sensor, ))
async_back = self.pool.apply_async(self.read_distance,
(self.back_sensor, ))
front_dist = async_front.get()
side_dist = async_side.get()
back_dist = async_back.get()
emergency_stop_front = front_dist < Car.min_stop_sensor_dist
emergency_stop_side = side_dist < Car.min_stop_sensor_dist
emergency_stop_back = back_dist < Car.min_stop_sensor_dist
print("Front = %.2f cm, side = %.2f cm, back %.2f cm" \
% (front_dist, side_dist, back_dist))
return front_dist, side_dist, back_dist, emergency_stop_front, emergency_stop_side, emergency_stop_back
def cleanup(self):
GPIO.cleanup()
import pwm
from motor import Motor
pwm.enable()
motor = Motor()
# pwmLeft, pwmRight, gpioLF, gpioLB, gpioRF, gpioRB
motor.attach("P9_16", "P9_31", "48", "49", "117", "115")
print "Attach successful!"
# move forward for 3 seconds
motor.move(3, "forward")
print "Move forward 3 seconds successful!"
# move left for 3 seconds
motor.direction(3, "left")
print "Move left 3 seconds successful!"
# move right for 3 seconds
motor.direction(3, "right")
print "Move right 3 seconds successful!"
# move back for 3 seconds
motor.move(3, "backward")
print "Move backward 3 seconds successful!"
motor.detach()
def cleanup(self):
for motor in self.all:
motor.stop()
Motor.cleanup_all_motors()
#!/usr/bin/env python
import rospy
from std_msgs.msg import String, Float64
from geometry_msgs.msg import Twist
from tf2_msgs.msg import TFMessage
from geometry_msgs.msg import TransformStamped
from nav_msgs.msg import Odometry
from math import *
import pigpio
from motor import Motor
from omni_Whiles import Omni_Control
pi = pigpio.pi()
ur_motor = Motor(pi, 14, 15)
ul_motor = Motor(pi, 17, 18)
dr_motor = Motor(pi, 22, 23)
dl_motor = Motor(pi, 9, 25)
motors = [ur_motor, ul_motor, dl_motor, dr_motor]
omni_control = Omni_Control(motors, 0.5)
twist_last = Twist()
twist_enable = False
jointstate = TFMessage()
def twist_stamped_callback(twist_msg):
global twist_last
global twist_enable
twist_last = twist_msg
twist_enable = True
class PiCar:
'''Builds a PiCar class'''
def __init__(self):
#TODO: impliment some behaviour if no client is found
#client = ClientConnection()
self.tilt = Servo(pwm_pin=0, center=515, minVal=425, maxVal=650)
self.pan = Servo(1, 330, 60, 600)
self.wheel = Servo(2, 370, 180, 520)
#motor = Motor(4,14,15)
self.motor = Motor(17, 27, 18)
self.sonar = UltraSounder()
self.camera = Camera(sonar=self.sonar, clientAddr=None)
#TODO: give car access to leds
# mainly so it can do the police car thing
def drive_f(self, speed=100):
'''
Drive forward
If no speed is specified, go full speed
'''
self.motor.drive(speed)
pass
def drive_r(self, speed=100):
'''
Drive in reverse
If no speed is specified, go full speed
'''
self.motor.drive(-speed)
pass
def turn_l(self, angle=None):
'''
Turn wheels to the left
If no angle specified, incriment from current position
'''
if angle:
self.wheel.rotate(angle)
else:
self.wheel.goto_max()
pass
def turn_r(self, angle=None):
'''
Turn wheels to the right
If no angle specified, incriment from current position
'''
if angle:
self.wheel.rotate(-angle)
else:
self.wheel.goto_min()
pass
def look_l(self, angle=None):
'''
Pan head to the left
If no angle specified, incriment from current position
'''
if angle:
self.pan.rotate(angle)
else:
self.pan.goto_max()
pass
def look_r(self, angle=None):
'''
Pan head to the right
If no angle specified, incriment from current position
'''
if angle:
self.pan.rotate(-angle)
else:
self.pan.goto_min()
pass
def look_u(self, angle=None):
'''
Tilt head up
If no angle specified, incriment from current position
'''
if angle:
self.tilt.rotate(angle)
else:
self.pan.goto_max()
pass
def look_d(self, angle=None):
'''
Tilt head down
If no angle specified, incriment from current position
'''
if angle:
self.tilt.rotate(-angle)
else:
self.pan.goto_min()
pass
def all_stop(self):
'''Stop motor'''
self.motor.stop()
pass
def all_ahead(self):
'''Move all servos to center position'''
self.wheel.goto_center()
self.pan.goto_center()
self.tilt.goto_center()
pass
def sonar_scan(self, ):
'''Scan sonar from left to right'''
pass
def __del__(self):
'''Close everything nicely so the resources can be used again'''
self.camera.close()
pass #TODO: impliment code that closes everything nicely
def shakedown(self):
'''Run through all capabilities'''
pass
class Controller(object):
"""Abstractions to interacts with the motors."""
def __init__(self):
"""Init."""
self.display = Display()
self.r_motor = Motor(Millimeters(0.16)) # 0.16mm per step
self.r_motor.set_hard_range(Millimeters(0.0), TABLE_RADIUS_MM)
self.r = 0.0
# self.r_motor.set_step(0.5) # Half step mode
self.t_motor = Motor(Degrees(0.05).radians()) # 0.05deg per step
self.grid_size = PolarResolution(self.r_motor.effective_step_distance, self.t_motor.effective_step_distance)
# def calculate_grid_size(self):
# circumference = math.tau * self.r
# # mm_per_raidian = circumference / Radiansmath.tau
# target_step_mm = Millimeters(0.1)
# target_steps = circumference / target_step_mm
# return PolarResolution(self.r_motor.effective_step_distance, Radiansmath.tau / target_steps)
def __str__(self):
"""Str."""
return u'Controller(<{0}>, <{1}>)'.format(self.t_motor, self.r_motor)
def step_r(self, steps):
"""Step the radius motor."""
self.display.hud['step_r'] = 'r stepping {}'.format(steps)
for movement in self.r_motor.steps(steps):
self.r += movement
yield movement
self.display.hud['step_r'] = 'r idle'
def step_t(self, steps):
"""Step the radius motor."""
self.display.hud['step_t'] = 't stepping {}'.format(steps)
for movement in self.t_motor.steps(steps):
yield movement
self.display.hud['step_t'] = 't idle'
if FAST_SKIP_MOTORS:
def step(self, steps):
"""Step the motors in sync."""
biggest = int(max(abs(steps.r), abs(steps.t)))
step_size = Steps(float(steps.r) / biggest, float(steps.t) / biggest)
accumulation = Steps(0.0, 0.0)
for i in range(biggest):
accumulation += step_size
while abs(accumulation.r) >= 1.0 or abs(accumulation.t) >= 1.0:
this_step = Polar(0.0, 0.0)
if abs(accumulation.r) >= 1.0:
r_step = self.r_motor.effective_step_distance * (self.r_motor.step_size * sign(steps.r))
this_step.r = r_step
accumulation.r = accumulation.r - sign(steps.r)
if abs(accumulation.t) >= 1.0:
this_step.t = self.t_motor.effective_step_distance * (self.t_motor.step_size * sign(steps.t))
accumulation.t = accumulation.t - sign(steps.t)
yield this_step
else:
def step(self, steps):
"""Step the motors in sync."""
biggest = int(max(abs(steps.r), abs(steps.t)))
step_size = Steps(float(steps.r) / biggest, float(steps.t) / biggest)
self.r_motor.set_direction(sign(steps.r))
self.t_motor.set_direction(sign(steps.t))
accumulation = Steps(0.0, 0.0)
for i in range(biggest):
accumulation += step_size
while abs(accumulation.r) >= 1.0 or abs(accumulation.t) >= 1.0:
this_step = Polar(0.0, 0.0)
if abs(accumulation.r) >= 1.0:
r_step = self.r_motor.step()
this_step.r = r_step
if r_step.is_boundry:
accumulation.r = 0.0
else:
accumulation.r = accumulation.r - sign(steps.r)
if abs(accumulation.t) >= 1.0:
this_step.t = self.t_motor.step()
accumulation.t = accumulation.t - sign(steps.t)
yield this_step
def motor_step_distance(self):
"""Return the step size of the motors."""
return Polar(self.r_motor.step_distance, self.t_motor.step_distance)
def __init__(self, bus):
super(MotorPlugin, self).__init__(bus)
logger.info('Initializing MotorPlugin')
self.motor = Motor(L1,L2,R1,R2,INIT)
def __init__(self, name):
Motor.__init__(self, name, name)
self.serial_com = serial.Serial(Config.get_swivel_encoder_port(),Config.get_swivel_encoder_baud())
self._last_angle = 0
class MotorKit: """ the 4 Adafruit DC motors kit""" def __init__(self, pca): self.pca = pca self.motor1 = Motor(self.pca, [0,1]) self.motor2 = Motor(self.pca, [3,2]) self.motor3 = Motor(self.pca, [4,5]) self.motor4 = Motor(self.pca,[6,7]) self.speed = 0.5 #default to 0.5 change it when needed, turns need slower def changeSpeed(self, speed): self.speed = speed def forward(self): """ move all motors forward for point one second """ self.motor1.moveForwardwithSpeed(self.speed,0) self.motor2.moveForwardwithSpeed(self.speed,0) self.motor3.moveForwardwithSpeed(self.speed,0) self.motor4.moveForwardwithSpeed(self.speed,0) time.sleep(0.1) def backward(self): """ move all motors backward for point one second """ self.motor1.moveBackwardwithSpeed(self.speed,0) self.motor2.moveBackwardwithSpeed(self.speed,0) self.motor3.moveBackwardwithSpeed(self.speed,0) self.motor4.moveBackwardwithSpeed(self.speed,0) time.sleep(0.1) def stop(self): """ stop all movements""" self.motor1.stop() self.motor2.stop() self.motor3.stop() self.motor4.stop() def forwardLeft(self): self.motor4.moveForwardwithSpeed(self.speed,0) self.motor3.moveForwardwithSpeed(self.speed,0) time.sleep(0.1) def forwardRight(self): self.motor1.moveForwardwithSpeed(self.speed,0) self.motor2.moveForwardwithSpeed(self.speed,0) time.sleep(0.1) def backwardLeft(self): self.motor1.moveBackwardwithSpeed(self.speed,0) self.motor2.moveBackwardwithSpeed(self.speed,0) time.sleep(0.1) def backwardRight(self): self.motor3.moveBackwardwithSpeed(self.speed,0) self.motor4.moveBackwardwithSpeed(self.speed,0) time.sleep(0.1)
def test_load_drivetrain_from_file(self):
testbot = SimpleDrivetrain()
filepath = 'drivetrain_test.xml'
testbot.load_drivetrain_from_file(filepath)
observed_motors = testbot.motors
pwm_bounds_std = (1100, 1500, 1900)
norm_const = np.sqrt(2) / 2
expected_motors = [
Motor('front_right', (1, 1, 0), (-norm_const, norm_const, 0), True,
pwm_bounds_std),
Motor('front_left', (-1, 1, 0), (norm_const, norm_const, 0), False,
pwm_bounds_std),
Motor('back_left', (-1, -1, 0), (norm_const, -norm_const, 0),
False, pwm_bounds_std),
Motor('back_right', (1, -1, 0), (-norm_const, -norm_const, 0),
False, pwm_bounds_std),
Motor('front_ascent', (0, 0.5, 0), (0, 0, 1), False,
pwm_bounds_std),
Motor('back_ascent', (0, -0.5, 0), (0, 0, 1), False,
pwm_bounds_std)
]
expected_orientation = (0, 0, 0)
observed_orientation = testbot.orientation
for i in range(0, len(expected_orientation)):
self.assertAlmostEqual(expected_orientation[i],
observed_orientation[i])
for i in range(0, len(expected_motors)):
observed_motor = observed_motors[i]
expected_motor = expected_motors[i]
observed_name = observed_motor.name
expected_name = expected_motor.name
observed_inverted = observed_motor.inverted
expected_inverted = expected_motor.inverted
observed_position = observed_motor.position
expected_position = expected_motor.position
observed_direction = observed_motor.direction
expected_direction = expected_motor.direction
observed_pwm_bounds = observed_motor.pwm_bounds
expected_pwm_bounds = expected_motor.pwm_bounds
self.assertEqual(expected_name, observed_name)
self.assertEqual(expected_inverted, observed_inverted)
for j in range(0, len(expected_position)):
self.__assertWithinRange(expected_position[j],
observed_position[j], 0.01)
for j in range(0, len(expected_direction)):
self.__assertWithinRange(expected_direction[j],
observed_direction[j], 0.01)
for j in range(0, len(expected_pwm_bounds)):
self.assertEqual(expected_pwm_bounds[j],
observed_pwm_bounds[j], 0.01)
U.turn90CW()
U.turn90CW()
else:
U.turn90(directionFlag)
if(move[0] == 'D'):
if(twoMoves):
D.turn90CW()
D.turn90CW()
else:
D.turn90(directionFlag)
twoMoves = False
directionFlag = False
turnDelay = .005 / 8 * 1/2
F = Motor(26,19)
L = Motor(13,6)
R = Motor(21, 20)
B = Motor(16,12)
U = Motor(5, 22)
D = Motor(27, 17)
screen = LCD.lcd()
screen.lcd_clear()
screen.lcd_display_string("hay 6 motores", 1)
F.setDelay(turnDelay)
L.setDelay(turnDelay)
R.setDelay(turnDelay)
B.setDelay(turnDelay)
U.setDelay(turnDelay)
Ridwan Alaoudian Tyler Bursa """ # Import classes from button import Button from linear_actuator import LinearActuator from load_cell_amplifier import LoadCellAmplifier from load_cell import LoadCell from motor import Motor from rotary_encoder import RotaryEncoder import multiprocessing # Initialize objects motor = Motor(20, 21) linear_actuator = LinearActuator(motor) rotary_encoder = RotaryEncoder(9, 11, 10) load_cell_amplifier = LoadCellAmplifier(5, 6) load_cell = LoadCell(load_cell_amplifier) # Initialize buttons up_button = Button(27, linear_actuator.move_up) down_button = Button(17, linear_actuator.move_down) stop_button = Button(2, linear_actuator.stop) inc_speed_button = Button(7, linear_actuator.increase_speed) dec_speed_button = Button(3, linear_actuator.decrease_speed) # Initialize limit switches bottom_limit_switch = Button(19, linear_actuator.stop) top_limit_switch = Button(26, linear_actuator.stop)
class Predict:
"""
Class for making a prediction based on real hardware
"""
def __init__(self, directory, model):
self.current_position = 0
self.iteration = 0
self.directory = directory
self.predictions = []
self.model = model
self.rotation = 0
self.mic_centre = np.array([1.5, 1.5])
self.prediction = 0
self.audio = audio_recorder(self.directory)
self.motor = Motor()
self.current_model = self.get_model() # init
self.motor_offset = 0
self.relative_prediction = 0
self.results = []
self.initial_adjust = False
def store_prediction(self, doa_list):
"""
convert relative prediction to home coordinates
"""
true_doas = [utility_methods.cylindrical(self.current_position + doa_list[0]),
utility_methods.cylindrical(self.current_position + doa_list[1])]
self.predictions.append(true_doas)
def get_model(self):
model = None
if self.model == "gcc_cnn":
model = final_models.gcc_cnn()
elif self.model == "gcc_dsp":
model = final_models.gcc_dsp()
else:
print("Error -> No file found")
return model
def record(self):
self.motor.red_led_on()
self.audio.record(self.iteration)
self.motor.red_led_off()
def load_audio(self):
"""
Reads wav file based on csv values, resamples audio to 8000hz, fixes length to 1 second
:return: numpy array of stereo audio, DOA from file
"""
df = pd.read_csv("{dir}/iteration_{iter}.csv".format(dir=self.directory, iter=self.iteration),
usecols=[1, 2])
wav_name = df.iloc[0][0]
filename = "{wav_name}".format(wav_name=wav_name)
y, sr = librosa.load(filename, mono=False)
y_8k = librosa.resample(y, sr, 8000)
o_env = librosa.onset.onset_strength(y_8k[0], sr=8000)
peaks = librosa.util.peak_pick(o_env, 3, 3, 3, 5, 0.25, 5)
times = librosa.frames_to_time(np.arange(len(o_env)),
sr=8000, hop_length=512)
peak_times = times[peaks]
time = 0
for i in range(1, len(peak_times) + 1):
if 3 - peak_times[-i] >= 0.75:
time = peak_times[-i] - 0.25
break
sample = librosa.time_to_samples(np.array([time]), sr=8000)
sliced_y = np.array([y_8k[0][sample[0]:], y_8k[1][sample[0]:]])
y_out = librosa.util.fix_length(sliced_y, 8000)
return y_out
def format_gcc_cnn(self):
"""
Format the stereo Audio file for input to the gcc_phat CNN
:return: data formatted for gcc_phat CNN, DOA read from file
"""
result_x = self.load_audio()
signal = result_x[0]
reference_signal = result_x[1]
_, raw_gcc_vector = gccphat.gcc_phat(signal=signal, reference_signal=reference_signal, fs=8000)
cross_correlation_vector = cnn.reshape_x_for_cnn(cnn.normalize_x_data(np.array([raw_gcc_vector])))
return cross_correlation_vector
def format_gcc_dsp(self):
"""
Format stereo audio file for gcc_dsp model
:return: signal, reference signal and doa_from_file
"""
result_x = self.load_audio()
return result_x
def load_and_process_audio(self):
"""
Wrapping loading and processing of models into a single function
"""
output_vector = None
if self.model == "gcc_cnn":
output_vector = self.format_gcc_cnn()
elif self.model == "gcc_dsp":
output_vector = self.format_gcc_dsp()
else:
print("Error -> No file found")
return output_vector
def compute_rotation(self):
"""
compute rotation based on current and prior predictions
:return:
"""
if self.predictions[self.iteration][0] == 90.0 or self.predictions[self.iteration][0] == 270.0:
self.rotation = 20
self.initial_adjust = True
return
if self.iteration == 0:
self.rotation = rotate.get_90_deg_rotation(self.predictions[self.iteration])
elif self.iteration == 1:
self.rotation = rotate.get_45_deg_rotation(self.predictions, self.current_position)
elif self.iteration >= 2:
self.rotation = rotate.get_fine_rotation(self.iteration)
def update_position(self):
"""
update current position of microphone based on rotation
:param rotation:
:return:
"""
self.current_position = utility_methods.cylindrical(self.current_position + self.rotation)
self.motor.rotate(np.abs(self.rotation), "cw" if self.rotation < 0 else "ccw")
def compute_rotation_to_prediction(self):
self.rotation = self.prediction - self.current_position - 90
# self.rotation = ((self.prediction - self.current_position) + 180) % 360 - 180 -90
def reset(self):
"""
This method resets the prediction, iteration, position and rotation values to initial state. Rotates The
motor back to 0 degrees
:return:
"""
self.rotation = -(self.prediction - 90)
self.update_position()
self.iteration = 0
self.predictions = []
self.prediction = 0
self.change_motor_offset(-self.motor_offset)
def change_motor_offset(self, offset):
self.motor_offset = offset
self.motor.rotate(np.abs(self.motor_offset), "ccw" if self.motor_offset < 0 else "cw")
def save_results(self):
results_row = [self.prediction, self.motor_offset, self.prediction - self.motor_offset, self.iteration]
self.results.append(results_row)
#
def run(self):
while self.iteration < 6:
self.record()
print("Recording Successful")
vector = self.load_and_process_audio()
doa_list = self.current_model.predict(vector)
print("Model Prediction: {list}".format(list=doa_list))
self.store_prediction(doa_list)
print("Prediction List: {list}".format(list=self.predictions))
val = utility_methods.check_if_twice(self.predictions, self.iteration)
if val is not None:
self.prediction = val
self.compute_rotation_to_prediction()
self.update_position()
return self.prediction
self.compute_rotation()
print("Rotation: {rotation}".format(rotation=self.rotation))
self.update_position()
print("Current Position: {position}".format(position=self.current_position))
self.iteration += 1
self.prediction = utility_methods.get_mean_prediction(prediction_list=self.predictions)
self.compute_rotation_to_prediction()
self.update_position()
self.motor.green_led()
return self.prediction
def evaluate(self):
for i in np.arange(0, 360, 45):
self.change_motor_offset(i)
self.run()
self.save_results()
self.reset()
df = pd.DataFrame(self.results, columns=['prediction', 'offset', 'prediction - offset', 'Number of iterations'])
df.to_csv("{dir}/iteration_{model}.csv".format(dir="evaluation_results", model=self.model))
robotY = 800
robot = Robot(screen, robotX, robotY)
y_change = 0
x_change = 0
barrierList = []
joysticks = []
pygame.joystick.init()
c = Controller()
fl = Motor(4)
fr = Motor(17)
br = Motor(22)
bl = Motor(27)
auto = Automobile(fr, br, fl, bl)
angle = 0
rotation = ''
running = True
while running:
screen.fill((0,0,0))
for event in pygame.event.get():
if event.type == pygame.QUIT:
from motor import Motor
import time
m = Motor('moteur 1', 5, 3, 10)
m.start()
time.sleep(3)
m.setPwmRapport(40)
time.sleep(1)
m.setPwmRapport(50)
time.sleep(1)
m.setPwmRapport(60)
time.sleep(1)
m.setPwmRapport(70)
time.sleep(1)
m.setPwmRapport(80)
time.sleep(1)
m.setPwmRapport(90)
time.sleep(1)
m.setPwmRapport(70)
time.sleep(1)
m.setPwmRapport(50)
time.sleep(1)
m.reverse(True)
time.sleep(3)
m.reverse(True)
time.sleep(3)
m.stop()
m.clear()
def panic_button(y, b, w, bl, io):
m1 = Motor(y, b, w, bl, io)
moves = [0, 0, 0, 0]
for move in moves:
m1.move_deg(move)
from motor_angle_control import MotorAngleControl
from motor import Motor
from encoder import Encoder
from car_config import gpio_dict
# 左侧电机
lmotor = Motor(gpio_dict['LEFT_MOTOR_A'],
gpio_dict['LEFT_MOTOR_B']) #,motor_install_dir=False)
# 右侧电机
rmotor = Motor(gpio_dict['RIGHT_MOTOR_A'], gpio_dict['RIGHT_MOTOR_B'])
# 左侧编码器
lencoder = Encoder(gpio_dict['LEFT_ENCODER_A'],
gpio_dict['LEFT_ENCODER_A'],
3,
motor_install_dir=False,
name='LeftEncoder',
is_debug=False)
mac = MotorAngleControl(lmotor, lencoder, kp=-10, is_debug=True)
mac.set_angle(90, is_reset=True)
import ultrasonic
from time import sleep
#from machine import Pin
from motor import Motor
motor_left = Motor("left", "D6", "D7", "D4")
motor_right = Motor("right", "D8", "D9", "D5")
print("Hello, world!") # Print a welcome message on reset
# These statements make the code more readable.
# Instead of a pin number "D13" or "D12" we can now write "TRIG" or "ECHO"
TRIG = "D13"
ECHO = "D12"
ultrasonic_sensor = ultrasonic.HCSR04(TRIG, ECHO)
while True:
dist = ultrasonic_sensor.distance_mm()
if dist >= 60:
print('if')
#motor_left.ctrl_alloc(1, 70)
#motor_right.ctrl_alloc(1, 70)
motor_left.set_forwards()
motor_right.set_forwards()
motor_left.duty(90)
motor_right.duty(90)
sleep(0.1)
class Square:
def __init__(self):
print "Set mode BCM"
config = json.load(open("config.json"))
GPIO.setmode(GPIO.BCM)
self.motorX = Motor(config["motor_x"]["pins"])
self.motorX.name = "X"
self.motorX.delay = config["motor_x"]["delay"]
self.motorX.steps_by_mm = config["motor_x"]["steps_by_mm"]
self.motorY = Motor(config["motor_y"]["pins"])
self.motorY.name = "Y"
self.motorY.delay = config["motor_y"]["delay"]
self.motorY.steps_by_mm = config["motor_y"]["steps_by_mm"]
self.laser = Laser(config["laser"]["pin"])
def start(self):
size = 10
self.laser.on()
self.move(self.motorY, Motor.RIGHT, size)
self.move(self.motorX, Motor.RIGHT, size)
self.move(self.motorY, Motor.LEFT, size)
self.move(self.motorX, Motor.LEFT, size)
self.laser.off()
self.motorX.off()
self.motorY.off()
def move(self, motor, direction, size):
print "motor %s %d %d" % (motor.name, direction,
size * motor.steps_by_mm)
for x in range(0, int(size * motor.steps_by_mm)):
motor.moveTo(direction)
def cleanUp(self):
GPIO.cleanup()