def set_motor_vel(vl, vr):
try:
motors.setSpeeds(vl, vr)
except:
motors.setSpeeds(0, 0)
motors.disable()
def end_journey(self):
print "\nStop..."
motors.setSpeeds(0, 0)
motors.disable()
print "\nKilling Thread..."
self.gpsp.stop()
self.gpsp.join()
def move(args):
command = args['command']
drivingSpeed = -180
if direction == 'F':
motors.enable()
motors.setSpeeds(-drivingSpeed, drivingSpeed)
time.sleep(0.3)
motors.setSpeeds(0, 0)
motors.disable()
print("Forward")
if direction == 'B':
motors.enable()
motors.setSpeeds(drivingSpeed, -drivingSpeed)
time.sleep(0.3)
motors.setSpeeds(0, 0)
motors.disable()
print("Backward")
if direction == 'L':
motors.enable()
motors.setSpeeds(drivingSpeed, drivingSpeed)
time.sleep(0.3)
motors.setSpeeds(0, 0)
motors.disable()
print("Left")
if direction == 'R':
motors.enable()
motors.setSpeeds(-drivingSpeed, -drivingSpeed)
time.sleep(0.3)
motors.setSpeeds(0, 0)
motors.disable()
print("Right")
def move(args):
command = args['command']
drivingSpeed = -180
if direction == 'F':
motors.enable()
motors.setSpeeds(-drivingSpeed, drivingSpeed)
time.sleep(0.3)
motors.setSpeeds(0, 0)
motors.disable()
if direction == 'B':
motors.enable()
motors.setSpeeds(drivingSpeed, -drivingSpeed)
time.sleep(0.3)
motors.setSpeeds(0, 0)
motors.disable()
if direction == 'L':
motors.enable()
motors.setSpeeds(drivingSpeed, drivingSpeed)
time.sleep(0.3)
motors.setSpeeds(0, 0)
motors.disable()
if direction == 'R':
motors.enable()
motors.setSpeeds(-drivingSpeed, -drivingSpeed)
time.sleep(0.3)
motors.setSpeeds(0, 0)
motors.disable()
def move(args):
command = args['button']['command']
drivingSpeed = -180
if direction == 'f':
motors.enable()
motors.setSpeeds(-drivingSpeed, drivingSpeed)
time.sleep(0.3)
motors.setSpeeds(0, 0)
motors.disable()
if direction == 'b':
motors.enable()
motors.setSpeeds(drivingSpeed, -drivingSpeed)
time.sleep(0.3)
motors.setSpeeds(0, 0)
motors.disable()
if direction == 'l':
motors.enable()
motors.setSpeeds(drivingSpeed, drivingSpeed)
time.sleep(0.3)
motors.setSpeeds(0, 0)
motors.disable()
if direction == 'r':
motors.enable()
motors.setSpeeds(-drivingSpeed, -drivingSpeed)
time.sleep(0.3)
motors.setSpeeds(0, 0)
motors.disable()
Rspeed = Rspeed + ACC_VALUE
elif Rspeed > Rtarget:
Rspeed = Rspeed - ACC_VALUE
# set left speed
if Ltarget == 0:
if Lspeed < Ltarget:
Lspeed = Lspeed + STOP_VALUE
elif Lspeed > Ltarget:
Lspeed = Lspeed - STOP_VALUE
elif Lspeed < Ltarget:
Lspeed = Lspeed + ACC_VALUE
elif Lspeed > Ltarget:
Lspeed = Lspeed - ACC_VALUE
# set speed
motors.setSpeeds(-Lspeed, Rspeed)
# print ("Measured Distance = %.1f cm, Current Speed = %.2f, %.2f" % (dist, Lspeed, Rspeed))
# print ("Left = %.1f cm, Right = %.1f cm" % (left, right))
# #time.sleep(.05)
# Reset by pressing CTRL + C
except KeyboardInterrupt:
print("Program stopped by User")
motors.setSpeeds(0, 0)
motors.disable()
GPIO.cleanup()
motors.setSpeeds(0, 0)
motors.disable()
from dual_mc33926_rpi import motors, MAX_SPEED
import time
mixer.init()
sound = mixer.music.Sound('./bb8 - he he he ho.wav')
def make_noise():
sound.play()
while mixer.get_busy():
sleep(1)
def forward(seconds):
motors.setSpeeds(seconds, seconds)
def reverse(seconds):
motors.setSpeeds(-seconds, -seconds)
try:
motors.enable()
make_noise()
forward(10)
make_noise()
reverse(2)
make_noise()
finally:
motors.setSpeeds(0,0)
motors.disable()
def run(self):
self.running = True
motors.enable()
motors.setSpeeds(0, 0)
try:
P = 1.2
I = 1.3
D = 0
L = 200
pid = PID.PID(P, I, D)
pid.SetPoint=0
pid.setSampleTime(0.01)
motor_val = 0
END = L
max_out = 150.0
thresh_up = 0.3*MAX_SPEED
thresh_lo = 0.15*MAX_SPEED
while not self.stopped():
try:
# receive data from client (data, addr)
d = self.s.recvfrom(1024)
except socket.error , msg:
continue
data = d[0]
addr = d[1]
if not data:
continue
data = data.strip().split()
print data
if int(data[1]) < 25:
print 'Obstacle in way!'
#motors.setSpeeds(0, 0)
motors.motor1.setSpeed(-int(1.2*thresh_up))
motors.motor2.setSpeed(-int(thresh_up))
continue
if len(data) > 2:
left = int(data[3])
right = int(data[5])
if abs(left - right) < 5:
motors.motor1.setSpeed(int(1.2*thresh_up))
motors.motor2.setSpeed(int(thresh_up))
print 'Move Straight'
continue
feedback = left - right
pid.update(feedback)
output = pid.output
print 'output: ', output
if output >= 0.0:
speed = thresh_up - output/4.0
else:
speed = thresh_up + output/4.0
if speed < thresh_lo:
drive = int(thresh_lo)
else:
drive = int(speed)
if output <= 0.0:
motors.motor1.setSpeed(int(1.2*thresh_up))
motors.motor2.setSpeed(drive)
print 'Move Right'
print 'Left: ', int(1.2*thresh_up)
print 'Right: ', drive
else:
motors.motor1.setSpeed(int(drive))
motors.motor2.setSpeed(int(thresh_up))
print 'Move Left'
print 'Left: ', drive
print 'Right: ', int(thresh_up)
else:
motors.motor1.setSpeed(int(1.2*thresh_up))
motors.motor2.setSpeed(int(thresh_up))
print 'Move Straight'
continue
except (KeyboardInterrupt, SystemExit): #when you press ctrl+c
print "\nStop..."
motors.setSpeeds(0, 0)
motors.disable()
self.s.close()
finally:
print "\nStopping..."
motors.setSpeeds(0, 0)
motors.disable()
def kill_motors():
motors.disable()
motors.setSpeeds(0,0)
def finish(self): motors.setSpeeds(0, 0) motors.disable()
def move_disable():
motors.disable()
