Tower.ks

// //note that the engines have to be tagged with W, A, S, D + 1 or 2 (see code) for identification. S for example is the engine that throttles up to pitch up.
//-------------------variables setup-------------------
// //rotational velocity PID setup
// pitch and yaw
set Kp1 to 0.045.
set Ki1 to 0.
set Kd1 to 0.
//roll
set Kp1r to 0.0018.
set Ki1r to 0.
set Kd1r to 0.

//maximum adjustment to engine throttle
set maxAdjust to 0.3.

//aggressiveness of engine throttle for pitch/yaw and roll
set PYTorqueFactor to 0.3.
set RTorqueFactor to 1.3.

// //horizontal guidance PID setup
set Kp2 to 0.05.
set Ki2 to 0.
set Kd2 to 0.
//max adjustment to steering vector
set maxCorrection to 0.5.

// //burn PID setup
//hovering
set Kp3 to 0.15.
set Ki3 to 0.15.
set Kd3 to 0.

//landing
set Kp4 to 0.3.
set Ki4 to 2.
set Kd4 to 0.

//aggressiveness (how much it cares to slow down)
set VertSpeedFactor to 0.5.
set VertSpeedDamping to 0.1.
set HorSpeedFactor to 2.

// //variables
set CPUheight to 1.1. 			//CPU height above ground when landed
set showarrows to FALSE.		//Show steering vectors
set maxHorSpeed to 10.
set maxVerSpeed to 2.

//-------------------program setup-------------------
if showarrows = true{set STEERINGMANAGER:SHOWFACINGVECTORS to true.}
lock speedx to SHIP:VELOCITY:SURFACE*VCRS(SHIP:UP:VECTOR,SHIP:NORTH:VECTOR).	//speed in longitudinal direction
lock speedy to SHIP:VELOCITY:SURFACE*SHIP:NORTH:VECTOR.							//speed in latitiudinal direction
set g to KERBIN:MU / KERBIN:RADIUS^2.
lock gforce to (SHIP:SENSORS:ACC - SHIP:SENSORS:GRAV):MAG/g.	
CLEARSCREEN.
// //differential throttle setup
LIST ENGINES IN AllEngines.
set AW1Engine to SHIP:PARTSDUBBED("AW1")[0].
set AW2Engine to SHIP:PARTSDUBBED("AW2")[0].
set WD1Engine to SHIP:PARTSDUBBED("WD1")[0].
set WD2Engine to SHIP:PARTSDUBBED("WD2")[0].
set SD1Engine to SHIP:PARTSDUBBED("SD1")[0].
set SD2Engine to SHIP:PARTSDUBBED("SD2")[0].
set AS1Engine to SHIP:PARTSDUBBED("AS1")[0].
set AS2Engine to SHIP:PARTSDUBBED("AS2")[0].

set PitchVelControlPID to PIDLOOP(Kp1,Ki1,Kd1,-0.5,0.5).		//those take the error in orientation from the steering manager and output an angular velocity setpoint			
set YawVelControlPID to PIDLOOP(Kp1,Ki1,Kd1,-0.5,0.5).
set RollVelControlPID to PIDLOOP(Kp1r,Ki1r,Kd1r,-0.3,0.3).
set PitchVelControlPID:SETPOINT to 0.
set YawVelControlPID:SETPOINT to 0.
set RollVelControlPID:SETPOINT to 0.

set PitchTorqueControlPID to PIDLOOP(PYTorqueFactor,0,0,-maxAdjust,maxAdjust).		//take the error in angular velocity and adjust the engines
set YawTorqueControlPID to PIDLOOP(PYTorqueFactor,0,0,-maxAdjust,maxAdjust).
set RollTorqueControlPID to PIDLOOP(RTorqueFactor,0,0,-maxAdjust,maxAdjust).

// //vertical guidance
set BurnPID to PIDLOOP(Kp3, Ki3, Kd3,0.1,1).
lock midval to 0.8*THROTTLE.					//middle value of thrust limiter
set VertSpeedPID to PIDLOOP(VertSpeedFactor,0,VertSpeedDamping,-0.5,maxVerSpeed).
lock BurnHeight to (SHIP:AIRSPEED^2/(2*(0.5*SHIP:MAXTHRUST/SHIP:MASS-g))).
lock BurnAccSpeed to (((SHIP:AIRSPEED^2)/(2*(ALT:RADAR)))/g+1). //necessary acceleration for suicide burn with constant g force
set thrott to 0.
set BurnSP to 0.
set BurnInput to 0.

//PID setup for vertical guidance by altitude setpoint (alt1)
function BurnPIDlanding
{
lock BurnSP to BurnAccSpeed.
lock BurnInput to gforce.
BurnPID:RESET().
set BurnPID:KP to Kp4.
set BurnPID:KI to Ki4.
set BurnPID:KD to Kd4.
}

