*/
@Override
public void actuate(EvolvableBoxAgent3DEnvironment env,
EvolvableBoxAgent3D agent) {
// Get the RotationalLimitMotor for the appendix.
EvolvableBoxAgent3DAppendix appendix = agent.getAppendices().get(this.appendixId);
RotationalLimitMotor rlm = appendix.getConstraint().getRotationalLimitMotor(appendix.getFreeAxis());
//NEU
double valueFromBrain = agent.getActuatorWerte()[appendixId-1];
//normalizedAngle = valueFromBrain;
normalizedAngle = normalizedAngle * Math.PI;
normalizedAngle = normalizedAngle + valueFromBrain* Math.PI;
normalizedAngle = Math.sin(normalizedAngle);
//normalizedAngle += valueFromBrain*Math.PI;
//normalizedAngle = Math.sin(normalizedAngle);
//normalizedAngle += agent.getActuatorWerte()[appendixId-1];
//normalizedAngle = (Math.sin(normalizedAngle*10.) +1) / 2. ;
//normalizedAngle = Math.sin(normalizedAngle*Math.PI) ;
//normalizedAngle = Math.sin(normalizedAngle) ;
float oneDegree = (float) Math.toRadians(1);
if (normalizedAngle < -1 || normalizedAngle > 1) {
System.err.println("INVALID NORMALIZED ANGLE! " + normalizedAngle);
}
// Only angles from -85 to 85 degrees.
//float desiredAngle = (float) (-Math.toRadians(85) + Math.toRadians(2*85) * this.normalizedAngle);
float desiredAngle = (float) (Math.toRadians(85) * this.normalizedAngle);
//System.out.println("\ndesired angle: " + Math.toDegrees(desiredAngle));
float lastAngle = rlm.loLimit + oneDegree;
//if ( Math.abs (lastAngle-desiredAngle) < oneDegree/5f) {
//System.out.println("Change too small.");
//return;
//}
float angle = 0;
// Make max turning speed tick-independent using a clock!
long microSeconds = clock.getTimeMicroseconds();
clock.reset();
double seconds = microSeconds / 1000000.;
// Not more than 720 degrees per second for rotation.
double maximumPerSecond = 360000;
float maxAngleChange = (float) (seconds * maximumPerSecond);
//System.out.println("" + seconds + "..." + maxAngleChange);
if (Math.toDegrees(desiredAngle-lastAngle) > maxAngleChange) {
angle = lastAngle + (float) Math.toRadians(maxAngleChange);
while (angle > Math.toRadians(80)) {
angle = angle - oneDegree/10f;
}
//System.out.println("Change too big, only added " + maxAngleChange);
}
else if (Math.toDegrees(lastAngle-desiredAngle) > maxAngleChange) {
angle = lastAngle - (float) Math.toRadians(maxAngleChange);
while (angle < Math.toRadians(-80)) {
angle = angle + oneDegree/10f;
}
//System.out.println("Change too big, only subtracted " + maxAngleChange);
}
else {
angle = desiredAngle;
//System.out.println("Change ok.");
}
//angle = desiredAngle;
//System.out.println("last angle: " + Math.toDegrees(lastAngle) + " actual angle: " + Math.toDegrees(angle));
//if (appendix.getAgentCore().getPosition().y > 12) {
//System.out.println("DANGER!!!!!!!!!!!!!!");
//}
/*
if (Math.abs(appendix.getConstraint().getAngle(appendix.getFreeAxis())) > Math.toRadians (89.9)){
System.out.println("ANGLE TOO HIGH!!!!!!!!!!" + Math.toDegrees(appendix.getConstraint().getAngle(appendix.getFreeAxis())));
}*/
/*
System.out.println("lastangle: " + Math.toDegrees(lastAngle)
+ " desired angle: " + Math.toDegrees(desiredAngle)
+ " allowed angle: " + Math.toDegrees(angle)
+ " seconds: " + seconds);*/
//Set the actual rotational limits so that the servo-like steering is possible.
rlm.loLimit = angle - oneDegree;
rlm.hiLimit = angle + oneDegree;
/*
rlm.limitSoftness = 0.1f;
rlm.damping = 2f;
rlm.ERP = .1f;
rlm.maxLimitForce = 11;
rlm.maxMotorForce = 10;
*/
/*
rlm.bounce = 0;
rlm.damping = 10;
rlm.maxMotorForce = appendix.getMass();
rlm.maxLimitForce = appendix.getMass()*1.5f; */
//rlm.maxLimitForce = appendix.getMass() * 1.5f;
//rlm.ERP = .1f;
//appendix.getAgentCore().getRigidBody().activate();
// Activate the rigidBody so that the movement is actually performed and not missed
// because it is in sleeping state!
appendix.getRigidBody().activate();
rlm.enableMotor = true;
//System.out.println("Servo for appendix " + appendixId + " set angle to: " + normalizedAngle);
}