def g2g(xd, yd, thetad):
global current_x
global current_y
global current_theta
# ~ file = open(save_folder2 + "x_"+str(xd)+",y_"+str(yd)+",theta_"+str(thetad)+".txt","a+")
#~ file = open(save_folder2 + "Triangle_Open" +".txt","a")
#~ file = open(save_folder2 + "Square_Open" +".txt","a")
while True:
xc = current_x
yc = current_y
thetac = current_theta
inv_rotation_mat = np.array([
np.cos(thetac),
np.sin(thetac), 0, -np.sin(thetac),
np.cos(thetac), 0, 0, 0, 1
]).reshape(3, 3)
d = np.sqrt(((xd - xc)**2) + ((yd - yc)**2))
phi = math.atan2((yd - yc), (xd - xc))
vel_global = np.array(
[d * np.cos(phi), d * np.sin(phi),
-1 * (current_theta - thetad)])[:, None]
vel_local = np.dot(inv_rotation_mat, vel_global)
v_x = vel_local[0]
v_y = vel_local[1]
v_theta = vel_local[2]
robot.move(v_x, v_y, v_theta)
pose = odemetryCalc(xc, yc, thetac)
pos = odemetry_RealSense()
current_x = pos_x
current_y = pos_y
current_theta = pose.item(2)
# ~ print (current_x)
# ~ print (current_y)
delta = np.sqrt(((xd - current_x)**2) + ((yd - current_y)**2)) #< 0.1
data_write = "x: " + str(pos_x) + " y: " + str(
pos_y) + " theta: " + str(pose.item(2))
print(data_write)
# ~ file.writelines(str(pose[0][0])+" , "+str(pose[1][0])+" , "+str(pose[2][0])+"\n")
if delta < 0.04:
# ~ file.close()
robot.stop()
break
# leftleft,left,center,right,rightright
us = [0,0,0,0,0]
alreadyForward = False
count = 0
while True:
for x in range(5):
us[x] = robot.ultrasound(x+1)
for x in range(5):
print("Ultrasound "+str(x)+": "+str(us[x]))
print(count)
if(count >=50):
robot.move(-.4,0,0)
time.sleep(.7)
robot.move(0,0,2)
time.sleep(.7)
count=0
if(us[2] <=15):
if(us[1] < us[3]):
robot.move(0,0,-2)
else:
robot.move(0,0,2)
count +=1
elif(us[1] <= 29):
robot.move(0,0,-2)
count +=1
elif(us[3] <=29):
robot.move(0,0,2)
def g2g_oa(xd, yd, thetad):
######################################### G2G #####################################
global current_x
global current_y
global current_theta
dt = 0.1
#PID goToGoal
Kp = 2
Ki = 0.05
Kd = 0
integral = np.array([0, 0, 0])[:, None]
preError = np.array([0, 0, 0])[:, None]
min_distance = 0.15
delta = np.sqrt(((xd - current_x)**2) + ((yd - current_y)**2))
while delta > min_distance:
file = open(
save_folder + "x_" + str(xd) + ",y_" + str(yd) + ",theta_" +
str(thetad) + ".txt", "a+")
xc = current_x
yc = current_y
thetac = current_theta
pose = odemetryCalc(xc, yc, thetac)
#PID Controller
setPoint = np.array([xd, yd, thetad])[:, None]
currentPoint = np.array([xc, yc, thetac])[:, None]
error = setPoint - currentPoint
preError = error
integral = integral + error
derivative = error - preError
output = Kp * error + Ki * integral + Kd * derivative
vel_global = output
#Inverse Kinematic
inv_rotation_mat = np.array([
np.cos(thetac),
np.sin(thetac), 0, -np.sin(thetac),
np.cos(thetac), 0, 0, 0, 1
]).reshape(3, 3)
#~ vel_global = np.array([ d*np.cos(phi), d*np.sin(phi), 0])[:,None]
vel_local = np.dot(inv_rotation_mat, vel_global)
v_x_g2g = vel_local[0]
v_y_g2g = vel_local[1]
v_theta_g2g = vel_local[2]
################################################################################################################
######################################### Obstacle Avoidance ############################
for x in range(6):
us[x] = robot.ultrasonic(x)
#~ print(str(us[0])+", "+str(us[1])+", "+str(us[2])+", "+str(us[3])+", "+str(us[4])+", "+str(us[5]))
global d
d = [us[0], us[1], us[2], us[3], us[4], us[5]]
if d[x] != 0:
flag[x] = 1
else:
flag[x] = 0
#~ print(flag)
if d[x] != 0:
d0 = d[0]
d1 = d[1]
d2 = d[2]
d3 = d[3]
d4 = d[4]
d5 = d[5]
vs0 = us0(d0)
vs1 = us1(d1)
vs2 = us2(d2)
vs3 = us3(d3)
vs4 = us4(d4)
vs5 = us5(d5)
global vl_s
vl_s = flag[0] * vs0 + flag[1] * vs1 + flag[2] * vs2 + flag[
3] * vs3 + flag[4] * vs4 + flag[5] * vs5
############################################### Combine G2G and OA #####################################
v_x_obs = -vl_s[0]
v_y_obs = -vl_s[1]
#~ print(str(v_x_obs)+' , '+str(v_y_obs))
#~ print(d)
for x in d:
global v_x
global v_y
global v_theta
if d == [0, 0, 0, 0, 0, 0]:
v_x = 0.3 * v_x_g2g
v_y = 0.3 * v_y_g2g
v_theta = v_theta_g2g
#~ speed = 120
elif x > 0.3 and x <= 0.4:
v_x = 0.3 * v_x_g2g + 0.7 * v_x_obs
v_y = 0.3 * v_y_g2g + 0.7 * v_y_obs
v_theta = v_theta_g2g
#~ print('30 cm')
#~ speed = 100
elif x > 0.2 and x <= 0.3:
v_x = 0.2 * v_x_g2g + 0.8 * v_x_obs
v_y = 0.2 * v_y_g2g + 0.8 * v_y_obs
v_theta = v_theta_g2g
#~ print('20 cm')
#~ speed = 80
elif x > 0.1 and x <= 0.2:
v_x = 0.1 * v_x_g2g + 0.9 * v_x_obs
v_y = 0.1 * v_y_g2g + 0.9 * v_y_obs
v_theta = v_theta_g2g
#~ print('10 cm')
#~ speed = 70
elif x > 0 and x <= 0.1:
v_x = v_x_obs
v_y = v_y_obs
v_theta = v_theta_g2g
#~ print('0 cm')
#~ speed = 200
elif d[0] != 0:
v_theta = 3.14
robot.move(v_x, v_y, v_theta)
#Odemetry
current_x = pose.item(0)
current_y = pose.item(1)
current_theta = pose.item(2)
delta = np.sqrt(((xd - current_x)**2) + ((yd - current_y)**2))
#~ time.sleep(dt)
data_write = "x: " + str(pose[0][0]) + " y: " + str(
pose[1][0]) + " theta: " + str(pose[2][0])
print(data_write)
file.writelines(
str(pose[0][0]) + " , " + str(pose[1][0]) + " , " +
str(pose[2][0]) + "\n")
#~ file.writelines(data_write)
file.close()
robot.stop()
# ~ initOdometry()
# ~ g2g_oa(xd,yd,thetad)
############### Contoller demo for testing ################
mode = str(input("Enter mode: c for controller "))
if mode == 'c':
joy = xbox.Joystick()
theta = 0
while True:
theta = 0
time.sleep(0)
if (joy.B()):
joy.close()
motors(0, 0, 0)
quit()
(x, y) = joy.leftStick()
(x1, y1) = joy.rightStick()
print("x: " + str(y))
print("y: " + str(x1))
robot.move(y / 1.5, -x / 1.5, x1 * np.pi)
## Ctrl + c to stop robot
except KeyboardInterrupt:
# Close serial connection
robot.stop()
#~ file.close()
print('\n\n Stop!!! See you again!')
robot.motorVelocity(0,50,-50)
else:
d0 = d[0]
d1 = d[1]
d2 = d[2]
d3 = d[3]
d4 = d[4]
d5 = d[5]
vs0 = us0(d0,xc,yc,thetac)
vs1 = us1(d1,xc,yc,thetac)
vs2 = us2(d2,xc,yc,thetac)
vs3 = us3(d3,xc,yc,thetac)
vs4 = us4(d4,xc,yc,thetac)
vs5 = us5(d5,xc,yc,thetac)
v = flag[0]*vs0 + flag[1]*vs1 + flag[2]*vs2 + flag[3]*vs3 + flag[4]*vs4 + flag[5]*vs5
xd = -v.item(0)
yd = -v.item(1)
robot.move(xd,yd,0)
## Ctrl + c to stop robot
except KeyboardInterrupt:
# Close serial connection
robot.stop()
print('\n Stop!!! See you again!')
#~ d0 = us[0]
#~ obs0pos = us0(d0,xc,yc,thetac)
#~ xd0 = -obs0pos.item(0)
#~ yd0 = -obs0pos.item(1)
#~ robot.move(xd0,yd0,0)
########################################## sensor 1 (left)
if (us[1] > 0) and (us[1] <= 0.2):
d1 = us[1]
obs1pos = us1(d1, xc, yc, thetac)
#~ print(obs1pos)
xd1 = -obs1pos.item(0)
yd1 = -obs1pos.item(1)
robot.move(xd1, yd1, 0)
########################################## sensor 2
elif (us[2] != 0):
d2 = us[2]
obs2pos = us2(d2, xc, yc, thetac)
xd2 = -obs2pos.item(0)
yd2 = -obs2pos.item(1)
robot.move(xd2, yd2, 0)
if (us[1] != 0):
robot.move(0, 0, -1.57)
time.sleep(.14)
elif (us[1] == 0) and (us[5] == 0):
robot.move(0, 0, -1.57)
time.sleep(.14)
def g2g_pid(xd, yd, thetad):
######################################### G2G #####################################
global current_x
global current_y
global current_theta
dt = 0.1
#PID goToGoal
Kp = 2
Ki = 0.05
Kd = 0
integral = np.array([0, 0, 0])[:, None]
preError = np.array([0, 0, 0])[:, None]
min_distance = 0.01
delta = np.sqrt(((xd - current_x)**2) + ((yd - current_y)**2))
while delta > min_distance:
file = open(
save_folder1 + "x_" + str(xd) + ",y_" + str(yd) + ",theta_" +
str(thetad) + ".txt", "a+")
#~ file = open(save_folder1 + "Triangle_Closed" +".txt","a")
#~ file = open(save_folder1 + "Square_Closed" +".txt","a")
xc = current_x
yc = current_y
thetac = current_theta
pose = odemetryCalc(xc, yc, thetac)
#PID Controller
setPoint = np.array([xd, yd, thetad])[:, None]
currentPoint = np.array([xc, yc, thetac])[:, None]
error = setPoint - currentPoint
preError = error
integral = integral + error
derivative = error - preError
output = Kp * error + Ki * integral + Kd * derivative
vel_global = output
#Inverse Kinematic
inv_rotation_mat = np.array([
np.cos(thetac),
np.sin(thetac), 0, -np.sin(thetac),
np.cos(thetac), 0, 0, 0, 1
]).reshape(3, 3)
#~ vel_global = np.array([ d*np.cos(phi), d*np.sin(phi), 0])[:,None]
vel_local = np.dot(inv_rotation_mat, vel_global)
v_x = vel_local[0]
v_y = vel_local[1]
v_theta = vel_local[2]
robot.move(v_x, v_y, v_theta)
#Odemetry
current_x = pos_x
current_y = pos_y
current_theta = pose.item(2)
delta = np.sqrt(((xd - current_x)**2) + ((yd - current_y)**2))
data_write = "x: " + str(pose[0][0]) + " y: " + str(
pose[1][0]) + " theta: " + str(pose[2][0])
print(data_write)
file.writelines(
str(pose[0][0]) + " , " + str(pose[1][0]) + " , " +
str(pose[2][0]) + "\n")
file.close()
robot.stop()
