10 package jp.go.aist.hrp.joystick.rtc;
12 import org.eclipse.swt.graphics.Point;
15 import jp.
go.
aist.rtm.RTC.DataFlowComponentBase;
17 import jp.
go.
aist.rtm.RTC.port.OutPort;
18 import jp.
go.
aist.rtm.RTC.util.DataRef;
19 import RTC.ReturnCode_t;
20 import RTC.TimedFloatSeq;
52 m_pos_val =
new TimedFloatSeq(
new Time(0,0),
new float[2]);
55 m_vel_val =
new TimedFloatSeq(
new Time(0,0),
new float[2]);
66 registerOutPort(TimedFloatSeq.class,
"pos",
m_posOut);
67 registerOutPort(TimedFloatSeq.class,
"vel",
m_velOut);
68 }
catch (Exception e) {
80 return ReturnCode_t.RTC_OK;
94 System.out.println(
"Joystick Finalized");
95 return super.onFinalize();
111 System.out.println(
"Joystick Started");
112 return super.onStartup(ec_id);
128 System.out.println(
"Joystick Shutting down");
129 return super.onShutdown(ec_id);
145 System.out.println(
"Joystick Activated");
146 return super.onActivated(ec_id);
162 System.out.println(
"Joystick Deactivated");
163 return super.onDeactivated(ec_id);
181 float[] pos = {p.
x,p.y};
190 return ReturnCode_t.RTC_OK;
202 double _th = Math.atan2(p.y,p.x);
203 double _v = k * Math.hypot(p.x, p.y);
204 double _vl = _v * Math.cos(_th - (Math.PI/4.0));
205 double _vr = _v * Math.sin(_th - (Math.PI/4.0));
206 if(_vr==-0.0) _vr*=-1;
207 float[] v = {(float) _vl, (
float) _vr};
224 System.out.println(
"Joystick Aborted");
225 return super.onAborting(ec_id);
241 System.out.println(
"Joystick Error : "+ec_id);
242 return super.onError(ec_id);
258 System.out.println(
"Joystick Reset");
259 return super.onReset(ec_id);
275 return super.onStateUpdate(ec_id);
291 return super.onRateChanged(ec_id);
303 protected DataRef<TimedFloatSeq>
m_pos;
312 protected DataRef<TimedFloatSeq>
m_vel;