Seek.ks

//KOS
//This is a Kerbal kOS and Infernal Robotics Integration tool by Peter Goddard.
//this script accepts variables for setting groups of robotic segments.
// an example command is seek(0,0) which says move the leg root rotatrons to 0 degrees.
// seek(0 degrees, main rotatron group)
// 0 {SET refmod to ROTMODS1 > Main Rotatrons
// 1 {SET refmod to FOLDMODS1 > First leg segment
// 2 {SET refmod to FOLDMODS2 > Second Leg segment
// 3 {SET refmod to FOLDMODS3 > Third Leg segment
// 4 {SET refmod to ROTMODS2 > Front Main Rotatrons
// 5 {SET refmod to ROTMODS3 > Rear Main Rotatrons
//needless to say there are limits to be observed in setting rotations.
//I will try and code the min/max later maybe.

//A few groups have been set out, but more are needed for complex scripts.
//
clearscreen.
declare parameter angle.
declare parameter sel.
set throttle to 0.
//if ag2 = false{ toggle ag2.}
print "DEPLOY IN PROGRESS".

	//FRont Left Leg names
SET FLext TO SHIP:PARTSTAGGED("FL Extendatron")[0].
SET FLr TO SHIP:PARTSTAGGED("FL Rotatron")[0].
SET FLf1 TO SHIP:PARTSTAGGED("FL Foldatron 1")[0].
SET FLf2 TO SHIP:PARTSTAGGED("FL Foldatron 2")[0].
SET FLf3 TO SHIP:PARTSTAGGED("FL Foldatron 3")[0].

//FRont Right Leg names
SET FRext TO SHIP:PARTSTAGGED("FR Extendatron")[0].
SET FRr TO SHIP:PARTSTAGGED("FR Rotatron")[0].
SET FRf1 TO SHIP:PARTSTAGGED("FR Foldatron 1")[0].
SET FRf2 TO SHIP:PARTSTAGGED("FR Foldatron 2")[0].
SET FRf3 TO SHIP:PARTSTAGGED("FR Foldatron 3")[0].

//Rear Left leg names
SET RLext TO SHIP:PARTSTAGGED("RL Extendatron")[0].
SET RLr TO SHIP:PARTSTAGGED("RL Rotatron")[0].
SET RLf1 TO SHIP:PARTSTAGGED("RL Foldatron 1")[0].
SET RLf2 TO SHIP:PARTSTAGGED("RL Foldatron 2")[0].
SET RLf3 TO SHIP:PARTSTAGGED("RL Foldatron 3")[0].

///Rear Right leg names
SET RRext TO SHIP:PARTSTAGGED("RR Extendatron")[0].
SET RRr TO SHIP:PARTSTAGGED("RR Rotatron")[0].
SET RRf1 TO SHIP:PARTSTAGGED("RR Foldatron 1")[0].
SET RRf2 TO SHIP:PARTSTAGGED("RR Foldatron 2")[0].
SET RRf3 TO SHIP:PARTSTAGGED("RR Foldatron 3")[0].

//Mid Left leg names
SET MLext TO SHIP:PARTSTAGGED("ML Extendatron")[0].
SET MLr TO SHIP:PARTSTAGGED("ML Rotatron")[0].
SET MLf1 TO SHIP:PARTSTAGGED("ML Foldatron 1")[0].
SET MLf2 TO SHIP:PARTSTAGGED("ML Foldatron 2")[0].
SET MLf3 TO SHIP:PARTSTAGGED("ML Foldatron 3")[0].

//Mid Right leg names
SET MRext TO SHIP:PARTSTAGGED("MR Extendatron")[0].
SET MRr TO SHIP:PARTSTAGGED("MR Rotatron")[0].
SET MRf1 TO SHIP:PARTSTAGGED("MR Foldatron 1")[0].
SET MRf2 TO SHIP:PARTSTAGGED("MR Foldatron 2")[0].
SET MRf3 TO SHIP:PARTSTAGGED("MR Foldatron 3")[0].

