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 __init__(self):
motors.enable() # Enables Motors from library
# 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 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()
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)
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()
def init(default_speed=240):
motors.enable()
motors.setSpeeds(0, 0)
set_speed(default_speed)
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 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 reset_motors():
motors.enable()
def start():
motors.setSpeeds(0,0)
motors.enable()
left = 'y'
return states.turn
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_enable():
motors.enable()
