-
Notifications
You must be signed in to change notification settings - Fork 1
/
begindrive.txt
182 lines (165 loc) · 11.2 KB
/
begindrive.txt
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
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158
159
160
161
162
163
164
165
166
167
168
169
170
171
172
173
174
175
176
177
178
179
180
181
182
/////////////
// BeginDrive
// ----------
// sets some global variables needed by the various
// programs that will be called during the drive
/////////////////////////////////////////////////////
// stats tracking and data logging
set recentLogList to list(). // the last few entries into the rover log
set maxLogEntries to 14. // how many recent log items can fit in the output window
set printedLogs to maxLogEntries. // used to determine when the log display should refresh with new logs
set logLineStart to 17. // the first line at which logging is displayed
set driveTimeBegin to time:seconds. // when we've started our drive
set driveDistance to 0. // how far we have driven
set numAirtime to 0. // how many times our wheels left the ground
set amountAirTime to 0. // how many seconds spent in the air
set maxAirHeight to 0. // the highest we've gotten off the ground
set maxAltitude to altitude. // the highest we've traveled
set minAltitude to altitude. // the lowest we've traveled
set avgSpeed to list(). // leep track of our speed for an averaging
set lastSpeedCheck to time:seconds. // last time we polled our speed
set speedCheckInterval to 5. // how many seconds between speed checks
set resumeTravelTime to 10. // how many seconds to delay speed checks after reaching a waypoint (to get back up to normal speed)
// note this is in addition to check interval time.
set highSpeed to 0. // the fastest we've traveled
set maxUpslope to 0. // the steepest upslope encountered
set maxDownslope to 0. // the steepest downslope encountered
set avgSpeedETASample to 5. // how many speed checks we should sample to create an average to use to calculate our ETA
set detailLogging to false. // outputs extra vehicle behavior information to the log
// data regarding waypoints
set numWaypoints to 4. // must be at least 1
set currentWaypoint to 2. // can start at any number from 1 to numWaypoints
set waypointName to "waypoint". // whatever you have your waypoint vessels named
set travelDirection to 1. // set to -1 if you are starting from the last waypoint heading to the first
set autoReturn to true. // whether the rover should return to the first waypoint upon reaching the last
set waypointHold to false. // whether to stop at a waypoint or not (useful for performing science prior to continuing drive)
// variables to tweak in order to control the rover driving
set overSpeedDownSlopeBrakeTime to 0.3. // number of seconds to brake when we are traveling in downhill overspeed mode
set slopeAbortDelayLength to 10. // number of seconds to wait after hitting an upslope straight from a downslope before a slope abort event can be triggered
set extremeSlopeAngle to 8. // defines the angle at which coasting down would greatly break through overSpeed
set extraBrakeTime to 0.7. // how many seconds longer to stand on the brakes when traveling down an extreme slope
set slopeAbortSpeed to 2. // m/s of how slow the rover needs to be going up a slope to trigger the drive abort
set groundHeight to 0.6. // how high (m) your rover sits off the ground. Used to detect air time, should leave margin for movement
set cruiseSpeedBrakeTime to 1. // how many seconds to apply brakes when needed during cruise mode
set upSlopeAngle to 5. // the angle at which the rover needs to speed up from cruise speed to maintain forward movement
set cruiseSpeed to 0.6. // this is how fast the rover will travel when driving straight and level
set upSlopeSpeed to 0.8. // this is how fast the rover will travel when driving up a slope greater than upSlopeAngle
set overSpeedCruise to 8. // m/s of how fast not to go when traveling over level ground
set overSpeedDownSlope to 4. // m/s of how fast not to go when traveling down a slope after we've exceeded brakeOveruse
set underSpeed to 2. // m/s less than overspeed we should be traveling over level ground before resuming cruise speed
set approachDistance to 25. // meters from the waypoint to begin slowing down
set approachSpeedReverse to -0.3. // how fast we should slow down once we pass approachDistance (should be a negative number!)
set stopDistance to 10. // meters from the waypoint we should put on the brakes to bring the rover to a stop
set levelSlopeAngle to 3. // degrees at which the rover decides it is level enough for cruiseSpeed (upslope.downslope)
set slopeApproachMultiplierUp to 0.2. // how much to increase speed based on the slope of the final approach to the waypoint
set slopeApproachMultiplierDown to 0.1. // how much to decrease speed based on the slope of the initial approach to the waypoint
set driveMonitorWaitTime to 4. // number of seconds to wait after beginning/resuming a drive before allowing the monitor program to kick in
set autoWarp to true. // set 2x physical warp when driving, 1x when approaching/departing waypoint
set abortReverse to true. // on slope abort, back down to level ground if True. Just enable brakes if False
// environment variables. Don't touch!
set onFinalApproach to false. // we've stopped short and need to tweak the throttle to close the gap
set restartDrive to false. // temp fix for bug that doesn't make target data immediately available
set wheelsUp to false. // are we off the ground?
set yesterday to time:day. // today is yesterday... until tomorrow
set currentSlopeType to 0. // 0 = level, 1 = upslope, 2 = downslope
set currentSlopeAngle to 0. // the degree of up/down pitch the rover is currently experiencing
set brakesOn to false. // braking recently activated or not
set lastBrake to -1. // when to let off the brakes
set brakeUseCount to 0. // how many times we've stepped on the brakes in close succession
set onApproach to false. // are we almost there yet?
set abortDrive to false. // did we meet a slope we can't match?
set currentSpeed to 0. // for various drive functions, the speed used can be any of the ones defined below
set currentOverSpeed to overSpeedCruise. // how fast we shouldn't be going, currently
set slopeAbortDelay to 0. // used to delay a slope abort event
set currentBrakeTime to cruiseSpeedBrakeTime. // current amount of braking time in seconds
set warpmode to "PHYSICS". // make sure we always warp in physics mode
clearscreen.
abort off.
set recentLogList:add to "<" + time:calendar + ">".
log "<" + time:calendar + ">" to RoverLog.
set recentLogList:add to "<" + time:clock + "> program initialized".
log "<" + time:clock + "> program initialized" to RoverLog.
// time to head for the first waypoint!
set target to waypointName + currentWaypoint.
set recentLogList:add to "<" + time:clock + "> target " + waypointName + currentWaypoint + " selected".
log "<" + time:clock + "> target " + waypointName + currentWaypoint + " selected" to RoverLog.
// let the program gather data on the target we just acquizired
wait 0.001.
set recentLogList:add to "<" + time:clock + "> driving " + round(target:distance, 2) + "m to " + waypointName + currentWaypoint.
log "<" + time:clock + "> driving " + round(target:distance, 2) + "m to " + waypointName + currentWaypoint to RoverLog.
set driveDistance to driveDistance + target:distance.
brakes off.
lock wheelsteering to target.
set currentSpeed to cruiseSpeed.
lock wheelthrottle to currentspeed.
// only need to print the output screen once, in full, then we just change values
print "=================================================".
print "|---------------Rover Variables-----------------|".
print "| |".
print "| currentBrakeTime: 00.0 currentSpeed: 00.00 |".
print "| currentOverspeed: 00.0 currentSlopeType: 0 |".
print "| currentWaypoint: 0/0 brakeUseCount: 0 |".
print "| currentSlopeAngle: -00.00 abortDrive: False |".
print "| |".
print "|------------Environment Information------------|".
print "| |".
print "| Height over ground: 0.00m |".
print "| Surface speed: 0.00m/s |".
print "| Distance remaining to target: 0.00m |".
print "| ETA: 0h 0m |".
print "| |".
print "|--------------Recent Log Entries---------------|".
print "| |".
print "| |".
print "| |".
print "| |".
print "| |".
print "| |".
print "| |".
print "| |".
print "| |".
print "| |".
print "| |".
print "| |".
print "| |".
print "| |".
print "| |".
print "| |".
print "|===============================================|".
print "Rover Driver v1.0 (c)Drew Kerman".
// only need to print the log once
set index to 0.
until index = maxLogEntries or index = recentLogList:length {
print recentLogList[index] at (2, logLineStart + index).
set index to index + 1.
}.
// let's not immediately hop into monitoring mode but give the slope detection time to smooth out
set rollOut to time:seconds.
until time:seconds - rollOut > driveMonitorWaitTime {
set upvec to up:vector.
set velvec to ship:velocity:surface:normalized.
set dp to vdot(velvec,upvec).
set currentSlopeAngle to 90 - arccos(dp).
wait 0.001.
print " " at (20,3).
print currentBrakeTime at (20,3).
print " " at (42,3).
print round(currentSpeed, 2) at (42,3).
print " " at (20,4).
print currentOverSpeed at (20,4).
wait 0.001.
print currentSlopeType at (46,4).
print currentWaypoint + "/" + numWaypoints at (19,5).
print brakeUseCount at (43,5).
print " " at (21,6).
print round(currentSlopeAngle, 2) at (21,6).
print " " at (22,10).
print round(alt:radar - groundHeight, 2) + "m" at (22,10).
wait 0.001.
print " " at (17,11).
print round(surfacespeed, 2) + "m/s" at (17,11).
print " " at (32,12).
print round(target:distance, 2) + "m" at (32,12).
}.
if autoWarp = true { set warp to 1. }.
run MonitorDrive.