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()
