Esempio n. 1
0
def set_motor_vel(vl, vr):
    try:
        motors.setSpeeds(vl, vr)

    except:
        motors.setSpeeds(0, 0)
        motors.disable()
Esempio n. 2
0
 def end_journey(self):
 	print "\nStop..."
     motors.setSpeeds(0, 0)
     motors.disable()
     print "\nKilling Thread..."
 	self.gpsp.stop()
 	self.gpsp.join()
Esempio n. 3
0
    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()
Esempio n. 4
0
	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()
Esempio n. 5
0
    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")
Esempio n. 6
0
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()
Esempio n. 7
0
    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)
Esempio n. 8
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
Esempio n. 10
0
    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()
Esempio n. 11
0
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()
Esempio n. 12
0
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)
Esempio n. 13
0
def turn_right_45():
    motors.setSpeeds(speed_f, speed_b)
    time.sleep(1.18)
Esempio n. 14
0
def turn_left_45():
    motors.setSpeeds(speed_b, speed_f)
    time.sleep(1.18)
Esempio n. 15
0
    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)
Esempio n. 16
0
def turn_left_90():
    motors.setSpeeds(speed_b, speed_f)
    time.sleep(2.40)
Esempio n. 17
0
 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)
Esempio n. 18
0
def move_backward():
    motors.setSpeeds(speed_b, speed_b)
Esempio n. 19
0
 def finish(self):
   motors.setSpeeds(0, 0)
   motors.disable()
Esempio n. 20
0
def turn_right_135():
    motors.setSpeeds(speed_f, speed_b)
    time.sleep(3.51)
Esempio n. 21
0
def turn_right_90():
    motors.setSpeeds(speed_f, speed_b)
    time.sleep(2.40)
Esempio n. 22
0
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()
Esempio n. 23
0
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()
Esempio n. 24
0
    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)      
Esempio n. 25
0
def turn_left_135():
    motors.setSpeeds(speed_b, speed_f)
    time.sleep(3.51)
Esempio n. 26
0
def reverse(seconds):
  motors.setSpeeds(-seconds, -seconds)
Esempio n. 27
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()
        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")
Esempio n. 28
0
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)
Esempio n. 30
0
def forward(seconds):
  motors.setSpeeds(seconds, seconds)
Esempio n. 31
0
def move_brake():
    motors.setSpeeds(0, 0)
Esempio n. 32
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()
Esempio n. 33
0
def start():
    motors.setSpeeds(0,0)
    motors.enable()
    left = 'y'
    return states.turn
Esempio n. 34
0
def init(default_speed=240):
    motors.enable()
    motors.setSpeeds(0, 0)
    set_speed(default_speed)
Esempio n. 35
0
def do_the_turn():
    motors.setSpeeds(480,-480)
    time.sleep(.2)
    motors.setSpeeds(-200,-200)
Esempio n. 36
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()
        #	z = threading.Thread(target=ultrasonic3, args=())
Esempio n. 37
0
def kill_motors():
    motors.disable()
    motors.setSpeeds(0,0)