def __init__(self):
self._motor = Motor()
self._socket = socket.socket(socket.AF_INET, socket.SOCK_STREAM)
self._socket.bind(('', 50007))
self._socket.listen(1)
pass
def __init__(self):
self.bus = smbus.SMBus(1)
self.pwm = PwmBoard(self.bus, 0x40)
self.motor1 = Motor(self.pwm, 0)
self.motor2 = Motor(self.pwm, 1)
self.motor3 = Motor(self.pwm, 2)
self.motor4 = Motor(self.pwm, 3)
self.mpu6050 = MPU6050(self.bus, 0x68)
self.mpu6050.start()
self.networkhandler = NetworkHandler(self)
self.running = True
self.pgain = 5.0
self.igain = 0.01
self.dgain = 1.5
self.roll_pid = PID(self.pgain, self.igain, self.dgain)
self.pitch_pid = PID(self.pgain, self.igain, self.dgain)
self.yaw_pid = PID(self.pgain, self.igain, self.dgain)
self.wantedroll = 0.0
self.wantedpitch = 0.0
self.throttle = 0.0
time.sleep(1)
def __init__(self, ser, model):
'''
Constructor
self (serial.Serial): the serial object bound to the device
model (string): the goniometer stage model ('GNL10' or 'GNL18')
'''
Motor.__init__(self, ser)
self.ext['ClassName'] = 'GonStage' # for logging
self.home()
# set internal constants based on model
if model == 'GNL18':
self.STEPS_PER_DEG = GNL18_STEPS_PER_DEG
self.STOP_LIMIT = GNL18_STOP_LIMIT
elif model == 'GNL10':
self.STEPS_PER_DEG = GNL10_STEPS_PER_DEG
self.STOP_LIMIT = GNL10_STOP_LIMIT
else:
self.logger.error('invalid model <{}>'.format(model),
extra=self.ext)
exit()
# move to middle
self.deg_pos = 0
self.deg_zero = 0
self.delta_rot(float(self.STOP_LIMIT))
self.set_as_zero(float(self.STOP_LIMIT))
def get_route(self, destination):
destination = self.get_uid_from_map(destination)
current_pos = self.get_current();
dest_id = self.get_index_from_uid(destination)
curr_id = self.get_index_from_uid(current_pos)
print "dest id = ", dest_id
print "curr id = ", curr_id
from Route import Route
route = Route(curr_id, dest_id, [], [], [])
path = route.findRoute()
from Imu import IMU
imu = IMU()
print "imu reading = ", imu.get_heading()
from obstacle import Obstacle
o = Obstacle()
print "setting up motors..."
from Motor import Motor
motor = Motor(o)
motor.setup()
print "setting up navigation..."
from navigate import Navigate
navigator = Navigate(motor,imu)
navigator.set_direc(imu)
for x in range(0,len(path)-1):
#print (path[x],path[x+1])
#navigator.set_direc(imu)
navigator.navigate(path[x], path[x+1])
def test_motors():
ms = []
ms.append(Motor(7, "test"))
ms.append(Motor(8, "test"))
ms.append(Motor(9, "test"))
ms.append(Motor(10, "test"))
for m in ms:
m.show_alive()
class RaspCar():
def __init__(self):
self._motor = Motor()
self._socket = socket.socket(socket.AF_INET, socket.SOCK_STREAM)
self._socket.bind(('', 50007))
self._socket.listen(1)
pass
def __del__(self):
self._socket.close()
def start(self):
print("start...")
while True:
conn, addr = self._socket.accept()
print("Connected by", addr)
while True:
try:
# receive data
data = conn.recv(1024)
data = data.decode("utf-8")
# force quit...
if data == "close":
conn.close()
self._socket.close()
return True
self._parseData(data)
#send feedback
conn.send(bytes("good", "utf8"))
except:
print("close socket")
break
conn.close()
def _parseData(self, data):
func = data.split(" ")
if len(func) < 2:
return
key = func[0].split(":")
act = func[1].split(":")
if key[0] == "arrow-key" and act[0] == "action" :
if act[1] == "press":
if key[1] == "up":
self._motor.forward()
elif key[1] == "mid-left":
self._motor.turnLeft()
elif key[1] == "mid-right":
self._motor.turnRight()
elif key[1] == "down":
self._motor.backward()
elif act[1] == "release":
self._motor.stop()
class RaspCar():
def __init__(self):
self._motor = Motor()
self._socket = socket.socket(socket.AF_INET, socket.SOCK_STREAM)
self._socket.bind(('', 50007))
self._socket.listen(1)
pass
def __del__(self):
self._socket.close()
def start(self):
print("start...")
while True:
conn, addr = self._socket.accept()
print("Connected by", addr)
while True:
try:
# receive data
data = conn.recv(1024)
data = data.decode("utf-8")
# force quit...
if data == "close":
conn.close()
self._socket.close()
return True
self._parseData(data)
#send feedback
conn.send(bytes("good", "utf8"))
except:
print("close socket")
break
conn.close()
def _parseData(self, data):
func = data.split(" ")
if len(func) < 2:
return
key = func[0].split(":")
act = func[1].split(":")
if key[0] == "arrow-key" and act[0] == "action":
if act[1] == "press":
if key[1] == "up":
self._motor.forward()
elif key[1] == "mid-left":
self._motor.turnLeft()
elif key[1] == "mid-right":
self._motor.turnRight()
elif key[1] == "down":
self._motor.backward()
elif act[1] == "release":
self._motor.stop()
def set_as_zero(self, zer_deg):
'''
change the origin (zero)
'''
n_zero = int(round(zer_deg * self.STEPS_PER_DEG))
Motor.set_as_zero(self, n_zero)
self.deg_zero = zer_deg
self.deg_pos -= zer_deg
def __init__(self, port_handler, packet_handler):
self.port_handler = port_handler
self.packet_handler = packet_handler
self.motor_list = [
Motor(1, port_handler, packet_handler, 355, 760, 240, 1, 32, 32),
Motor(2, port_handler, packet_handler, 206, 660, 240, 1, 32, 32,
0),
Motor(3, port_handler, packet_handler, 50, 800, 64, 2, 8, 8),
Motor(4, port_handler, packet_handler, 200, 793, 128, 1, 32, 32)
]
self.boot_up()
def testPollute(self):
file_name = "mario"
file = open(self.TMP_PATH + "\\" + file_name, "w")
file.writelines("luigi")
file.seek(3000)
file.write("\0")
file.close()
file = open(self.TMP_PATH + "\mario", "w")
file.writelines("luigi")
file.close()
os.makedirs(self.TMP_PATH + "\directory", 20)
Motor.pollute(self.TMP_PATH, 2)
self.assertTrue(True)
def __init__(self):
self.max_speed = 50
self.backLeftMotor = Motor("P8_11", "P8_12", "P8_13", "P8_14")
self.frontLeftMotor = Motor("P8_17", "P8_15", "P8_19", "P8_14")
self.backRightMotor = Motor("P9_11", "P9_12", "P9_14", "P9_17")
self.frontRightMotor = Motor("P9_13", "P9_15", "P9_16", "P9_17")
return
def __init__(self, **kwargs):
Controller.__init__(self, **kwargs)
self.L3X = 0
self.L3Y = 0
self.R3X = 0
self.R3Y = 0
self.leftMotorPWM = 0
self.rightMotorPWM = 0
self.arrow = "None"
self.motor = Motor()
self.servo = Servo()
self.motor.InitMotorHw()
self.servo.InitServoHw()
self.BASESTEPSIZE = 5
def set2DCMotor():
global motor1
global motor2
global MOTOR1_PIN1
global MOTOR1_PIN2
global MOTOR2_PIN1
global MOTOR2_PIN2
try:
#webiopi.debug('set2DCMotor')
motor1 = Motor(MOTOR1_PIN1, MOTOR1_PIN2)
motor2 = Motor(MOTOR2_PIN1, MOTOR2_PIN2)
except Exception as Err:
webiopi.debug('%s' % Err)
def __init__(self):
self._motor = Motor()
self._socket = socket.socket(socket.AF_INET, socket.SOCK_STREAM)
self._socket.bind(('', 50007))
self._socket.listen(1)
pass
def __init__(self):
self.lenkung = Lenkung()
self.motor = Motor()
self.licht = Licht()
self.bremse = Bremse()
self.gearOld = False
self.speedOld = 0
pass
class Controler:
def __init__(self):
self.lenkung = Lenkung()
self.motor = Motor()
self.licht = Licht()
self.bremse = Bremse()
self.gearOld = False
self.speedOld = 0
pass
def update(self, speed, angle, gear, licht):
#print ("speed: ", speed)
#print ("angle: ", angle)
#print("gear: ", gear)
""" TODO: Update an die Hardware des RaspCar weitergeben!! """
if gear != self.gearOld:
print ("gear ungleich gearOld")
if gear:
self.motor.move(2)
else:
self.motor.move(1)
self.gearOld = gear
if licht == 1:
self.licht.lichtschalter(1)
else:
self.licht.lichtschalter(0)
if speed != self.speedOld:
self.motor.speed(speed)
if self.speedOld > speed:
self.bremse.bremslicht(1)
else:
self.bremse.bremslicht(0)
self.speedOld = speed
else:
self.bremse.bremslicht(0)
self.lenkung.lenken(angle)
pass
class Quadcopter():
def __init__(self):
self.bus = smbus.SMBus(1)
self.pwm = PwmBoard(self.bus, 0x40)
self.motor1 = Motor(self.pwm, 0)
self.motor2 = Motor(self.pwm, 1)
self.motor3 = Motor(self.pwm, 2)
self.motor4 = Motor(self.pwm, 3)
self.mpu6050 = MPU6050(self.bus, 0x68)
self.mpu6050.start()
self.networkhandler = NetworkHandler(self)
self.running = True
self.pgain = 5.0
self.igain = 0.01
self.dgain = 1.5
self.roll_pid = PID(self.pgain, self.igain, self.dgain)
self.pitch_pid = PID(self.pgain, self.igain, self.dgain)
self.yaw_pid = PID(self.pgain, self.igain, self.dgain)
self.wantedroll = 0.0
self.wantedpitch = 0.0
self.throttle = 0.0
time.sleep(1)
def start(self):
lasttime = time.time()
time_diff = 0.01
while self.running:
self.mpu6050.read()
time.sleep(time_diff - 0.005)
x, y, z = self.mpu6050.getlastvalues()
delta_time = time.time() - lasttime
lasttime = time.time()
roll_pid_result = self.roll_pid.Compute(x, self.wantedroll, delta_time)
pitch_pid_result = self.pitch_pid.Compute(y, self.wantedpitch, delta_time)
#yaw_pid_result = self.yaw_pid.Compute(z, 0, delta_time)
yaw_pid_result = 0
if self.throttle != 0:
self.motor1.update((self.throttle + roll_pid_result + pitch_pid_result + yaw_pid_result))
self.motor2.update((self.throttle + roll_pid_result - pitch_pid_result - yaw_pid_result))
self.motor3.update((self.throttle - roll_pid_result - pitch_pid_result + yaw_pid_result))
self.motor4.update((self.throttle - roll_pid_result + pitch_pid_result - yaw_pid_result))
def changepidgain(self, pgain, igain, dgain):
self.roll_pid.changegain(pgain, igain, dgain)
self.pitch_pid.changegain(pgain, igain, dgain)
def setroll(self, roll):
if (roll > -50) & (roll < 50):
self.wantedroll = roll
def setpitch(self, pitch):
if (pitch > -50) & (pitch < 50):
self.wantedpitch = pitch
def setthrottle(self, throttle):
if throttle == 0:
self.throttle = 0
time.sleep(0.1)
self.motor1.stop()
self.motor2.stop()
self.motor3.stop()
self.motor4.stop()
else:
throttle = throttle * (((self.motor1.max - 180) - (self.motor1.min + 180)) / 100) + (self.motor1.min + 180)
if (throttle > (self.motor1.min + 180)) & (throttle < (self.motor1.max - 180)):
self.throttle = throttle
def getdata(self):
gx, gy, ax, ay, x, y, z = self.mpu6050.getextendedvalues()
return gx, gy, ax, ay, x, y, z, self.motor1.getpercent(), self.motor2.getpercent(), self.motor3.getpercent(), self.motor4.getpercent()
def stop(self):
self.running = False
self.mpu6050.stop()
time.sleep(1)
self.motor1.stop()
self.motor2.stop()
self.motor3.stop()
self.motor4.stop()
self.networkhandler.stop()
from Adafruit_MCP230xx import Adafruit_MCP230XX
import RPi.GPIO as GPIO
from Motor import Motor
from Encoder import Encoder
from Sonar import Sonar
from Supervisor import Supervisor
from test import *
if __name__ == '__main__':
# MCP23017
mcp = Adafruit_MCP230XX(busnum=1, address=0x20, num_gpios=16)
# Objects declaration
m = Motor(mcp)
e = Encoder()
s = Sonar(mcp)
sup = Supervisor(m, e, s)
# Initialisation
m.init_motor()
e.initEncoder()
s.initSonar()
# Ultrasonic sensors activation
s.activate_us()
# Vacuum cleaner starting
m.vacuum_cleaner_start()
m.change_speed_vacuum(50)
#GPIO.setmode(GPIO.BCM) print 'Initializing sensors and misc boards...' temperature = Temperature() gyro = Gyro(1) #gps = GPS() #lcd = LCD() #camera = Camera() print 'Initializing motors...' motor_pins = [11,13,15,16] front_left = Motor(motor_pins[0], GPIO) front_right = Motor(motor_pins[1], GPIO) rear_left = Motor(motor_pins[2], GPIO) rear_right = Motor(motor_pins[3], GPIO) print 'Starting stand-by mode...' front_left.start() front_right.start() rear_left.start() rear_right.start() # Wait 2 seconds, to let motors finish initialization time.sleep(2);
if cap.isOpened() is False:
return False
raise("Camera is not available.")
cap.set(3, WIDTH)
cap.set(4, HEIGHT)
time.sleep(2) # camera might take 1 or 2 second to change parameters
return True
if __name__ == "__main__":
cap = cv2.VideoCapture(0)
if camera_check() is False:
exit
# mode
mode = "detect" # detect, fish, wait
motor = Motor()
status = "attract" # "attract", "detect", "fish", "wait"
## start following
last_sec = time.ctime()
fps = 0
last_point_list = []
display = False
step = 0
while(cap.isOpened()):
step += 1
if mode == "detect":
# read 1 frame
ret, frame = cap.read()
#! /usr/bin/python3
from KinectWrapper import Device
from Motor import Motor
motor = Motor()
dev = Device(0)
dev.open()
step = 2
for angle in range(0, 360, step):
filename = "../data/box{:03d}.pcd".format(angle)
print(filename)
dev.start(50)
dev.save(filename)
motor.turn(-step)
dev.stop()
# print("../data/box{:02d}.pcd".format(angle))
# for i in dev.get_width():
# for j in dev.get_heigth():
# pc.add(())
