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
rpm = [] printSetup = 1 enablePrint(printSetup) for line in lines: x = line.split(',')[0] y = line.split(',')[1] timer = x rpm = y RPM_Values(int(rpm)) #~ file = open(save_folder + "Motor: "+str(motor_num)+".txt","a") file = open(save_folder + "All Motors: 1,2,3 " + ".txt", "a") start = time.time() while time.time() - float(start) <= float(timer): data = [0] if (ser.inWaiting() > 0): data = (ser.readline().decode()) sys.stdout.write(data) sys.stdout.write(rpm) file.writelines(" {0} , {1}".format(data[:-2], rpm)) file.close ## Ctrl + c to stop robot except KeyboardInterrupt: # Close serial connection robot.stop() print('\n\n Stop!!! See you again!')
def path_f(xd, yd, thetad, b, a, R, step): global current_x global current_y global current_theta delta_min = 0.1 vr = 0.1 k = 5 Kx = k Ky = k Kz = k file = open( save_folder + "Circle" + "_vr_" + str(vr) + "_K_" + str(k) + "_delta_" + str(delta_min) + '_R_' + str(R) + '_step_' + str(step) + ".txt", "a") # ~ file = open(save_folder2 + "Circle"+"_vr_"+str(vr)+"_K_"+str(Kx)+"_"+str(Ky)+"_"+str(Kz)+"_delta_"+str(delta_min)+".txt","a") while True: xc = current_x yc = current_y thetac = current_theta if (a - xd == 0): x_dot_d = vr y_dot_d = 0 theta_dot_d = 0.1 if (yd - b == 0): x_dot_d = 0 y_dot_d = vr theta_dot_d = 0.1 else: dydx = (a - xd) / (yd - b) x_dot_d = vr / np.sqrt(1 + dydx * dydx) y_dot_d = dydx * x_dot_d theta_dot_d = 0.1 q_dot_d = np.array([x_dot_d, y_dot_d, theta_dot_d]).reshape(3, 1) j = (2 * np.pi * 0.03 / 60) * np.array( [(2 / 3) * np.sin(thetad + np.pi / 3), (-2 / 3) * np.sin(thetad), (2 / 3) * np.sin(thetad - np.pi / 3), (-2 / 3) * np.cos(thetad + np.pi / 3), (2 / 3) * np.cos(thetad), (-2 / 3) * np.cos(thetad - np.pi / 3), -1 / (3 * 0.19), -1 / (3 * 0.19), -1 / (3 * 0.19)]).reshape(3, 3) j_inv = np.linalg.inv(j).reshape(3, 3) K = np.array([Kx, 0, 0, 0, Ky, 0, 0, 0, Kz]).reshape(3, 3) e = np.array([(xc - xd), (yc - yd), (thetac - thetad)]).reshape(3, 1) s = np.dot(-K, e, out=None) + q_dot_d ######################################################## Input W (RPM) w = np.dot(j_inv, s, out=None) motor_spd_vec = w # ~ print(vm) ######################################################## commanded RPM wheel1RPM = motor_spd_vec[0] # motor 2 speed [rpm] wheel0RPM = motor_spd_vec[1] # motor 1 speed [rpm] wheel2RPM = motor_spd_vec[2] # motor 3 speed [rpm] c_rpm1 = float(wheel0RPM) c_rpm2 = float(wheel1RPM) c_rpm3 = float(wheel2RPM) robot.motor_rpm(int(wheel0RPM), int(wheel1RPM), int(wheel2RPM)) pose = odometryCalc(xc, yc, thetac) pos = odometry_RealSense() current_x = pose.item(0) current_y = pose.item(1) current_theta = pose.item(2) #use RealSense as feedback # ~ current_x = pos_x # ~ current_y = pos_y # ~ current_theta = theta_rs vel = np.sqrt(vel_x * vel_x + vel_y * vel_y + vel_z * vel_z) # ~ vel = str(vel_x)+" , "+str(vel_y) # ~ print(vel) delta = np.sqrt(((xd - current_x)**2) + ((yd - current_y)**2)) # ~ print(delta) m1_rpm = robot.rpm(0) m2_rpm = robot.rpm(1) m3_rpm = robot.rpm(2) data_rpm = str(m1_rpm) + ' , ' + str(m2_rpm) + ' , ' + str(m3_rpm) # ~ print(data_rpm) # ~ start = time.time() time_running = time.time() # ~ print(time_running) data_pose = "x: " + str(pose[0][0]) + " y: " + str( pose[1][0]) + " theta: " + str(pose[2][0]) print(data_pose) # ~ file.writelines(str(pose[0][0])+" , "+str(pose[1][0])+" , "+str(pose[2][0])+" , "+str(pos_x)+" , "+str(pos_y)+" , "+str(m1_rpm)+" , "+str(m2_rpm)+" , "+str(m3_rpm)+" , "+str(time_running)+ "\n") # ~ file.writelines(str(pose[0][0])+" , "+str(pose[1][0])+" , "+str(pose[2][0])+" , "+str(pos_x)+" , "+str(pos_y)+" , "+str(m1_rpm)+" , "+str(m2_rpm)+" , "+str(m3_rpm)+" , "+str(vel_x)+" , "+str(vel_y)+" , "+str(vel)+" , "+str(time_running)+ "\n") file.writelines( str(pose[0][0]) + " , " + str(pose[1][0]) + " , " + str(pose[2][0]) + " , " + str(pos_x) + " , " + str(pos_y) + " , " + str(m1_rpm) + " , " + str(m2_rpm) + " , " + str(m3_rpm) + " , " + str(c_rpm1) + " , " + str(c_rpm2) + " , " + str(c_rpm3) + " , " + str(vel_x) + " , " + str(vel_y) + " , " + str(vel) + " , " + str(time_running) + "\n") if delta < delta_min: file.close() robot.stop() break
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()
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()