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
