This repository has been archived by the owner on Sep 3, 2023. It is now read-only.
-
Notifications
You must be signed in to change notification settings - Fork 2
/
buoy.py
116 lines (100 loc) · 2.72 KB
/
buoy.py
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
from navigation.rc import RCLib
from navigation.ac import ACLib
import time
import navigation.imu as imu
from navigation.log import *
from navigation.fwdSonar import *
log = LogLib()
rc = RCLib(log)
ac = ACLib()
log.setLevel(INFO)
fwdSonar = FwdSonarThread(ac, log)
fwdSonar.start()
rc.setmode('MANUAL')
rc.arm()
#print(rc.getDeg())
# global "origin" parameter
START = 239
DEPTH_SEC = 5.2
SEC_PER_METER = 1
GATECENTER = 2500
def leftAlign(value) :
pingReturn = ac.get_distance_fwd()
wallDist = pingReturn[0]
confidence = pingReturn[1]
counter = 0
while ((confidence < 90) and (counter < 50)):
pingReturn = ac.get_distance_fwd()
wallDist = pingReturn[0]
confidence = pingReturn[1]
counter = counter + 1
print 'wall' , wallDist
if wallDist > 3800:
wallDist = 3800
if wallDist < 500:
wallDist = 500
print 'wall' , wallDist
print 'counter ' , counter
diff = value - wallDist
print 'Laterl diff ' , diff
rc.lateralDist(diff)
def rightAlign(value) :
pingReturn = ac.get_distance_right()
wallDist = pingReturn[0]
confidence = pingReturn[1]
counter = 0
while ((confidence < 90) and (counter < 50)):
pingReturn = ac.get_distance_right()
wallDist = pingReturn[0]
confidence = pingReturn[1]
counter = counter + 1
print 'wall' , wallDist
if wallDist > 3800:
wallDist = 3800
if wallDist < 500:
wallDist = 500
print 'wall' , wallDist
print 'counter ' , counter
diff = wallDist - value
print 'Laterl diff ' , diff
rc.lateralDist(diff)
def lowestSonar(speed) :
#pwm = 1500 + (0.4*speed*1000)
pwm=1560
rc.raw('yaw', pwm)
prev_val = 0
current_val, _ = fwdSonar.dist()
diff_accum = 0
diff = 0
while (abs(diff) >25 or abs(diff_accum) < 2500):
dist, conf = fwdSonar.dist()
if(conf > 70):
prev_val = current_val
current_val = dist
diff = abs(current_val-prev_val)
diff_accum = diff+diff_accum
rc.raw('yaw', pwm)
log.info(' prev %d' % prev_val)
log.info(' current %d' % current_val )
log.info(' diff %d' % diff)
log.info(' accu_diff %d' % diff_accum)
else:
rc.raw('yaw', pwm)
pass
time.sleep(0.01)
rc.raw('yaw', 1500)
print("Starting Buoy Test")
log.info('Starting buoy test')
rc.setmode('ALT_HOLD')
rc.throttle("time", 2, -0.25)
lowestSonar(0.16)
fwdSonar.stop()
buoyDist, _ = fwdSonar.dist()
buoyDist = buoyDist/2000
log.info("buoyDist %d" % buoyDist)
rc.forward("time", 5, 0.32)
rc.close()
log.info('Done buoy test')
time.sleep(1)
log.flush()
print("Done Buoy Test")