-
Notifications
You must be signed in to change notification settings - Fork 1
/
dock.ks
73 lines (68 loc) · 2.9 KB
/
dock.ks
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
{
local dock is lex(
"translate", translate@,
"get_port", get_port@,
"approach", approach@
).
function init_pids {
parameter s is 5.
set pid_fore to pidloop(5,0.5,5,-s,s).
set pid_top to pidloop(5,0.5,5,-s,s).
set pid_star to pidloop(5,0.5,5,-s,s).
}
function translate {
parameter vector.
if vector:mag > 1 set vector to vector:normalized.
set ship:control:starboard to vector * ship:facing:starvector.
set ship:control:fore to vector * ship:facing:forevector.
set ship:control:top to vector * ship:facing:topvector.
}
function get_port {
parameter trgtv, type is "any".
local dports to list().
for port in trgtv:dockingports {
if port:state = "ready" and ( type = "any" or port:name = type ) dports:add(port).
}
local closestp to dports[0].
for port in dports { if port:nodeposition:mag < closestp:nodeposition:mag set closestp to port. }
return closestp.
}
function approach {
parameter tp, dp, distance is 150, speed is 1.
init_pids(speed).
rcs off. sas off.
dp:controlfrom().
lock steering to lookdirup(-tp:portfacing:vector, tp:portfacing:upvector).
wait until vang(dp:facing:vector, -tp:portfacing:vector) < 1.
lock dist_off to tp:portfacing:vector * distance.
lock app_v to tp:nodeposition - dp:nodeposition + dist_off.
lock rel_v to ship:velocity:orbit - tp:ship:velocity:orbit.
lock star_error to -1*vdot(dp:facing:starvector,app_v).
lock top_error to -1*vdot(dp:facing:topvector,app_v).
lock fore_error to -1*vdot(dp:facing:vector,app_v).
rcs on.
until false {
lock steering to lookdirup(-tp:portfacing:vector, tp:portfacing:upvector).
set vd_trgt_p_facing to vecdraw(tp:position,(tp:portfacing:vector)*20,white,"target port",1,true,0.2).
set vd_app_vec to vecdraw(dp:nodeposition,app_v,magenta,"target spot",1,true,0.2).
set vd_fore to vecdraw(dp:position,dp:facing:vector*10,red,"fore",1.0,true,0.2).
set vd_top to vecdraw(dp:position,dp:facing:topvector*10,blue,"top",1.0,true,0.2).
set vd_star to vecdraw(dp:position,dp:facing:starvector*10,green,"star",1.0,true,0.2).
set ship:control:fore to pid_fore:update(time:seconds,fore_error).
set ship:control:top to pid_top:update(time:seconds,top_error).
set ship:control:starboard to pid_star:update(time:seconds,star_error).
print "sf: " + rp(pid_speed_fore:setpoint) + " st: " + rp(pid_speed_top:setpoint) + " ss: " + rp(pid_speed_star:setpoint) +
" cf: " + rp(ship:control:fore) + " ct: " + rp(ship:control:top) + " cs: "+ rp(ship:control:starboard).
if app_v:mag < 0.1 break.
wait 0.1.
}
until rel_v:mag < 0.1 {
translate(-1 * rel_v).
}
translate(v(0,0,0)).
rcs off.
clearvecdraws().
}
function rp { parameter i, rnd is 3, pr is 6. return round(i,rnd):tostring:padright(pr). }
export(dock).
}