package demo.helicopter; import java.text.*; // for class DecimalFormat import java.util.*; // for class Vector import mil.navy.nps.dis.*; // for class EntityStatePdu import mil.navy.nps.testing.*; // for MulticastPduSender import mil.navy.nps.util.*; import mil.navy.nps.disEnumerations.*; //================================================================================ public class TurretActionInterpreter extends Thread { private float pduSendInterval, // time delay between pdus speed, // not velocity, no directional information torque; // current power setting private DecimalFormat df; // pretty print screen output private EntityStatePdu espdu; // Pdu to send entity state to VRML scene private FirePdu fpdu; // Pdu to send fire info to VRML scene private BehaviorStreamBuffer networkInterface; // tie in to the network private String ipAddress; private int portNumber; private TankParent Mom; private TankControlPanel myPanel; //============================================================================================= // Constructors & Start //============================================================================================= /* public TurretActionInterpreter() { speed = torque = 0.0f; // initially helo on ground with no fwd airspeed pduSendInterval = 5.25f; // approx 300 ms between pdu sends df = new DecimalFormat("###.00"); espdu = new EntityStatePdu(); // instantiate an espdu, used as a data structure fpdu = new FirePdu(); // instantiate a fire Pdu, used as data structure espdu.setEntityLocationX(0.001f); // set starting location into espdu, all other espdu.setEntityLocationY(0.001f); // initial quantities are set to zero when the espdu.setEntityLocationZ(0.001f); // espdu is constructed in the dis-java-vrml code // avoid 0 0 0 since it is trapped as a possible bug espdu.setEntityOrientationPsi(0f); // set starting heading, points towards tower short[] entityID = new short[3]; // create entity id triplet for espdu & fire pdu entityID[0] = 0; entityID[1] = 1; entityID[2] = 117; espdu.setEntityID(entityID[0], entityID[1], entityID[2]); // set entity id triplet into fpdu.setFiringEntityID(espdu.getEntityID()); // the espdu and fie pdu // instantiate a BehaviorStreamBuffer to handle the network interface, instantiate with // the multicast IP address and port EntityID temp = espdu.getEntityID(); espdu.setMarking("R71-630" + temp.getEntityID()); ipAddress = "224.2.181.145"; portNumber = 62040; networkInterface = new BehaviorStreamBuffer(ipAddress, portNumber); Thread pduReader = new Thread(networkInterface); pduReader.start(); } // end constructor */ public TurretActionInterpreter(String ipAddr, int portNum, short siteNum, short appNum, short id, TankControlPanel panel) { myPanel = panel; speed = torque = 0.0f; // initially helo on ground with no fwd airspeed pduSendInterval =0.5f; // approx 300 ms between pdu sends df = new DecimalFormat("###.00"); espdu = new EntityStatePdu(); // instantiate an espdu, used as a data structure fpdu = new FirePdu(); // instantiate a fire Pdu, used as data structure espdu.setEntityLocationX(0.01f); // set starting location into espdu, all other espdu.setEntityLocationY(0.01f); // initial quantities are set to zero when the espdu.setEntityLocationZ(0.01f); // espdu is constructed in the dis-java-vrml code espdu.setEntityOrientationPsi(0f); // set starting heading, points towards tower short[] entityID = new short[3]; // create entity id triplet for espdu & fire pdu entityID[0] = siteNum; entityID[1] = appNum; entityID[2] = id; espdu.setEntityID(entityID[0], entityID[1], entityID[2]); // set entity id triplet into fpdu.setFiringEntityID(espdu.getEntityID()); // the espdu and fie pdu // instantiate a BehaviorStreamBuffer to handle the network interface, instantiate with // the multicast IP address and port EntityID temp = espdu.getEntityID(); espdu.setMarking("R71-630" + temp.getEntityID()); ipAddress = ipAddr; portNumber = portNum; networkInterface = new BehaviorStreamBuffer(ipAddress, portNumber); Thread pduReader = new Thread(networkInterface); pduReader.start(); } // end constructor public void run() {startDriving();} // start sending espdus, called from TestFrame.java // calls driving which is an infinite espdu sending // loop //============================================================================================= // Properties //============================================================================================= /* // called by TestFrame.java whenever the airspeed scrollbar on the control panel is adjusted // This method calculates new X & Y linear velocities and sets the new values in the entity // state Pdu. The new values are then sent as part of the espdu the next time the // startFlying() method loops public void setSpeed(float spd) { System.out.println("entering setSpeed() w/arg.. " + spd); speed = .5144f * spd; // convert from knots to meters/second espdu.setEntityLinearVelocityX((float)(speed * Math.cos(espdu.getEntityOrientationPsi()))); espdu.setEntityLinearVelocityY((float)(speed * Math.sin(espdu.getEntityOrientationPsi()))); System.out.println("Setting speed to ... " + speed + " m/s"); } // end setSpeed() public double getSpeed() {return speed;} */ public void set_parent(TankParent Value){ Mom = Value;} public EntityStatePdu getEspdu() {return espdu;} //============================================================================================= // Utility Methods //============================================================================================= // this method loops indefinitely sending espdus every 300 ms or so. This loop runs on its // own thread public void startDriving(){ while(true){ // loops until mother thread exits thus killing this thread // update orientations espdu.setEntityOrientationPhi(espdu.getEntityOrientationPhi() + espdu.getEntityAngularVelocityX() * pduSendInterval); espdu.setEntityOrientationPsi(espdu.getEntityOrientationPsi() + espdu.getEntityAngularVelocityZ() * pduSendInterval); espdu.setEntityOrientationTheta(espdu.getEntityOrientationTheta() + espdu.getEntityAngularVelocityY() * pduSendInterval); // Dennis, this is new, I think you may be doing this already, if so add a call to // your method in place of this call // checkForGroundCollision(); System.out.print("\n\nPosition ==> (" + df.format(espdu.getEntityLocationX()) + ", " + df.format(espdu.getEntityLocationY()) + ", " + df.format(espdu.getEntityLocationZ()) + ")" + " Orientation ==> (" + df.format(espdu.getEntityOrientationPhi()) + ", " + df.format(espdu.getEntityOrientationTheta()) + ", " + df.format(espdu.getEntityOrientationPsi()) + ")" + "\nLin-Vel ==> (" + df.format(espdu.getEntityLinearVelocityX()) + ", " + df.format(espdu.getEntityLinearVelocityY()) + ", " + df.format(espdu.getEntityLinearVelocityZ()) + ")"); // send espdu to the specified multicast address espdu.makeTimestampCurrent(); networkInterface.sendPDU(espdu, ipAddress, portNumber); // collectFirePdus(); try{ Thread.sleep(450); // put thread to sleep for 250 ms, thus when // Thread.sleep(450); // time to complete the loop (hopefully about } // end try // 50ms) is added the pdu send interval will catch(InterruptedException ite){ // be approximately 300ms ite.printStackTrace(); } // end catch } // end while } // end startFlying() public void stopRun(){ networkInterface.setRunDone(true); } public String getFrameHeaderInfo(){ EntityID temp = espdu.getEntityID(); return new String("R71-630" + temp.getEntityID() + " (" + temp.getSiteID() + "/" + temp.getApplicationID() + "/" + temp.getEntityID() + ")"); } // end getFrameheaderInfo //============================================================================================= // Button Methods - These methods are all called by TestFrame.java //============================================================================================= public void reset(){ // return entity to starting position with all espdu.setEntityLocationX(0.01f); // state variables (other than location) zeroed espdu.setEntityLocationY(0.01f); // out espdu.setEntityLocationZ(0.01f); espdu.setEntityOrientationPhi(0.0f); espdu.setEntityOrientationPsi(0f); espdu.setEntityOrientationTheta(0.0f); espdu.setEntityLinearVelocityX(0.0f); espdu.setEntityLinearVelocityY(0.0f); espdu.setEntityLinearVelocityZ(0.0f); espdu.setEntityAngularVelocityX(0.0f); espdu.setEntityAngularVelocityY(0.0f); espdu.setEntityAngularVelocityZ(0.0f); speed = 0; } // end reset() public void sendFirePdu(){ // much more work needs to be done here (overall) fpdu.setLocationInWorldCoordinate(Mom.get_TankActionInterpreter().getEspdu().getEntityLocationX(), Mom.get_TankActionInterpreter().getEspdu().getEntityLocationY(), Mom.get_TankActionInterpreter().getEspdu().getEntityLocationZ()); // must include weapons initial velocity here fpdu.setVelocity((float)(Mom.get_TankActionInterpreter().getEspdu().getEntityLinearVelocityX() + 1000 * Math.cos((Mom.get_TankActionInterpreter().getEspdu().getEntityOrientationPsi() + espdu.getEntityOrientationPsi()))), (float)(Mom.get_TankActionInterpreter().getEspdu().getEntityLinearVelocityY() + 1000 * Math.sin((espdu.getEntityOrientationPsi() + Mom.get_TankActionInterpreter().getEspdu().getEntityOrientationPsi()))), Mom.get_TankActionInterpreter().getEspdu().getEntityLinearVelocityZ()); fpdu.makeTimestampCurrent(); fpdu.setRange(3000.0f); espdu.makeTimestampCurrent(); networkInterface.sendPDU(fpdu, ipAddress, portNumber); networkInterface.sendPDU(espdu, ipAddress, portNumber); } // end sendFirePdu() public void brake(){ // stabilize the helo at a hover at the espdu.setEntityOrientationPhi(0.0f); // current location espdu.setEntityLinearVelocityX(0.0f); espdu.setEntityLinearVelocityY(0.0f); espdu.setEntityLinearVelocityZ(0.0f); espdu.setEntityAngularVelocityX(0.0f); espdu.setEntityAngularVelocityY(0.0f); espdu.setEntityAngularVelocityZ(0.0f); speed = 0; } // end public hover() public void leftTurretTurn(){ espdu.setEntityAngularVelocityZ(-.1058f); } // end leftpedalTurn() public void rightTurretTurn(){ espdu.setEntityAngularVelocityZ(.1058f); } // end rightPedalTurn() } // end class MotionInterpreter