Katapult/launch.groovy at main · OperationSmallKat/Katapult · GitHub

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
import com.neuronrobotics.sdk.addons.kinematics.DHParameterKinematics
import com.neuronrobotics.sdk.addons.kinematics.MobileBase
import com.neuronrobotics.sdk.addons.kinematics.math.RotationNR
import com.neuronrobotics.sdk.addons.kinematics.math.TransformNR
import com.neuronrobotics.sdk.common.DeviceManager
import com.neuronrobotics.sdk.util.ThreadUtil
import com.neuronrobotics.bowlerstudio.BowlerStudio
import com.neuronrobotics.bowlerstudio.assets.ConfigurationDatabase
import com.neuronrobotics.bowlerstudio.scripting.ScriptingEngine
import com.neuronrobotics.sdk.addons.gamepad.BowlerJInputDevice;
import com.neuronrobotics.sdk.addons.gamepad.IGameControlEvent

//ScriptingEngine.pull("https://github.com/CommonWealthRobotics/DeviceProviders.git");
//ScriptingEngine.gitScriptRun("https://github.com/CommonWealthRobotics/DeviceProviders.git",
//		"loadAll.groovy", null);

String robotName = ConfigurationDatabase.getObject("katapult", "robotName", "Luna")
String robotGit = ConfigurationDatabase.getObject("katapult", "robotGit", "https://github.com/OperationSmallKat/Luna.git")
String robotGitFile = ConfigurationDatabase.getObject("katapult", "robotGitFile", "MediumKat.xml")
String linkDeviceName = ConfigurationDatabase.getObject("katapult", "linkDeviceName", "midnight")
def gameControllerNames = ConfigurationDatabase.getObject("katapult", "gameControllerNames", [
	"Dragon",
	"X-Box",
	"Game",
	"XBOX",
	"Microsoft",
	"GPD"
])
println "Loading robot "+robotName+" "+robotGit+" "+robotGitFile+" "+linkDeviceName
MobileBase cat =DeviceManager.getSpecificDevice(robotName, {
	return ScriptingEngine.gitScriptRun(	robotGit,
			robotGitFile,null);
})
def device = DeviceManager.getSpecificDevice(linkDeviceName)
try{
	if(device.simple.isVirtual()) {
		println "SmallKat Device is virtual"
		//return;
	}
}catch(Throwable t){
	// not all devices have this class structure and will except on this test
}
BowlerJInputDevice g
List<String> alreadyConnected = DeviceManager.listConnectedDevice(BowlerJInputDevice.class)
if(alreadyConnected.size()==0) {
	try {
		//Check if the device already exists in the device Manager
		g=DeviceManager.getSpecificDevice("gamepad",{
			def t = new BowlerJInputDevice(gameControllerNames); //
			t.connect(); // Connect to it.
			return t
		})
	}catch(Throwable t) {
		println "No game controllers found, Searched:\n"+gameControllerNames
		return;
	}
}else {
	def name =alreadyConnected.get(0)
	g=DeviceManager.getSpecificDevice(name)
	if(!gameControllerNames.contains(name)) {
		def newList=[]
		newList.addAll(gameControllerNames)
		newList.add(name)
		ConfigurationDatabase.setObject("katapult", "gameControllerNames", newList)
		ConfigurationDatabase.save();
	}
}


def x =0;

def straif=0;
def rz=0;
def ljud =0;
def walkMode = true
def startPose
def tips
long timeOfLast = System.currentTimeMillis()

IGameControlEvent listener = new IGameControlEvent() {
			@Override public void onEvent(String name,float value) {
				timeOfLast = System.currentTimeMillis()

				if(name.contentEquals("l-joy-left-right")){
					straif=-value;
				}
				else if(name.contentEquals("r-joy-up-down")){
					x=-value;
				}
				else if(name.contentEquals("l-joy-up-down")){
					ljud=value;
				}
				else if(name.contentEquals("r-joy-left-right")){
					rz=-value;
				}
				else if(name.contentEquals("x-mode")){
					if(value>0) {
						if(!walkMode) {
							cat.pose(new TransformNR(),cat.getIMUFromCentroid(),tips)
						}
						walkMode=true;
					}
				}
				else if(name.contentEquals("y-mode")){
					if(value>0) {
						walkMode=false;
						startPose = cat.getFiducialToGlobalTransform()

						tips = cat.getTipLocations()
					}
				}
				//	System.out.println(name+" is value= "+value);

			}
		}
g.clearListeners()
// gamepad is a BowlerJInputDevice
g.addListeners(listener);
// wait while the application is not stopped
try{
	while(!Thread.interrupted() && cat.isAvailable()){
		ThreadUtil.wait(30)
		if(walkMode) {
			if(Math.abs(x)>0.001 || Math.abs(straif)>0.001 || Math.abs(rz)>0.001 || Math.abs(ljud)>0.001) {

				def newPose = new TransformNR(x*0.4,straif*0.2,0,new RotationNR(0, rz*0.2, 0))
				//println newPose
				cat.DriveArc(newPose, 0.0020);

			}
		}else {
			// pose mode
			def newPose = new TransformNR(0,-10*rz,10*x,new RotationNR(0,10*straif,5*ljud))
			def around = cat.getIMUFromCentroid()
			cat.pose(newPose,around,tips)
		}
	}
}catch(Throwable t){
	t.printStackTrace()
}
//remove listener and exit
g.removeListeners(listener);