def setMotorSpeeds(): speeds = go.read_motor_speed() # If one motor is moving slower, change the other to match it if speeds[0] < speeds[1]: print 'MOTOR: Left motor moving slower' go.set_speed(speeds[0]) elif speeds[1] < speeds[0]: print 'MOTOR: Right motor moving slower' go.set_speed(speeds[1])
def _on_receive(self, data): logger.debugf('received message: {}', data) t = data.linear.x r = data.angular.z logger.debugf('t={}, r={}', t, r) mag = math.sqrt(pow(t, 2) + pow(r, 2)) if mag < 0.1: logger.debugf( 'too small value, linear.x: {}, angular.z: {}, mag: {}', t, r, mag) motor1(0, 0) motor2(0, 0) logger.debugf('moter speed: {}', read_motor_speed()) return logger.debugf('mag={}', mag) m1 = t / mag * abs(t) m2 = t / mag * abs(t) logger.debugf('m1={}, m2={}', m1, m2) m1 += r / mag * abs(r) m2 += -r / mag * abs(r) logger.debugf('m1={}, m2={}', m1, m2) if m1 != 0: m1 = m1 / abs(m1) * min(abs(m1), 1.0) if m2 != 0: m2 = m2 / abs(m2) * min(abs(m2), 1.0) logger.debugf('m1={}, m2={}', m1, m2) motor1(m1 >= 0, abs(int(m1 * 255))) motor2(m2 >= 0, abs(int(m2 * 255))) logger.debugf('moter speed: {}', read_motor_speed())
def rs(INIT_LEFT, INIT_RIGHT): global encoder_left_init, encoder_right_init encoder_left_init = INIT_LEFT encoder_right_init = INIT_RIGHT current_left_encoder_differential, current_right_encoder_differential = read_encoders( ) print "Current Left Encoder Value: ", current_left_encoder_differential print "Current Right Encoder Value: ", current_right_encoder_differential correctMotors(current_left_encoder_differential, current_right_encoder_differential, go.read_motor_speed()[1])
def __init__(self, system_info, command_queue, user_set_speed, safe_distance): """ Initializes the rover object and sets the values based on the given parameters and the current state of the rover. :param multiprocessing.Queue command_queue: The queue that the rover will pull commands from. :param float user_set_speed: The user set speed. None will cause a reasonable default to be used. :param int safe_distance: The user set safe distance. None will cause a reasonable default to be used. """ self.system_info = system_info # An object that relays ACC, rover, and obstacle information to the user # interface self.command_queue = command_queue # A concurrent queue that will contain commands for the ACC for changing # settings and shutting it down self.user_set_speed = None # The speed that the user desires the rover to move at (power units) if user_set_speed is None: motor_speeds = gopigo.read_motor_speed() self.user_set_speed = (motor_speeds[0] + motor_speeds[1]) / 2.0 else: self.user_set_speed = user_set_speed self.safe_distance = None # The distance that the user desires the rover to maintain from the obstacle (cm) if safe_distance is None: self.safe_distance = 2 * BUFFER_DISTANCE else: self.safe_distance = safe_distance self.initial_ticks_left = 0 # The number of left motor ticks when the ACC was started (ticks) self.initial_ticks_right = 0 # The number of right motor ticks when the ACC was started (ticks) self.elapsed_ticks_left = 0 # The number of left motor ticks elapsed since the ACC was started (ticks) self.elapsed_ticks_right = 0 # The number of right motor ticks elapsed since the ACC was started (ticks) self.speed = INITIAL_SPEED # The speed that the rover is/should be going at (power units) self.power_on = False # A value representing if the ACC is on or not self.obstacle_distance = None # The distance of the obstacle from the rover (cm) self.obstacle_relative_speed = None # The velocity of the obstacle relative to the rover (cm/s) self.critical_distance = 0 # The critical distance of the rover to the obstacle (cm) self.minimum_settable_safe_distance = 0 # The minimum settable safe distance (cm) self.alert_distance = 0 # The alert distance of the rover to the obstacle (cm) self.t = 0 # A value used for calculating the delta time for the main loop (s) self.dists = collections.deque(maxlen=SAMPLE_SIZE) # A log of previous obstacle distance measures (cm) self.dts = collections.deque(maxlen=SAMPLE_SIZE - 1) # A log of previous delta times for the main loop (s)
def __process_commands(self): """ Processes the next command in the ACC's command queue if there are any queued commands. """ if not self.command_queue.empty(): command = self.command_queue.get() if isinstance(command, commands.ChangeSettingsCommand): if command.userSetSpeed is not None: self.user_set_speed = command.userSetSpeed else: motor_speeds = gopigo.read_motor_speed() self.user_set_speed = (motor_speeds[0] + motor_speeds[1]) / 2.0 if command.safeDistance is not None: self.safe_distance = command.safeDistance else: self.safe_distance = gopigo.us_dist(gopigo.USS) if isinstance(command, commands.TurnOffCommand): self.power_on = False
def main(): go.set_speed(MIN_SPEED) INIT_LEFT_ENC, INIT_RIGHT_ENC = read_encoders() print('Left: ', INIT_LEFT_ENC, ' Right: ', INIT_RIGHT_ENC) time.sleep(5) go.forward() while True: try: if STATE == 3: go.stop() else: go.forward() print 'MOTORS: ', go.read_motor_speed() rs.rs(INIT_LEFT_ENC, INIT_RIGHT_ENC) stateCheck() # Shuts the ACC down when a Ctrl + c command is issued except KeyboardInterrupt: print '\nACC shut off' go.stop() sys.exit()
def __init__(self, command_queue, user_set_speed, safe_distance): """ :param command_queue: This parameter is created in the "Power On" sequence, in the initial __main__() method :param user_set_speed: This parameter is provided by the initial __main__() method, with the value determined as outlined in the "Power On" sequence diagram. :param safe_distance: This parameter is provided by the initial __main__() method, with the value determined as outlined in the "Power On" sequence diagram. This function sets class parameters utilized in indicating the User's settings. In the case that a user_set_speed and safe_distance are not provided, they are set to default values. """ if user_set_speed is None: motor_speeds = gopigo.read_motor_speed() self.user_set_speed = (motor_speeds[0] + motor_speeds[1]) / 2.0 if safe_distance is None: self.safe_distance = 2 * BUFFER_DISATANCE self.command_queue = command_queue self.power_on = True self.stop = False self.elapsed_ticks_left = 0 self.elapsed_ticks_right = 0
import gopigo from gopigo import * import sys, os #permet de récupérer le paramètre envoyé en ligne de commande a = sys.argv[1] #Effectue une instruction en fonction du paramètre reçu if a == 'H': fwd() #permet au GoPigo 2 d'avancer led_off(LED_R) #permet l'extinction de la led droite led_off(LED_L) #permet l'extinction de la led gauche elif a == 'G': left() #permet au GoPigo 2 de faire une rotation sur la gauche elif a == 'D': right() #permet au GoPigo 2 de faire une rotation sur la droite elif a == 'B': bwd() #permet au GoPigo 2 de reculer led_on(LED_R) #permet l'allumage de la led droite led_on(LED_L) #permet l'allumage de la led droite elif a == 'X': stop() #permet au Gopigo 2 de ne plus bouger led_off(LED_R) led_off(LED_L) elif a == 'T': increase_speed() #permet d'augmenter la vitesse du robot elif a == 'L': decrease_speed() #permet de diminuer la vitesse du robot spd = gopigo.read_motor_speed() #permet de récupérer la vitesse des moteurs print("Vitesse -> M1:%d ,M2:%d " % (spd[0], spd[1]))
gopigo.motor2(1, 255) time.sleep(2) print("Motor 2 moving forward at half speed") gopigo.motor2(1, 127) time.sleep(2) print("Motor 2 stopping ") gopigo.motor2(1, 0) time.sleep(1) print("Motor 2 moving back at full speed") gopigo.motor2(0, 255) time.sleep(2) print("Motor 2 moving back at half speed") gopigo.motor2(0, 127) time.sleep(2) print("Motor 2 stopping") gopigo.motor2(1, 0) spd = gopigo.read_motor_speed() print ("Current speed M1:%d ,M2:%d " % (spd[0], spd[1])) print("Changing speed") gopigo.set_speed(200) # Setting motor speed to 200, so that the motors still move when the next program uses it spd = gopigo.read_motor_speed() print ("New speed M1:%d ,M2:%d " % (spd[0], spd[1]))
def getCurrentSpeed(): return go.read_motor_speed()[1]
#go.set_right_speed(speed) go.set_left_speed(speed) while True: time.sleep(0.25) left = go.enc_read(0) right = go.enc_read(1) leftDiff = left - prevLeft rightDiff = right - prevRight print 'TICKS LEFT: ', leftDiff, 'TICKS RIGHT: ', rightDiff error = rightDiff - leftDiff print 'ERROR: ', error speeds = go.read_motor_speed() set_slave(speeds[0] + error) #set_slave(speeds[1] + error) new_speeds = go.read_motor_speed() print 'LEFT MOTOR: ', new_speeds[1], 'RIGHT MOTOR: ', new_speeds[ 0] prevRight = right prevLeft = left
def set_slave_motor(error): go.set_right_speed(go.read_motor_speed()[0] + error)
def do_command(command=None): logging.debug(command) if command in ["forward", "fwd"]: gopigo.fwd() elif command == "left": gopigo.left() elif command == "left_rot": gopigo.left_rot() elif command == "right": gopigo.right() elif command == "right_rot": gopigo.right_rot() elif command == "stop": gopigo.stop() elif command == "leftled_on": gopigo.led_on(0) elif command == "leftled_off": gopigo.led_off(0) elif command == "rightled_on": gopigo.led_on(1) elif command == "rightled_off": gopigo.led_off(1) elif command in ["back", "bwd"]: gopigo.bwd() elif command == "speed": logging.debug("speed") speed = flask.request.args.get("speed") logging.debug("speed:" + str(speed)) if speed: logging.debug("in if speed") gopigo.set_speed(int(speed)) left_speed = flask.request.args.get("left_speed") logging.debug("left_speed:" + str(left_speed)) if left_speed: logging.debug("in if left_speed") gopigo.set_left_speed(int(left_speed)) right_speed = flask.request.args.get("right_speed") logging.debug("right_speed:" + str(right_speed)) if right_speed: logging.debug("in if right_speed") gopigo.set_right_speed(int(right_speed)) speed_result = gopigo.read_motor_speed() logging.debug(speed_result) return flask.json.jsonify({"speed": speed_result, "right": speed_result[0], "left": speed_result[1]}) elif command == "get_data": speed_result = gopigo.read_motor_speed() enc_right = gopigo.enc_read(0) enc_left = gopigo.enc_read(1) volt = gopigo.volt() timeout = gopigo.read_timeout_status() return flask.json.jsonify( { "speed": speed_result, "speed_right": speed_result[0], "speed_left": speed_result[1], "enc_right": enc_right, "enc_left": enc_left, "volt": volt, "timeout": timeout, "fw_ver": gopigo.fw_ver(), } ) elif command in ["enc_tgt", "step"]: tgt = flask.request.args.get("tgt") direction = flask.request.args.get("dir") if tgt: gopigo.gopigo.enc_tgt(1, 1, int(tgt)) if dir: if dir == "bwd": gopigo.bwd() else: gopigo.fwd() else: gopigo.fwd() return ""
if (us_Distance <= critical_distance): go.stop() elif ((us_Distance <= slowdown_distance)): if(target_speed < initial_speed): while(temp_speed > target_speed): us_Distance = go.us_dist(15) # print "Distance to object: ", us_Distance, " Critical Distance: ", critical_distance if (us_Distance <= critical_distance): # print "INSIDE: Distance to object: ", us_Distance, " Critical Distance: ", critical_distance break temp_speed = temp_speed - 2 #print "Current TempSpeed: ", temp_speed, "Current TargetSpeed: ", target_speed go.set_speed(temp_speed) print "Currentspeed is: ", go.read_motor_speed()
def getCurrentSpeed(): print('Current Speed: ', go.read_motor_speed()[0]) return go.read_motor_speed()[0]
print("Motor 2 moving forward at full speed") gopigo.motor2(1,255) time.sleep(2) print("Motor 2 moving forward at half speed") gopigo.motor2(1,127) time.sleep(2) print("Motor 2 stopping ") gopigo.motor2(1,0) time.sleep(1) print("Motor 2 moving back at full speed") gopigo.motor2(0,255) time.sleep(2) print("Motor 2 moving back at half speed") gopigo.motor2(0,127) time.sleep(2) print("Motor 2 stopping") gopigo.motor2(1,0) spd=gopigo.read_motor_speed() print ("Current speed M1:%d ,M2:%d " %(spd[0],spd[1])) print("Changing speed") gopigo.set_speed(200) # Setting motor speed to 200, so that the motors still move when the next program uses it spd=gopigo.read_motor_speed() print ("New speed M1:%d ,M2:%d " %(spd[0],spd[1]))
def read_motor_speed(kargs): r = {'return_value': gopigo.read_motor_speed()} return r