//Create group lists for parts to act together
//extgroup1 is the list of ALL the extendatrons.
SET extgroup1 TO list().
extgroup1:ADD(FRext).
extgroup1:ADD(FLext).
extgroup1:ADD(RRext).
extgroup1:ADD(RLext).
extgroup1:ADD(MRext).
extgroup1:ADD(MLext).
// A corresponding list of all the mumechtoggle's attached to those parts:
SET EXTMODS1 TO LIST().
FOR L IN extgroup1 {EXTMODS1:ADD(L:GETMODULE("MUMECHTOGGLE")). }.

//extgroup2 is the list of the extendatrons at the 4 corners.
SET extgroup2 TO list().
extgroup2:ADD(FRext).
extgroup2:ADD(FLext).
extgroup2:ADD(RRext).
extgroup2:ADD(RLext).
// A corresponding list of all the mumechtoggle's attached to those parts:
SET EXTMODS2 TO LIST().
FOR L IN extgroup2 {EXTMODS2:ADD(L:GETMODULE("MUMECHTOGGLE")). }.

//extgroup3 is the list of the extendatrons at the middle sides
SET extgroup3 TO list().
extgroup3:ADD(MRext).
extgroup3:ADD(MLext).
// A corresponding list of all the mumechtoggle's attached to those parts:
SET EXTMODS3 TO LIST().
FOR L IN extgroup3 {EXTMODS3:ADD(L:GETMODULE("MUMECHTOGGLE")). }.

//rotgroup1 is the list of ALL base rotatrons.
SET rotgroup1 TO list().
rotgroup1:ADD(FLr).
rotgroup1:ADD(FRr).
rotgroup1:ADD(MLr).
rotgroup1:ADD(MRr).
rotgroup1:ADD(RRr).
rotgroup1:ADD(RLr).
// A corresponding list of all the mumechtoggle's attached to those parts:
SET ROTMODS1 TO LIST().
FOR L IN rotgroup1 {ROTMODS1:ADD(L:GETMODULE("MUMECHTOGGLE")). }.

//rotgroup2 is the list of the rotatrons at the front corners.
SET rotgroup2 TO list().
rotgroup2:ADD(FLr).
rotgroup2:ADD(FRr).
// A corresponding list of all the mumechtoggle's attached to those parts:
SET ROTMODS2 TO LIST().
FOR L IN rotgroup2 {ROTMODS2:ADD(L:GETMODULE("MUMECHTOGGLE")). }.

//rotgroup3 is the list of the rotatrons at the rear corners.
SET rotgroup3 TO list().
rotgroup3:ADD(RLr).
rotgroup3:ADD(RRr).
// A corresponding list of all the mumechtoggle's attached to those parts:
SET ROTMODS3 TO LIST().
FOR L IN rotgroup3 {ROTMODS3:ADD(L:GETMODULE("MUMECHTOGGLE")). }.


//foldgroup1 is the list of ALL joint 1 foldatrons.
SET foldgroup1 TO list().
foldgroup1:ADD(FLf1).
foldgroup1:ADD(FRf1).
foldgroup1:ADD(MLf1).
foldgroup1:ADD(MRf1).
foldgroup1:ADD(RLf1).
foldgroup1:ADD(RRf1).
// A corresponding list of all the mumechtoggle's attached to those parts:
SET foldmods1 TO LIST().
FOR L IN foldgroup1 {foldmods1:ADD(L:GETMODULE("MUMECHTOGGLE")). }.


//foldgroup2 is the list of ALL joint 2 foldatrons.
SET foldgroup2 TO list().
foldgroup2:ADD(FLf2).
foldgroup2:ADD(FRf2).
foldgroup2:ADD(MLf2).
foldgroup2:ADD(MRf2).
foldgroup2:ADD(RLf2).
foldgroup2:ADD(RRf2).
// A corresponding list of all the mumechtoggle's attached to those parts:
SET FOLDMODS2 TO LIST().
FOR L IN foldgroup2 {FOLDMODS2:ADD(L:GETMODULE("MUMECHTOGGLE")). }.

