def calc(): global maxX global minX global maxY global minY t_end2 = time.time() + 30 setspeed = 100 #motor = Motor.Motor(18, 25, 24,13, 27, 17) #time.sleep(5) while time.time() < t_end2: magnet = mpu92_forTest.get_magnet() print(magnet) if (maxX < magnet[0]): maxX = magnet[0] if (minX > magnet[0]): minX = magnet[0] if (maxY < magnet[1]): maxY = magnet[1] if (minY > magnet[1]): minY = magnet[1] # print("maxx",maxX) # print("minx",minX) # print() # print("maxy",maxY) # print("miny",minY) # print() motor.set_speed(-setspeed, setspeed) time.sleep(1)
def main(): init() n = 0 while True: n = n + 1 time.sleep(1) #motor.set_speed(n%90,n%90) motor.set_speed(100, 100) print(get_gps())
def forward(): integral_error = 0 last_error = feedback_loop.ideal_right_wall_dist - us.dist() while go_forward == True: values = feedback_loop.adjustment_values(us.dist(), integral_error, last_error) pid_value = values[0] / 100 integral_error = values[1] last_error = values[2] print(pid_value) motor.set_speed('right', motor.default_motor_speed - pid_value) motor.set_speed('left', motor.default_motor_speed + pid_value) time.sleep(0.150)
def turn(direction): if direction == 'right': motor.set_speed('r', -80) motor.set_speed('l', 80) time.sleep(0.8) elif direction == 'left': motor.set_speed('l', -80) motor.set_speed('r', 80) time.sleep(0.8) #set sleep time motor.set_speed('b', 0) #forward()
def main(): init() mpu92_forTest.MPU9265_init() target_location_x= 34.805843 target_location_y= 135.778176 target_rad=0 while True: target_rad = calrad(target_location_x,target_location_y) print("target_rad") print(target_rad * (180/np.pi)) print() if target_rad >0.174533 :#0.174533=10度 motor.set_speed(-100,100) elif target_rad < -0.174533: motor.set_speed(100,-100) else: pass time.sleep(1)
def main(): #set up #センサーセットアップ init() mpu92_forTest.MPU9265_init() #目標をセット target_location_x = 34.805843 target_location_y = 135.778176 target_rad = 0.0 #中心 calc() center_x = get_cecter_x(maxX, minX) center_y = get_cecter_y(maxY, minY) print(center_x) print(center_y) #roverの状態(フェーズ) phase = 0 #move while True: if phase == 0: #x秒前進 print("phase:0") motor.set_speed(100, 100) time.sleep(5) #単位は秒 phase = 1 #次のフェーズへ elif phase == 1: print("phase:1") if -0.001 < (get_gps()[0] - target_location_x) and ( get_gps()[0] - target_location_x) < 0.001: # 緯度の差が0.01以内で if -0.001 < (get_gps()[1] - target_location_y) and ( get_gps()[1] - target_location_y) < 0.001: # かつ経度の差が0.01以内ならば phase = 2 # 次のフェーズへ continue target_rad = calrad(target_location_x, target_location_y, center_x, center_y) print(target_rad * (180 / np.pi)) if target_rad > 0.174533: #0.174533=10度 print("左回転") #target_radを減らす方向 motor.set_speed(50, 100) elif target_rad < -0.174533: print("右回転") # target_radを増やす方向 motor.set_speed(100, 50) else: # -10度< Θ < 10度 print("直進") motor.set_speed(100, 100) time.sleep(1) elif phase == 2: #カメラモード print("phase:2") phase = 3 elif phase == 3: print("phase:3") break GPIO.cleanup() print("正常終了")
def adjustment_values(direction, wall_dist, integral_error, last_error): #defining distances #self.right_wall_dist=right_wall_dist #self.left_wall_distance=left_wall_distance #init errors error = ideal_right_wall_dist - wall_dist integral_error += error * delay diff_error = (error - last_error) / delay last_error = error #if it strikes the wall if error > 4 or error < -400: motor.set_speed('b', -100) time.sleep(0.2) if direction == 'right': motor.set_speed('r', 80) else: motor.set_speed('l', 80) time.sleep(0.3) motor.set_speed('b', 0) #so that it doesnt bend towards openings after turning right if error < -15: error = -0.2 # #as it shouldnt get hyper # if error>18: # error=2 #define tuning constants Kp = 0.75 Ki = 0.0 Kd = 0.32 P = Kp * error I = Ki * integral_error D = Kd * diff_error print(P) print(I) print(D) #if integral_error>=0.15 or integral_error<=-0.15: integral_error=0 pid_value = P + I + D if pid_value > 15: pid_value = 12 if pid_value < -15: pid_value = -12 return [pid_value, integral_error, last_error] '''set_motor_speed('right',motion.default_motor_speed-(P+I+D))
def forward(direction): if direction == 'right': trigger = us.right_trig echo = us.right_echo other = 'left' if direction == 'left': trigger = us.left_trig echo = us.left_echo other = 'right' speed = motor.default_motor_speed + 30 global go_forward integral_error = 0 go_forward = True last_error = feedback_loop.ideal_right_wall_dist - us.dist(trigger, echo) while go_forward == True: values = feedback_loop.adjustment_values(direction, us.dist(trigger, echo), integral_error, last_error) pid_value = values[0] print('motion: ', pid_value) integral_error = values[1] last_error = values[2] motor.set_speed(direction, speed + pid_value) motor.set_speed(other, speed - pid_value) time.sleep(feedback_loop.delay) dist = us.dist(us.front_trig, us.front_echo) if dist < 40: speed = 35 + dist motor.set_speed('b', speed + 30) if us.dist(us.front_trig, us.front_echo) < 15: stop()
def scan_qr(): ideal_qr_us_dist = 19.5 timer = 0.25 #Thread( target=qr_scanner.scan) sleep(1.5) if check_function(): return True while True: error = ideal_qr_us_dist - us.dist(qr_trig, qr_echo) Kp = 2 P = Kp * error if check_function: return True if us.dist(us.qr_trig, us.qr_echo) < ideal_qr_us_dist: motor.set_speed('b', -50 - P) sleep(0.25) motor.set_speed('b', 0) if check_function(): return True else: motor.set_speed('r', -50) sleep(timer) motor.set_speed('r', 50) sleep(0.5) if check_function(): return True motor.set_speed('l', -50) sleep(timer) sleep(0.5) if check_function(): return True motor.sleep('l', 50) sleep(timer) sleep(0.5) if check_function(): return True motor.set_speed('b', 0) timer += 0.05 motor.set_speed('b', 0) return True
def scan_qr(): ideal_qr_us_dist = 19.5 last_error = 0 timer = 0.1 #Thread( target=qr_scanner.scan) sleep(1.5) if check_function(): return True while True: error = ideal_qr_us_dist - us.dist(qr_trig, qr_echo) diff_error = (error - last_error) / feedback_loop.delay last_error = error Kp = 0.5 Kd = 0.25 P = Kp * error D = Kd * diff_error pid_value = P + D if check_function: return True if us.dist(us.qr_trig, us.qr_echo) < ideal_qr_us_dist: motor.set_speed('b', -40 - pid_value) sleep(0.25) motor.set_speed('b', 0) if check_function(): return True else: motor.set_speed('r', -50) sleep(timer) motor.set_speed('r', 50) sleep(0.5) if check_function(): return True motor.set_speed('l', -50) sleep(timer) sleep(0.5) if check_function(): return True motor.sleep('l', 50) sleep(timer) sleep(0.5) if check_function(): return True motor.set_speed('b', 0) timer += 0.05 motor.set_speed('b', 0) return True
def stop(): global go_forward go_forward = False motor.set_speed('right', 0) motor.set_speed('left', 0)
def main(): #set up #センサーセットアップ # motor.set_speed(40, -40) # time.sleep(1.5) # motor.set_speed(80, -80) # time.sleep(1.5) # motor.set_speed(50, -50) # time.sleep(1.5) init() mpu92_forTest.MPU9265_init() #落下 time.sleep(30) #ニクロム線カット nikurom() time.sleep(15) #前進 motor.set_speed(-40, 40) time.sleep(2) motor.set_speed(-80, 80) time.sleep(2) motor.set_speed(-50, 50) time.sleep(2) #目標をセット34.800286, 135.769161 target_location_x = 135.769161 target_location_y = 34.800286 target_rad = 0.0 #中心 # calc() # center_x = get_cecter_x(maxX,minX) # center_y = get_cecter_y(maxY,minY) center_x = -39.6240234375 center_y = 75.29296875 print(center_x) print(center_y) #roverの状態(フェーズ) phase = 1 #move while True: if phase == 0: #x秒前進 print("phase:0") motor.set_speed(-100, 100) time.sleep(1) #単位は秒 phase = 1 #次のフェーズへ elif phase == 1: print("phase:1") print(get_gps()[0], get_gps()[1]) if -0.00005 < (get_gps()[0] - target_location_x) and ( get_gps()[0] - target_location_x) < 0.00005: # 緯度の差が0.01以内で if -0.00005 < (get_gps()[1] - target_location_y) and ( get_gps()[1] - target_location_y) < 0.00005: # かつ経度の差が0.01以内ならば phase = 2 # 次のフェーズへ continue target_rad = calrad(target_location_x, target_location_y, center_x, center_y) print("target_rad") print(target_rad * (180 / np.pi)) if target_rad > 0.174533: #0.174533=10度 print("左回転") #target_radを減らす方向 motor.set_speed(-100, 50) elif target_rad < -0.174533: print("右回転") # target_radを増やす方向 motor.set_speed(-50, 100) else: # -10度< Θ < 10度 print("直進") motor.set_speed(-100, 100) time.sleep(1) elif phase == 2: #カメラモード print("phase:2") phase = 3 elif phase == 3: print("phase:3") GPIO.cleanup() break GPIO.cleanup() print("正常終了")
other='right' speed=80 global go_forward integral_error=0 go_forward=True last_error=feedback_loop.ideal_right_wall_dist_wall_dist-us.dist(trigger,echo) while go_forward==True: values=feedback_loop.adjustment_values(direction,us.dist(trigger,echo),integral_error,last_error) pid_value= values[0] print(pid_value) integral_error=values[1] last_error=values[2] motor.set_speed(direction,motor.default_motor_speed+pid_value) motor.set_speed(other,motor.default_motor_speed-pid_value) time.sleep(feedback_loop.delay) dist=us.dist(us.front_trig,us.front_echo) if dist<40: speed=35+dist motor.set_speed('b',speed+30) if us.dist(front_trig,front_echo)<15: stop() def turn(direction): if direction=='right':
if vector is not None: # Figure out the motor inputs based on speed and vector absvector = abs(vector) speed_1 = absvector * speed speed_2 = (1.0 - absvector) * speed if (vector > 0): left_motor_speed = speed_1 right_motor_speed = speed_2 else: right_motor_speed = speed_1 left_motor_speed = speed_2 else: # Spin in a circle until you find something left_motor_speed = 0.5 right_motor_speed = -0.5 # Set course for adventure print("left_motor_speed", left_motor_speed, "right_motor_speed", right_motor_speed) motor.set_speed(left_motor_speed, right_motor_speed) # Clear out for next frame rawCapture.truncate(0) # Next frame frame_number = frame_number + 1 finally: motor.stop() GPIO.cleanup()