u11 = np.sign(u1[0]) * (np.abs(u1[0]))**(c3)
u21 = np.sign(u1[1]) * (np.abs(u1[1]))**(c3)
u31 = np.sign(u1[2]) * (np.abs(u1[2]))**(c3)
u = np.array([u11, u21, u31]).reshape(3, 1)
# ~ u = np.sign(u1) * (np.abs(u1)) ** (c3)
# ~ print(str(u) +' , '+ str(u1))
# ~ print(str(int(u[1]))+" , "+str(int(u[0]))+" , "+str(int(u[2])))
m2_pwm_control = float(u[0]) # motor 2 speed [rpm]
m1_pwm_control = float(u[1]) # motor 1 speed [rpm]
m3_pwm_control = float(u[2]) # motor 3 speed [rpm]
######################################################## Sending input U to Arduino
robot.motor_pwm(int(m1_pwm_control), int(m2_pwm_control),
int(m3_pwm_control))
######################################################## Getting RPM from Arduino
m1_rpm_u = int(robot.rpm(0))
m2_rpm_u = int(robot.rpm(1))
m3_rpm_u = int(robot.rpm(2))
data_rpm_u = str(m1_rpm_u) + ' , ' + str(
m2_rpm_u) + ' , ' + str(m3_rpm_u)
# ~ print(data_rpm_u)
######################################################## odometry using encoder
pose = odometryCalc(xc, yc, thetac)
pos = odometry_RealSense()
current_x = pose.item(0)
current_y = pose.item(1)
u = np.array([u11,u21,u31]).reshape(3,1) print(u1) ######################################################## commanded RPM # ~ wheel1RPM = float(u[0]) # motor 2 speed [rpm] # ~ wheel0RPM = float(u[1]) # motor 1 speed [rpm] # ~ wheel2RPM = float(u[2]) # motor 3 speed [rpm] wheel1RPM = float(u1[0]) # motor 2 speed [rpm] wheel0RPM = float(u1[1]) # motor 1 speed [rpm] wheel2RPM = float(u1[2]) # motor 3 speed [rpm] data_c_rpm = str(wheel0RPM)+' , ' +str(wheel1RPM)+ ' , ' +str(wheel2RPM) ######################################################## Sending input RPM to Arduino robot.motor_pwm(int(wheel0RPM),int(wheel1RPM),int(wheel2RPM)) ######################################################## odometry using encoder pose = odometryCalc(xc,yc,thetac) pos = odometry_RealSense() current_x = pose.item(0) current_y = pose.item(1) current_theta = pose.item(2) ######################################################## odometry using RealSense # ~ current_x = pos_x # ~ current_y = pos_y # ~ current_theta = pose.item(2) ######################################################## RealSense velocities
#folder where saving all the data
save_folder = "traf_data/"
current_directory = os.getcwd()
save_directory = os.path.join(current_directory, save_folder)
try:
while True:
mode = str(input("Enter s to start "))
if mode == 's':
while True:
file = open(save_folder + "RPM_filter_openloop" + ".txt", "a")
######################################################## Sending input U to Arduino
robot.motor_pwm(int(100), int(150), int(255))
######################################################## Getting RPM from Arduino
m1_rpm = int(robot.rpm(0))
m2_rpm = int(robot.rpm(1))
m3_rpm = int(robot.rpm(2))
data_rpm = str(m1_rpm) + ' , ' + str(m2_rpm) + ' , ' + str(
m3_rpm)
print(data_rpm)
######################################################## Recording data
time_running = time.time()
file.writelines(
str(m1_rpm) + " , " + str(m2_rpm) + " , " + str(m3_rpm) +
" , " + str(time_running) + "\n")