//foldgroup3 is the list of ALL joint 3 foldatrons.
SET foldgroup3 TO list().
foldgroup3:ADD(FLf3).
foldgroup3:ADD(FRf3).
foldgroup3:ADD(MLf3).
foldgroup3:ADD(MRf3).
foldgroup3:ADD(RLf3).
foldgroup3:ADD(RRf3).
// A corresponding list of all the mumechtoggle's attached to those parts:
SET FOLDMODS3 TO LIST().
FOR L IN foldgroup3{FOLDMODS3:ADD(L:GETMODULE("MUMECHTOGGLE")). }.

///set variables according to command line RUN SEEK(angle in degrees,servo#)////
If sel = 0 {SET refmod to ROTMODS1.} ELSE IF sel = 1 {SET refmod to FOLDMODS1.} ELSE IF sel = 2 {SET refmod to FOLDMODS2.}ELSE IF sel = 3 {SET refmod to FOLDMODS3.} ELSE IF sel = 4 {SET refmod to ROTMODS2.} ELSE IF sel = 5 {SET refmod to ROTMODS3.}
SET x to 0.
SET TEST to 0.
SET BYE to 0.
UNTIL x =1{
	SET TIMECHECK TO TIME:SECONDS.
  	WAIT UNTIL TIME:SECONDS > TIMECHECK. // Force a physics tick to pass here.
	SET INDEX to 0.
	UNTIL INDEX >= refmod:LENGTH{ //loop for the number of items in the list

		SET RMOD TO refmod[INDEX].//get the current item from the list
		//gather some rotation angle information
		SET CURRENTPOS TO RMOD:GETFIELD("rotation:").
		SET DELTA TO round(ABS(CURRENTPOS - ANGLE ),2).//difference between where I am and where I want to be.

    		// ramp up and ramp down speed for realism and accuracy setting the delta mutiplier to a bigger number shortens the curve. 2 or 3, not 500.
		RMOD:SETFIELD("fine speed", -0.08).
		RMOD:SETFIELD("coarse speed", MIN(3,DELTA*.15)).
    //Stopping accuracy is a tradeoff. Parts oscillate when they come to a stop and this increses with speed. In the following line, changing the DELTA <=X.XX will change the accuracy of your angles. The number is in degrees. For some actions the speed of the routine is greatly improved by giving a larger number (0.5 to 1.5) Smaller numbers give it the jitters as it bounces back and forth, and it takes forever to succeed. Remember you are confirming up to 6 leg positions at once.
    IF DELTA <=0.05 {//if it reaches seekpoint then stop
      	RMOD:DOACTION("MOVE -",false).
      	RMOD:DOACTION("MOVE +",false).
	SET test to 1.//adds a token to the stack saying "I am here"
         }
    	ELSE IF CURRENTPOS < ANGLE {//move to seek angle +
      	RMOD:DOACTION("MOVE -",false).
      	RMOD:DOACTION("MOVE +",true).
         }
    	ELSE IF CURRENTPOS > ANGLE {//move to seek angle -
      	RMOD:DOACTION("MOVE -",true).
      	RMOD:DOACTION("MOVE +",false).
         }
   	ELSE {//if nothing triggers do nothing(probably not necessary)
      	RMOD:DOACTION("MOVE -",false).
      	RMOD:DOACTION("MOVE +",false).
         }.
	IF test = 1 {set bye to bye+1.  // count up if token set
   	RMOD:DOACTION("MOVE -",false).
     	RMOD:DOACTION("MOVE +",false).
	}
	IF bye = 6 set x to x+1.//if the tokens ever add up to the value if INDEX then all the jpoints have reached the seekpoint.

//remove comment for debug
//print delta"   "+bye+"  "+test.

SET INDEX TO INDEX + 1.
				      }

	   }
//wanted
//////seeking routine for leg proportional movement//////////
////adapt self leveling routine
///add logic for extendatrons... should be easy
//create a splash screen with instructions
//create more movement routines