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 motor_comm(self): # Sends signals to motors
rate = rospy.Rate(20)
while not rospy.is_shutdown():
mtr1_speed = self.mtr1_dir * self.mtr1_PWM
mtr2_speed = self.mtr2_dir * self.mtr2_PWM # Motor speeds and directions
motors.setSpeeds(mtr1_speed, mtr2_speed)
rate.sleep()
def __init__(self):
self.mtr1_dir = 1
self.mtr1_pwm = 100
motors.enable()
motors.setSpeeds(0,0)
# ROS communication
rospy.init_node('Motor_Control', anonymous=True)
self.motor_sub = rospy.Subscriber('/motor_control', DualMotorController, callback=self.motor_callback)
self.motor_comm()
def run(self):
global socket
if not self.stopped():
print 'Turning to bearing angle'
self.turn()
motors.setSpeeds(0, 0)
if not self.stopped():
print 'Driving to destination'
self.estimator = estimator.Estimator(0.5)
self.drive()
motors.setSpeeds(0, 0)
if not self.stopped():
print 'Reached destination'
socket.send("Finished")
def main():
rospy.init_node('cmd_vel_subscriber')
global first_time
global left_vel
global right_vel
global last_time
global vl
global vr
global pid_left
global pid_right
global flame_stop
kp = 1.0
ki = 1700.0
kd = 0.5
sample_time = 0.009
pid_left = pid.pid(kp, ki, kd)
pid_right = pid.pid(kp, ki, kd)
flame_stop = False
# pid_left.setSampleTime(sample_time)
# pid_right.setSampleTime(sample_time)
vl = 0.0
vr = 0.0
left_vel = 0.0
right_vel = 0.0
first_time = True
if first_time:
rospy.loginfo("MOTORS ARE UP...")
motors.enable()
motors.setSpeeds(0, 0)
rospy.Subscriber("/ard_odom", Twist, encoder_callback)
rospy.Subscriber("/cmd_vel", Twist, cmd_callback)
rospy.Subscriber("/flame_seen", Bool, flame_callback)
first_time = False
rospy.spin()
def start_sensors(self, s, e, sock):
global SETTINGS_FILE
global imu
global start
global end
global socket
start = gps_obj.GPS(s.latitude, s.longitude)
end = gps_obj.GPS(e.latitude, e.longitude)
socket = sock
# create the threads
self.gpsp = gps_poller.GpsPoller()
self.gpsp.start()
print("Using settings file " + SETTINGS_FILE + ".ini")
if not os.path.exists(SETTINGS_FILE + ".ini"):
print("Settings file does not exist, will be created")
print("IMU Name: " + imu.IMUName())
if (not imu.IMUInit()):
print("IMU Init Failed")
sys.exit(1)
else:
print("IMU Init Succeeded")
# this is a good time to set any fusion parameters
imu.setSlerpPower(0.02)
imu.setGyroEnable(True)
imu.setAccelEnable(True)
imu.setCompassEnable(True)
self.estimator = estimator.Estimator(0.5)
self.poll_interval = imu.IMUGetPollInterval()
print("Recommended Poll Interval: %dmS\n" % self.poll_interval)
self.check_gps()
motors.enable()
motors.setSpeeds(0, 0)
trig3 = 19
echo3 = 26
def thread_function(name):
while True:
print(ultrasonic_distance.distance(trig1, echo1))
#print(ultrasonic_distance.distance(trig2, echo2))
#print(ultrasonic_distance.distance(trig3, echo3))
print("")
time.sleep(1)
if __name__ == "__main__":
try:
ultrasonic_distance.init(trig1, echo1)
#ultrasonic_distance.init(trig2, echo2)
#ultrasonic_distance.init(trig3, echo3)
print("Starting")
motors.enable()
x = threading.Thread(target=thread_function, args=(1, ))
x.daemon = True
x.start()
while True:
motors.setSpeeds(200, 200)
except KeyboardInterrupt:
print("Program stopped by User")
motors.setSpeeds(0, 0)
motors.disable()
GPIO.cleanup()
def get_rolling():
motors.setSpeeds(-250,-250)
# time.sleep(duration)
return
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()
from __future__ import print_function
import time
from dual_mc33926_rpi import motors, MAX_SPEED
# Set up sequences of motor speeds.
test_forward_speeds = list(range(0, MAX_SPEED, 1)) + \
[MAX_SPEED] * 200 + list(range(MAX_SPEED, 0, -1)) + [0]
test_reverse_speeds = list(range(0, -MAX_SPEED, -1)) + \
[-MAX_SPEED] * 200 + list(range(-MAX_SPEED, 0, 1)) + [0]
try:
motors.enable()
motors.setSpeeds(100, 100)
time.sleep(5)
finally:
# Stop the motors, even if there is an exception
# or the user presses Ctrl+C to kill the process.
motors.setSpeeds(0, 0)
motors.disable()
def set_motors(state):
if state == states.straight_turn_s:
motors.setSpeeds(-200,-180)
elif state == states.straight_turn_l:
motors.setSpeeds(0,-480)
elif state == states.straight_turn_r:
motors.setSpeeds(-480,0)
elif state == states.right_turn_r:
motors.setSpeeds(-280,0)
elif state == states.right_turn_s:
motors.setSpeeds(-200,-200)
elif state == states.left_turn_l:
motors.setSpeeds(0,-380)
elif state == states.left_turn_s:
motors.setSpeeds(-200,-200)
elif state == state.turn:
motors.setSpeeds(0,0)
def turn_right_45():
motors.setSpeeds(speed_f, speed_b)
time.sleep(1.18)
def turn_left_45():
motors.setSpeeds(speed_b, speed_f)
time.sleep(1.18)
def drive(self):
global start
global end
global imu
self.check_gps()
self.check_imu()
self.last_waypoint = start
current_timestamp = time.time() # gpsp.get_timestamp()
self.last_waypoint.set_timestamp(current_timestamp)
data = imu.getIMUData()
fusionPose = data["fusionPose"]
yaw = math.degrees(fusionPose[2])
heading = nav.yaw_to_heading(yaw, -90.0)
print 'Heading: %f' % heading
bearing = nav.get_bearing(start, end)
print 'Bearing: ', bearing
P = 1.2
I = 1
D = 0
L = 200
pid = PID.PID(P, I, D)
pid.SetPoint=bearing
pid.setSampleTime(0.01)
motor_val = 0
thresh_up = 0.35*MAX_SPEED
thresh_lo = 0.2*MAX_SPEED
while not self.stopped():
print nav.get_distance(self.last_waypoint, end)
if nav.get_distance(self.last_waypoint, end) < 5.0:
print "Hit waypoint"
break
if abs(360.0 + heading - bearing) < abs(heading - bearing):
feedback = heading + 360.0
else:
feedback = heading
pid.update(feedback)
output = pid.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 abs(heading - bearing) < 2.0:
motors.motor1.setSpeed(int(1.2*thresh_up))
motors.motor2.setSpeed(int(thresh_up))
print 'Both'
elif output > 0.0:
motors.motor1.setSpeed(int(1.2*thresh_up))
motors.motor2.setSpeed(drive)
print 'Left'
elif output < 0.0:
motors.motor1.setSpeed(int(drive))
motors.motor2.setSpeed(int(thresh_up))
print 'Right'
self.check_gps()
self.check_imu()
self.estimate_position()
data = imu.getIMUData()
fusionPose = data["fusionPose"]
yaw = math.degrees(fusionPose[2])
heading = nav.yaw_to_heading(yaw, -90.0)
print 'Heading: %f' % heading
bearing = nav.get_bearing(self.last_waypoint, end)
print 'Bearing: ', bearing
sleep(0.1)
motors.setSpeeds(0, 0)
def turn_left_90():
motors.setSpeeds(speed_b, speed_f)
time.sleep(2.40)
def __init__(self): motors.enable() motors.setSpeeds(0, 0) self.angle = 0 self.speed = 0 self.calc_motor_values = calc_motor_values.Motor(MAX_SPEED)
def move_backward():
motors.setSpeeds(speed_b, speed_b)
def finish(self): motors.setSpeeds(0, 0) motors.disable()
def turn_right_135():
motors.setSpeeds(speed_f, speed_b)
time.sleep(3.51)
def turn_right_90():
motors.setSpeeds(speed_f, speed_b)
time.sleep(2.40)
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()
from pygame import mixer # Load the required library
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 turn(self):
global start
global end
global imu
self.check_imu()
data = imu.getIMUData()
fusionPose = data["fusionPose"]
yaw = math.degrees(fusionPose[2])
heading = nav.yaw_to_heading(yaw, -90.0)
print 'Heading: %f' % heading
bearing = nav.get_bearing(start, end)
print 'Bearing: ', bearing
P = 1.2
I = 1.3
D = 0
L = 200
pid = PID.PID(P, I, D)
pid.SetPoint = bearing
pid.setSampleTime(0.01)
motor_val = 0
thresh_up = 0.7*MAX_SPEED
thresh_lo = 0.12*MAX_SPEED
i = 0
while not self.stopped():
i += 1
if abs(360.0 + heading - bearing) < abs(heading - bearing):
feedback = heading + 360.0
else:
feedback = heading
pid.update(feedback)
output = pid.output
motor_val += (output - (1/i))
print 'Output: %f' % output
if output >= 0.0:
if output > thresh_up:
drive = int(thresh_up)
elif output < thresh_lo:
drive = int(thresh_lo)
else:
drive = int(output)
else:
if output < -thresh_up:
drive = int(thresh_up)
elif output > -thresh_lo:
drive = int(thresh_lo)
else:
drive = int(-output)
if output > 0.0:
motors.motor1.setSpeed(drive)
motors.motor2.setSpeed(-drive)
elif output < 0.0:
motors.motor1.setSpeed(-drive)
motors.motor2.setSpeed(drive)
self.check_imu()
data = imu.getIMUData()
fusionPose = data["fusionPose"]
yaw = math.degrees(fusionPose[2])
heading = nav.yaw_to_heading(yaw, -90.0)
print 'Heading: %f' % heading
print 'Bearing: ', bearing
if abs(heading - bearing) < 1.0:
print "Hit bearing angle"
break
sleep(0.1)
motors.setSpeeds(0, 0)
def turn_left_135():
motors.setSpeeds(speed_b, speed_f)
time.sleep(3.51)
def reverse(seconds): motors.setSpeeds(-seconds, -seconds)
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_forward():
motors.setSpeeds(speed_f, speed_f)
import numpy as np
from dual_mc33926_rpi import motors, MAX_SPEED
# set GPIO pin
#=========Set Up GPIO Pins==================#
count = 0
print('Use left toggle on ps4 controller to maneuver the robot')
moves = []
#=========initialize variable x==================#
x = 0.1
My_test = True
motors.enable()
motors.setSpeeds(0, 0)
#=========define functions for driving robot==================#
def right(x):
endtime = time.time() + x
while time.time() < endtime:
motors.motor1.setSpeed(440)
motors.motor2.setSpeed(420)
time.sleep(0.25)
def left(x):
endtime = time.time() + x
while time.time() < endtime:
motors.motor1.setSpeed(-440)
def forward(seconds): motors.setSpeeds(seconds, seconds)
def move_brake():
motors.setSpeeds(0, 0)
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 start():
motors.setSpeeds(0,0)
motors.enable()
left = 'y'
return states.turn
def init(default_speed=240):
motors.enable()
motors.setSpeeds(0, 0)
set_speed(default_speed)
def do_the_turn():
motors.setSpeeds(480,-480)
time.sleep(.2)
motors.setSpeeds(-200,-200)
def ultrasonic3():
while True:
return 100
# global right
# right = running_average.update(ultrasonic_distance.distance(trig3, echo3))
# time.sleep(SENSOR_DELAY)
if __name__ == '__main__':
try:
print("Starting")
#motor setup
motors.enable()
motors.setSpeeds(0, 0)
Lspeed = 0
Rspeed = 0
lst = [MIN_DIST] * WINDOW_LENGTH
#ultrasonic setup
running_average.average_init(lst)
ultrasonic_distance.init(trig1, echo1)
# ultrasonic_distance.init(trig2, echo2)
# ultrasonic_distance.init(trig3, echo3)
x = threading.Thread(target=ultrasonic1, args=())
x.daemon = True
x.start()
# y = threading.Thread(target=ultrasonic2, args=())
# y.daemon = True
# y.start()
# z = threading.Thread(target=ultrasonic3, args=())
def kill_motors():
motors.disable()
motors.setSpeeds(0,0)