//PID setup for landing burn with constant acceleration
function BurnPIDhovering
{
lock BurnSP to VertSpeedSP.
lock BurnInput to SHIP:VERTICALSPEED.
BurnPID:RESET().
set BurnPID:KP to Kp3.
set BurnPID:KI to Ki3.
set BurnPID:KD to Kd3.
}

function activateEngines{	
	for eng IN AllEngines {
	eng:activate.
	}.
}

function shutdownEngines{	
	for eng IN AllEngines {
	eng:shutdown.
	}.
}

// //steering setup
set xGuidancePID to PIDLOOP(Kp2, Ki2, Kd2,-maxCorrection,maxCorrection).
set yGuidancePID to PIDLOOP(Kp2, Ki2, Kd2,-maxCorrection,maxCorrection).
set xcorrection to 0.
set ycorrection to 0.

// //horizontal guidance
lock EastVec to VCRS(SHIP:UP:VECTOR,SHIP:NORTH:VECTOR).
lock NorthVec to SHIP:NORTH:VECTOR.

set DefaultUpVec to NorthVec.
set UpVec to DefaultUpVec.

//initial position
set lon0 to SHIP:LONGITUDE.	
set lat0 to SHIP:LATITUDE.
set alt0 to SHIP:ALTITUDE.
//seek position default
set lon1 to lon0.
set lat1 to lat0.
set alt1 to alt0.
lock LonError to SHIP:LONGITUDE - lon1.
lock LatError to SHIP:LATITUDE - lat1.
lock PosError to abs(LonError) + abs(LatError).
//speed control: take the error in lat/lon and output a speed setpoint
set xSpeedPID to PIDLOOP(HorSpeedFactor,0,0,-maxHorSpeed,maxHorSpeed).
set ySpeedPID to PIDLOOP(HorSpeedFactor,0,0,-maxHorSpeed,maxHorSpeed).
set xSpeedPID:SETPOINT to 0.
set ySpeedPID:SETPOINT to 0.



// //loop: refreshes all variables
function Loop{

	// //attitude control
	set pitchVelSP to PitchVelControlPID:UPDATE(TIME:SECONDS,STEERINGMANAGER:PITCHERROR)/(0.8*THROTTLE+0.0001).
	set yawVelSP to YawVelControlPID:UPDATE(TIME:SECONDS,STEERINGMANAGER:YAWERROR)/(0.8*THROTTLE+0.0001).
	set rollVelSP to RollVelControlPID:UPDATE(TIME:SECONDS,STEERINGMANAGER:ROLLERROR)/(0.8*THROTTLE+0.0001).
	set PitchTorqueControlPID:SETPOINT to pitchVelSP.
	set YawTorqueControlPID:SETPOINT to yawVelSP.
	set RollTorqueControlPID:SETPOINT to rollVelSP.
	set pitchAdjust to PitchTorqueControlPID:UPDATE(TIME:SECONDS,(SHIP:ANGULARVEL*SHIP:FACING:STARVECTOR)).
	set yawAdjust to YawTorqueControlPID:UPDATE(TIME:SECONDS,-(SHIP:ANGULARVEL*SHIP:FACING:UPVECTOR)).
	set rollAdjust to RollTorqueControlPID:UPDATE(TIME:SECONDS,(SHIP:ANGULARVEL*SHIP:FACING:FOREVECTOR)).

	// //burn PID
	set VertSpeedPID:SETPOINT to alt1.
	set VertSpeedSP to VertSpeedPID:UPDATE(TIME:SECONDS,SHIP:ALTITUDE).
	set BurnPID:SETPOINT to BurnSP.
	set thrott to BurnPID:UPDATE(TIME:SECONDS,BurnInput).
	
	//apply all adjustments
	set AW1Engine:THRUSTLIMIT to (midval + pitchAdjust + yawAdjust+rollAdjust)*100.
	set AW2Engine:THRUSTLIMIT to (midval + pitchAdjust + yawAdjust-rollAdjust)*100.
	set WD1Engine:THRUSTLIMIT to (midval + pitchAdjust - yawAdjust+rollAdjust)*100.
	set WD2Engine:THRUSTLIMIT to (midval + pitchAdjust - yawAdjust-rollAdjust)*100.
	set SD1Engine:THRUSTLIMIT to (midval - pitchAdjust - yawAdjust+rollAdjust)*100.
	set SD2Engine:THRUSTLIMIT to (midval - pitchAdjust - yawAdjust-rollAdjust)*100.
	set AS1Engine:THRUSTLIMIT to (midval - pitchAdjust + yawAdjust+rollAdjust)*100.
	set AS2Engine:THRUSTLIMIT to (midval - pitchAdjust + yawAdjust-rollAdjust)*100.
	
	// //update guidance vector
	set speedxSP to xSpeedPID:UPDATE(TIME:SECONDS,LonError*1000).
	set speedySP to ySpeedPID:UPDATE(TIME:SECONDS,LatError*1000).
	set xGuidancePID:SETPOINT to speedxSP.
	set yGuidancePID:SETPOINT to speedySP.
	set xcorrection to  xGuidancePID:UPDATE(TIME:SECONDS,speedx).
	set ycorrection to  yGuidancePID:UPDATE(TIME:SECONDS,speedy).
	
}

