def move(X, Y):
distX = 1000
distY = 1000
while abs(distX) > 55 or abs(distY) > 55:
L, R = get_motors()
Odo = odometry(L, R)
#time.sleep(0.1)
distX = X - Odo[0]
distY = Y - Odo[1]
print Odo
print 'distX ' + str(distX) + ' distY ' + str(distY)
distToGoal = np.sqrt(distX * distX + distY * distY)
angleToGoal = np.arctan2(distY, distX) - Odo[2]
if angleToGoal > 2 * np.pi - angleToGoal:
angleToGoal = -(2 * np.pi - angleToGoal)
elif angleToGoal < -np.pi:
angleToGoal = 2*np.pi + angleToGoal
print 'angleToGoal ' + str(angleToGoal) + ' distToGoal ' + str(distToGoal)
# Movimiento
velGain = distToGoal * 0.5
angleGain = angleToGoal * 2.5
print 'velGain ' + str(velGain) + ' angleGain ' + str(angleGain)
dist_L = 1000
dist_R = (angleGain * 2 * S) + dist_L
speed = min(velGain,300)
envia(ser, 'SetMotor LWheelDist ' + str(dist_L) + ' RwheelDist ' + str(dist_R) + ' Speed ' + str(speed))
def goto(y_neato, theta_neato, speed):
# x and y where we want to go
# x_neato and y_neato where we are
S = 121.5
tiempo = 5
estat = 'RECUPERANT'
if y_neato > 100 or y_neato < -100:
theta = math.atan2(float(-y_neato), 500)
theta = ((theta - theta_neato) % (2 * math.pi))
if theta > math.pi:
theta = theta - 2 * math.pi
distancia_R = ((speed + (S * theta)) * tiempo)
distancia_L = ((speed + (-S * theta)) * tiempo)
elif theta_neato > 0.1 or theta_neato < -0.1:
print theta_neato
theta = theta_neato
if theta > math.pi:
theta = theta - 2 * math.pi
theta = -theta
distancia_R = (S * theta)
distancia_L = (-S * theta)
else:
estat = 'RECTE'
distancia_R = (speed * tiempo)
distancia_L = (speed * tiempo)
comando = 'SetMotor LWheelDist ' + str(distancia_L) + ' RWheelDist ' + str(
distancia_R) + ' Speed ' + str(speed)
envia(ser, comando)
return estat
def stop_motors():
direccion = 0
speed = 0
tita_dot = 0
distancia_L = 0
distancia_R = 0
envia(ser, 'SetMotor LWheelDisable RWheelDisable', 0.2)
envia(ser, 'SetMotor RWheelEnable LWheelEnable', 0.2)
def wait_motors():
L, R = get_motors()
Odo = odometry(L, R)
time.sleep(0.2)
PrevOdo = Odo[:]
PrevOdo[0] = PrevOdo[0] + 1
while PrevOdo != Odo:
L, R = get_motors()
PrevOdo = Odo[:]
Odo = odometry(L, R)
time.sleep(0.2)
envia(ser, 'PlaySound 2')
def enable_laser(ser, b):
""" Activates or deactivates the laser depending on whether the value of b is True or False. """
# msg = ''
if b:
msg = envia(ser, 'SetLDSRotation On')
print("Enable Laser\n")
else:
msg = envia(ser, 'SetLDSRotation Off')
print("Disable Laser\n")
if msg != '\032':
print("[ROBOT LASER]", msg)
def all_stop(viewer): envia(ser,'SetMotor LWheelDisable RWheelDisable', 0.2) envia(ser,'SetMotor RWheelEnable LWheelEnable', 0.2) envia(ser,'SetLDSRotation Off',0.1) termios.tcsetattr(sys.stdin, termios.TCSADRAIN, old_settings) envia(ser, 'TestMode Off', 0.2) ser.close() viewer.quit()
def get_motors():
""" Ask to the robot for the current state of the motors. """
msg = envia(ser, 'GetMotors LeftWheel RightWheel').split('\n')
# For better understanding see the neato commands PDF.
l = int(msg[4].split(',')[1])
r = int(msg[8].split(',')[1])
return l, r
def getLDS():
msg = envia(ser, 'GetLDSScan', 0.2, False)
var = []
for line in msg.split('\r\n')[2:362]:
s = line.split(',')
var.append([s[0], s[1], s[2], s[3]])
return var
def getLDS():
res = envia(ser, 'GetLDSScan',0.2,False)
var = []
for line in res.split('\r\n')[2:362]:
l = line.split(',')
var.append([l[0], l[1], l[2], l[3]])
return var
def get_laser():
msg = envia(ser, 'GetLDSScan', 0.2, True)
#time.sleep(0.1)
var = []
for line in msg.split('\r\n')[2:362]:
s = line.split(',')
var.append([s[0], s[1], s[2], s[3]])
return var
def get_laser():
msg = envia(ser,'GetLDSScan',0.1)
laser_values=[]
for line in msg.split('\r\n')[2:362]:
s = line.split(',')
if int(s[3])==0:
lr = [int(s[0]), int(s[1])]
laser_values.append(lr)
return laser_values
def get_laser(ser):
""" Ask to the robot for the current values of the laser. """
msg = envia(ser, 'GetLDSScan')
# print(msg)
ret = []
for line in msg.split('\r\n')[2:362]:
s = line.split(',')
lr = [s[0], s[1], s[2], s[3]]
ret.append(lr)
return ret
def get_laser():
msg = envia(ser, 'GetLDSScan', 0.2, True)
print("GetLDSScan ----------------------------")
print msg
var = []
for line in msg.split('\r\n')[2:362]:
s = line.split(',')
var.append([s[0], s[1], s[2], s[3]])
return var
def getWalls():
global laserData
msg = envia(ser, 'GetLDSScan').split('\n')
laserData = []
for i in range(0, 360):
distMM = (float(msg[i + 2].split(',')[1]))
laserData.append([float(i), distMM])
relativeWalls()
def odometry(L, R):
#Implement the pos integration. Assume initial conditions [0,0,0].
#Use global variables, discoment this line
#global VARIABLES
global x_world, y_world, theta_world
global L_ini, R_ini
new_L, new_R = L - L_ini, R - R_ini
L_ini = L
R_ini = R
envia(ser, 'PlaySound 19')
delta_d = (new_L + new_R) / 2.
delta_theta = (new_R - new_L) / 243.
x_world += delta_d * np.cos(theta_world)
y_world += delta_d * np.sin(theta_world)
theta_world += delta_theta
theta_world = np.mod(theta_world, 2 * np.pi)
#print(new_L, new_R)
#print(x_world, y_world, theta_world)
#print(L, R)
return [x_world, y_world, theta_world]
def driveTo(X, Y):
L, R = get_motors()
odom = odometry(L, R)
x_go = X - odom[0]
y_go = Y - odom[1]
angleRest = numpy.arctan2(y_go, x_go)
if odom[2] - angleRest > 2 * math.pi - (odom[2] - angleRest):
angle = -(2 * math.pi - (odom[2] - angleRest))
else:
angle = odom[2] - angleRest
envia(
ser, 'SetMotor LWheelDist ' + str(angle * distance_weels_mid) +
' RWheelDist ' + str(-angle * distance_weels_mid) + ' Speed ' +
str(speed))
time.sleep(abs((angle * distance_weels_mid) / speed))
Dist = numpy.sqrt(x_go * x_go + y_go * y_go)
envia(
ser, 'SetMotor LWheelDist ' + str(Dist) + ' RwheelDist ' + str(Dist) +
' Speed ' + str(speed))
time.sleep(Dist / speed)
envia(
ser, 'SetMotor LWheelDist ' + str(-angle * distance_weels_mid) +
' RWheelDist ' + str(angle * distance_weels_mid) + ' Speed ' +
str(speed))
time.sleep(abs((angle * distance_weels_mid) / speed))
def move(X, Y):
#Moves the robot to the point X, Y
difTheta = np.arctan2(Y, X)
difThetaInv = difTheta + 2 * np.pi
#if Odo[2] - ElAnguloQueHayEntreLosDos > 2*np.pi - (Odo[2] - ElAnguloQueHayEntreLosDos):
if -difTheta > difThetaInv:
NewTheta = -difThetaInv
else:
NewTheta = -difTheta
#Turn
envia(
ser, 'SetMotor LWheelDist ' + str(NewTheta * S) + ' RWheelDist ' +
str(-NewTheta * S) + ' Speed ' + str(150))
L, R = get_motors()
PostOdo = odometry(L, R)
time.sleep(0.1)
PrevOdo = PostOdo[:]
PrevOdo[0] = PrevOdo[0] + 1
while PrevOdo != PostOdo:
L, R = get_motors()
PrevOdo = PostOdo[:]
PostOdo = odometry(L, R)
#time.sleep(0.1)
#Move
Dist = np.sqrt(X * X + Y * Y) * 0.85
envia(
ser, 'SetMotor LWheelDist ' + str(Dist) + ' RwheelDist ' + str(Dist) +
' Speed ' + str(300))
L, R = get_motors()
PostOdo = odometry(L, R)
time.sleep(0.1)
PrevOdo = PostOdo[:]
PrevOdo[0] = PrevOdo[0] + 1
while PrevOdo != PostOdo:
L, R = get_motors()
PrevOdo = PostOdo[:]
PostOdo = odometry(L, R)
#time.sleep(0.1)
return PostOdo
def move(X, Y, odometry_queue, laser_queue, laser_set):
distX = 1000
distY = 1000
while abs(distX) > 55 or abs(distY) > 55:
L, R = get_motors()
Odo = odometry(L, R)
laser_points = get_laser()
var = polar_to_cart(laser_points)
if len(var) > 0:
var = cart_to_world(var, Odo)
for i in var:
tuple = (int(i[0]), int(i[1]))
if tuple not in laser_set:
laser_queue.put([(i[0], i[1]), (100, 100)])
laser_set.add(tuple)
odometry_queue.put([(Odo[0], Odo[1]), (100, 100)])
#time.sleep(0.1)
distX = X - Odo[0]
distY = Y - Odo[1]
#print Odo
#print 'distX ' + str(distX) + ' distY ' + str(distY)
distToGoal = np.sqrt(distX * distX + distY * distY)
angleToGoal = np.arctan2(distY, distX) - Odo[2]
if angleToGoal > 2 * np.pi - angleToGoal:
angleToGoal = -(2 * np.pi - angleToGoal)
elif angleToGoal < -np.pi:
angleToGoal = 2 * np.pi + angleToGoal
#print 'angleToGoal ' + str(angleToGoal) + ' distToGoal ' + str(distToGoal)
# Movimiento
velGain = distToGoal * 0.5
angleGain = angleToGoal * 2.5
#print 'velGain ' + str(velGain) + ' angleGain ' + str(angleGain)
dist_L = 1000
dist_R = (angleGain * 2 * S) + dist_L
speed = min(velGain, velocity_max)
envia(
ser, 'SetMotor LWheelDist ' + str(dist_L) + ' RwheelDist ' +
str(dist_R) + ' Speed ' + str(speed))
def move(X, Y):
#Moves the robot to the point X, Y
L, R = get_motors()
Odo = odometry(L, R)
time.sleep(0.1)
DistX = X - Odo[0]
DistY = Y - Odo[1]
ElAnguloQueHayEntreLosDos = np.arctan2(DistY, DistX)
if Odo[2] - ElAnguloQueHayEntreLosDos > 2 * np.pi - (
Odo[2] - ElAnguloQueHayEntreLosDos):
NewTheta = -(2 * np.pi - (Odo[2] - ElAnguloQueHayEntreLosDos))
else:
NewTheta = Odo[2] - ElAnguloQueHayEntreLosDos
print(ElAnguloQueHayEntreLosDos)
print(DistX, DistY)
#Turn
envia(
ser, 'SetMotor LWheelDist ' + str(NewTheta * S) + ' RWheelDist ' +
str(-NewTheta * S) + ' Speed ' + str(100))
L, R = get_motors()
PostOdo = odometry(L, R)
time.sleep(0.1)
PrevOdo = PostOdo[:]
PrevOdo[0] = PrevOdo[0] + 1
while PrevOdo != PostOdo:
L, R = get_motors()
PrevOdo = PostOdo[:]
PostOdo = odometry(L, R)
time.sleep(0.1)
envia(ser, 'PlaySound 2')
#Move
Dist = np.sqrt(DistX * DistX + DistY * DistY)
envia(
ser, 'SetMotor LWheelDist ' + str(Dist) + ' RwheelDist ' + str(Dist) +
' Speed ' + str(200))
L, R = get_motors()
PostOdo = odometry(L, R)
time.sleep(0.1)
PrevOdo = PostOdo[:]
PrevOdo[0] = PrevOdo[0] + 1
while PrevOdo != PostOdo:
L, R = get_motors()
PrevOdo = PostOdo[:]
PostOdo = odometry(L, R)
time.sleep(0.1)
envia(ser, 'PlaySound 2')
def handler(signum, frame):
print "Signal detected"
# Finalizar -------------------------------
print '----------------------------'
envia(ser, 'PlaySound 2')
envia(ser, 'TestMode Off', 0.2)
print 'FIN: TestMode -> off'
envia(ser, 'SetLDSRotation Off', 0.2, False)
time.sleep(3)
print 'FIN: SetLaser -> off'
# Close the Serial Port.
ser.close()
print 'FIN: SerialPort -> off'
print "FIN: Programa finalizado"
exit(-1)
finally:
termios.tcsetattr(fd, termios.TCSADRAIN, old_settings)
print ch
return ch
# Llamada a la funcion main
if __name__ == '__main__':
global ser
# Open the Serial Port.
ser = serial.Serial(port='/dev/ttyACM0', baudrate=115200, timeout=1)
envia(ser,'TestMode On', 0.2)
envia(ser,'PlaySound 1', 0.3)
envia(ser,'SetMotor RWheelEnable LWheelEnable', 0.2)
# Parametros Robot.
S = 121.5 # en mm
distancia_L = 0 # en mm
distancia_R = 0 # en mm
speed = 0 # en mm/s
tita_dot = 0
tiempo = 20
direccion = 0
print "########################"
resfinal = [res[2], res[1], res[0], res[9], res[8]]
print(resfinal)
return resfinal
#leftMotor.setVelocity(initialVelocity - (centralRightSensorValue + outerRightSensorValue) / 2)
#rightMotor.setVelocity(initialVelocity - (centralLeftSensorValue + outerLeftSensorValue) / 2 - centralSensorValue)
if __name__ == "__main__":
# Open the Serial Port.
global ser
ser = serial.Serial(port='/dev/ttyACM0', baudrate=115200, timeout=0.05)
envia(ser, 'TestMode On')
envia(ser, 'PlaySound 1')
envia(ser ,'SetMotor RWheelEnable LWheelEnable')
envia(ser, 'SetLDSRotation On',0.2,False)
try:
#Acercarse al muro
getLaserValues()
while resfinal[2] > 60:
if resfinal[2] < 100:
envia(ser, 'SetMotor LWheelDist 100 RWheelDist 100 Speed 50')
else:
envia(ser, 'SetMotor LWheelDist 200 RWheelDist 200 Speed 100')
def get_motors():
msg = envia(ser, 'GetMotors LeftWheel RightWheel').split('\n')
L = int(msg[4].split(',')[1])
R = int(msg[8].split(',')[1])
return (L, R)
msg = envia(ser, 'GetLDSScan').split('\n')
laserData = []
for i in range(0, 360):
distMM = (float(msg[i + 2].split(',')[1]))
laserData.append([float(i), distMM])
relativeWalls()
if __name__ == "__main__":
# Open the Serial Port.
global ser
ser = serial.Serial(port='/dev/ttyACM0', baudrate=115200, timeout=0.05)
envia(ser, 'TestMode On')
envia(ser, 'PlaySound 1')
envia(ser, 'SetLED LEDRed')
envia(ser, 'SetMotor RWheelEnable LWheelEnable')
envia(ser, 'SetLDSRotation On')
global distanciaIniR, distanciaIniL, sensors, distL, distR, S
#INITIAL CONDITIONS
distanciaIniL, distanciaIniR = get_motors()
S = 121.5
speed = 200 # en mm/s
distL = 0
distR = 0
time.sleep(3)
ser, 'SetMotor LWheelDist ' + str(Dist) + ' RwheelDist ' + str(Dist) +
' Speed ' + str(speed))
time.sleep(Dist / speed)
envia(
ser, 'SetMotor LWheelDist ' + str(-angle * distance_weels_mid) +
' RWheelDist ' + str(angle * distance_weels_mid) + ' Speed ' +
str(speed))
time.sleep(abs((angle * distance_weels_mid) / speed))
if __name__ == "__main__":
# Open the Serial Port.
global ser
ser = serial.Serial(port='/dev/ttyACM0', baudrate=115200, timeout=0.05)
envia(ser, 'TestMode On')
envia(ser, 'PlaySound 1')
envia(ser, 'SetMotor RWheelEnable LWheelEnable')
global L_ini, R_ini
suma_theta = 0
x = y = 0
try:
i = 0
L_ini, R_ini = get_motors()
driveTo(800, 800)
finally:
termios.tcsetattr(fd, termios.TCSADRAIN, old_settings)
print ch
return ch
# Llamada a la funcion main
if __name__ == '__main__':
global ser
# Open the Serial Port.
ser = serial.Serial(port='/dev/ttyACM0', baudrate=115200, timeout=1)
envia(ser, 'TestMode On', 0.2)
envia(ser, 'PlaySound 1', 0.3)
envia(ser, 'SetMotor RWheelEnable LWheelEnable', 0.2)
envia(ser, 'SetLDSRotation On', 0.1)
# Parametros Robot.
S = 121.5 # en mm
distancia_L = 0 # en mm
distancia_R = 0 # en mm
speed = 0 # en mm/s
tita_dot = 0
tiempo = 20
direccion = 0
distL = distL * speed / 100
distR = distR * speed / 100
print('SetMotor LWheelDist ' + str(distL) + ' RWheelDist ' + str(distR) +
' Speed ' + str(speed))
envia(
ser, 'SetMotor LWheelDist ' + str(distL) + ' RWheelDist ' +
str(distR) + ' Speed ' + str(speed))
return res
if __name__ == "__main__":
# Open the Serial Port.
global ser
ser = serial.Serial(port='/dev/ttyACM0', baudrate=115200, timeout=0.05)
envia(ser, 'TestMode On')
envia(ser, 'PlaySound 1')
envia(ser, 'SetMotor RWheelEnable LWheelEnable')
envia(ser, 'SetLDSRotation On', 0.2, False)
#envia(ser, 'SetMotor LWheelDist '+ str(100) +' RWheelDist ' + str(100) + ' Speed ' + str(speed))
try:
L, R = get_motors()
while True:
getLaserValues()
time.sleep(0.1)
envia(ser, 'TestMode Off', 0.2)
#print laser[j][1],' < ',sensor[i]
if (int(laser[j][1]) < sensor[i]):
sensor[i] = int(laser[j][1])
return sensor
# -------------------------------------------------------------------------------
# MAIN
# -------------------------------------------------------------------------------
if __name__ == "__main__":
# Iniciamos -------------------------------
global ser
ser = serial.Serial(port='/dev/ttyACM0', baudrate=115200, timeout=0.05)
print 'INI: SerialPort -> on'
envia(ser, 'TestMode On', 0.2)
envia(ser, 'PlaySound 1')
print 'INI: TestMode -> on'
envia(ser, 'SetMotor RWheelEnable LWheelEnable')
print 'INI: SetMotor -> on'
envia(ser, 'SetLDSRotation On', 0.2, False)
time.sleep(3)
print 'INI: SetLaser -> on'
print '----------------------------'
# Config inicial -------------------------------
global L_ini, R_ini
L_ini, R_ini = get_motors()
Odo = odometry(L_ini, R_ini)
time.sleep(0.2)
def getLaserValues():
global speed
res = [2000, 2000, 2000, 2000, 2000, 2000, 2000, 2000, 2000, 2000]
msg = getLDS()
values = [2000 for num in range(360)]
for value in msg:
if int(value[1]) > 0:
values[int(value[0])] = int(value[1])
# Coger 5 posiciones: 0-36, 37-72, 73-108, 109-144, 145-180
# Init
res[0] = values[0]
res[1] = values[36]
res[2] = values[72]
res[3] = values[108]
res[4] = values[144]
res[5] = values[180]
res[6] = values[216]
res[7] = values[252]
res[8] = values[288]
res[9] = values[324]
for i in range(0, 359):
if i >= 0 and i <= 35:
# Zona izquierda
if values[i] < res[0] and values[i] != 0:
res[0] = values[i]
if i >= 36 and i <= 71:
# Zona izquierda central
if values[i] < res[1] and values[i] != 0:
res[1] = values[i]
if i >= 72 and i <= 107:
# Zona central
if values[i] < res[2] and values[i] != 0:
res[2] = values[i]
if i >= 108 and i <= 143:
# Zona derecha central
if values[i] < res[3] and values[i] != 0:
res[3] = values[i]
if i >= 144 and i <= 179:
# Zona derecha
if values[i] < res[4] and values[i] != 0:
res[4] = values[i]
if i >= 180 and i <= 215:
# Zona izquierda
if values[i] < res[5] and values[i] != 0:
res[5] = values[i]
if i >= 216 and i <= 251:
# Zona izquierda central
if values[i] < res[6] and values[i] != 0:
res[6] = values[i]
if i >= 252 and i <= 287:
# Zona central
if values[i] < res[7] and values[i] != 0:
res[7] = values[i]
if i >= 288 and i <= 323:
# Zona derecha central
if values[i] < res[8] and values[i] != 0:
res[8] = values[i]
if i >= 324 and i <= 359:
# Zona derecha
if values[i] < res[9] and values[i] != 0:
res[9] = values[i]
resfinal = [res[2], res[1], res[0], res[9], res[8]]
print(resfinal)
res[0] = res[0] / 10
res[1] = res[1] / 10
res[2] = res[2] / 10
res[8] = res[8] / 10
res[9] = res[9] / 10
print(res)
distInicial = 200
speed = 300
distL = distInicial - (200 - (res[8] + res[9]) / 2) - (200 - res[0])
distR = distInicial - (200 - (res[2] + res[1]) / 2) - (200 - res[0])
distL = distL * speed / 100
distR = distR * speed / 100
print('SetMotor LWheelDist ' + str(distL) + ' RWheelDist ' + str(distR) +
' Speed ' + str(speed))
envia(
ser, 'SetMotor LWheelDist ' + str(distL) + ' RWheelDist ' +
str(distR) + ' Speed ' + str(speed))
return res
def odometry(L, R): #Implement the pos integration. Assume initial conditions [0,0,0]. #Use global variables, discoment this line #global VARIABLES new_L, new_R = L - L_ini, R - R_ini print(new_L, new_R, new_L/new_R) # Return x_word, y_word, theta_word if __name__ == "__main__": # Open the Serial Port. global ser ser = serial.Serial(port='/dev/ttyACM0', baudrate=115200, timeout=0.05) envia(ser, 'TestMode On') envia(ser, 'PlaySound 1') envia(ser ,'SetMotor RWheelEnable LWheelEnable') global L_ini, R_ini L_ini, R_ini = get_motors() speed = 100 # en mm/s envia(ser, 'SetMotor LWheelDist '+ str(5519) +' RWheelDist ' + str(7046) + ' Speed ' + str(speed)) while True: L, R = get_motors() odometry(L, R)