lock STEERING to LOOKDIRUP( SHIP:UP:VECTOR + xcorrection*EastVec + ycorrection*NorthVec , UpVec).

//-------------------sequence of events-------------------lexicon
//	the script needs to have a lat/lon setpoint (with lat1/lon1)
//	and an altitude setpoint (alt1)
//	it also needs to know whether you want to hover (BurnPIDhovering) or land (BurnPIDlanding)
//	activateEngines() and shutdownEngines()
//	for questions: /u/EmbersArc on reddit or EmbersArc on the KSP forums

SAS OFF.
RCS ON.

set t0 to TIME:SECONDS.
Loop().
lock THROTTLE to thrott.
SET instr TO READJSON("instructions.json").
set FlightPhase to 41.
set ReferenceShip to Vessel("Reference").
set latRef to ReferenceShip:LATITUDE.
set lonRef to ReferenceShip:LONGITUDE.
set unit to 1/(2*(KERBIN:RADIUS+ReferenceShip:ALTITUDE)*CONSTANT:PI)*360. //1 meter in lat/lon
set level to 2.
set cube to 1.


//start here

//ag1 release grapple
//ag2 toggle arm

	BurnPIDhovering().
	activateEngines().
	toggle ag2.

until level = 4 {	

	if MOD(level,2) = 0 
	{set angle to vang(ReferenceShip:FACING:UPVECTOR,EastVec) + 22.5.}
	else 
	{set angle to vang(ReferenceShip:FACING:UPVECTOR,EastVec).}

	until cube = 9
	{
			//move to cube 
			set tar to Vessel("Cube Probe").
			set tar:SHIPNAME to "Cube " + level + cube.
			lock UpVec 	to tar:FACING:STARVECTOR.
			lock lat1 	to tar:LATITUDE.
			lock lon1 	to tar:LONGITUDE.
			set alt1 to	ReferenceShip:ALTITUDE + 18 + 6*unit*level.
			print "Moving to cube " + level + "-" + cube.
			
		until PosError < 0.00004 and SHIP:AIRSPEED < 0.1 {Loop().}
		
			//lower down
			print "Lowering down".
			set xSpeedPID:Ki to 0.1. //start integrating for precision
			set ySpeedPID:Ki to 0.1.
			lock lat1 to tar:LATITUDE.
			lock lon1 to tar:LONGITUDE.
			set alt1 to tar:ALTITUDE.
		
		until SHIP:STATUS = "LANDED" {Loop().}
			
			set xSpeedPID:Ki to 0.	
			xSpeedPID:RESET().	
			set ySpeedPID:Ki to 0.
			ySpeedPID:RESET().	
			set alt1 to ReferenceShip:ALTITUDE + 10.
			print "Lifting".
			
		until abs(alt1-SHIP:ALTITUDE) < 2 {Loop().}
		
			//move to drop off location
			set UpVec to NorthVec*cos(angle) - EastVec*sin(angle).
			set alt1 to ReferenceShip:ALTITUDE + 15 + 6*unit*level.
			lock lon1 to lonRef + 12.08*unit*cos(angle).
			lock lat1 to latRef + 12.08*unit*sin(angle).
			print "Move to drop off location".
		
		until PosError < 0.00003 and SHIP:AIRSPEED < 0.05 {Loop().}
		
			//lower down
			set xSpeedPID:Ki to 0.1.
			set ySpeedPID:Ki to 0.1.
			set alt1 to ReferenceShip:ALTITUDE + 6*unit*level.
			print "Lowering down".
		
		until SHIP:ELEMENTS:LENGTH > 1 {Loop().}
			
			toggle ag1.
			set xSpeedPID:Ki to 0.
			xSpeedPID:RESET().	
			set ySpeedPID:Ki to 0.
			ySpeedPID:RESET().	
			
		print "Element " + level + "-" + cube + " placed successfully".
		print "-------".
			
			set angle to angle + 45.
			set cube to cube + 1.
	}
	
	set level to level + 1.
	print "starting level " + level.

	set cube to 1.

}
	
		//done and landing
		print "done".
		set VertSpeedPID:MINOUTPUT to -5.
		lock lon1 to ReferenceShip:LONGITUDE + 15*unit.
		lock lat1 to ReferenceShip:LATITUDE + 15*unit.
		lock alt1 to 100.
		
	until PosError < 1*1e-4 and SHIP:AIRSPEED < 0.1 {Loop().}
	
		lock alt1 TO 65.
		
	until SHIP:STATUS = "LANDED" {Loop().}	
	
	shutdownEngines().