private boolean hitTheWall(Pose pose) {
float poseAngle = (float) Math.toRadians(Const.angleToByte(pose.getHeading()) * 90);
byte sin = (byte) Math.round(Math.sin(poseAngle)), cos = (byte) Math.round(Math.cos(poseAngle));
byte mapPoseX = (byte) (Const.roundToMapArea(Const.middlePointToBlock(pose.getX()), "X") + cos),
mapPoseY = (byte) (Const.roundToMapArea(Const.middlePointToBlock(pose.getY()), "Y") + sin);
// return true and update pose if it hit an obstacle
if (mapPoseX < 0
|| mapPoseY < 0
|| mapPoseX > 8
|| mapPoseY > 5
|| wayClass.getOccupancyMap(mapPoseX, mapPoseY) > 0) {
Rover.poseProvider.setPose(
new Pose(
cos != 0
? (mapPoseX * Const.BLOCK_WIDTH + (cos > 0 ? -10 : 10 + Const.BLOCK_WIDTH))
: pose.getX(),
sin != 0
? (mapPoseY * Const.BLOCK_WIDTH + (sin > 0 ? -10 : 10 + Const.BLOCK_WIDTH))
: pose.getY(),
cacheWayAngle));
return true;
}
return false;
}
/**
* Creates the default kettle properties file, containing the standard header.
*
* @param directory the directory
*/
private static void createDefaultKettleProperties(String directory) {
String kpFile = directory + Const.FILE_SEPARATOR + Const.KETTLE_PROPERTIES;
File file = new File(kpFile);
if (!file.exists()) {
FileOutputStream out = null;
try {
out = new FileOutputStream(file);
out.write(Const.getKettlePropertiesFileHeader().getBytes());
} catch (IOException e) {
System.err.println(
BaseMessages.getString(
PKG,
"Props.Log.Error.UnableToCreateDefaultKettleProperties.Message",
Const.KETTLE_PROPERTIES,
kpFile));
System.err.println(e.getStackTrace());
} finally {
if (out != null) {
try {
out.close();
} catch (IOException e) {
System.err.println(
BaseMessages.getString(
PKG,
"Props.Log.Error.UnableToCreateDefaultKettleProperties.Message",
Const.KETTLE_PROPERTIES,
kpFile));
System.err.println(e.getStackTrace());
}
}
}
}
}
/* called when clicked */
public void onClick(View v) {
if (v == (View) this.endButton) {
// called when end button clicked
Const.Trace(this, "end button clicked");
finish();
}
}
public String toString() {
String string = "";
if (sourceRepository && !Const.isEmpty(directory) && !Const.isEmpty(repositoryName)) {
string += "[" + repositoryName + "] ";
if (directory.endsWith(RepositoryDirectory.DIRECTORY_SEPARATOR)) {
string += ": " + directory + filename;
} else {
string += ": " + RepositoryDirectory.DIRECTORY_SEPARATOR + filename;
}
} else {
string += filename;
}
return string;
}
public static void init() {
Add.init();
Address.init();
Align.init();
Alloc.init();
Anchor.init();
And.init();
Bad.init();
Bitcast.init();
Block.init();
Builtin.init();
Call.init();
Cmp.init();
Cond.init();
Confirm.init();
Const.init();
Conv.init();
CopyB.init();
Deleted.init();
Div.init();
Dummy.init();
End.init();
Eor.init();
Free.init();
IJmp.init();
Id.init();
Jmp.init();
Load.init();
Member.init();
Minus.init();
Mod.init();
Mul.init();
Mulh.init();
Mux.init();
NoMem.init();
Not.init();
Offset.init();
Or.init();
Phi.init();
Pin.init();
Proj.init();
Raise.init();
Return.init();
Sel.init();
Shl.init();
Shr.init();
Shrs.init();
Size.init();
Start.init();
Store.init();
Sub.init();
Switch.init();
Sync.init();
Tuple.init();
Unknown.init();
}
/**
* Creates the kettle home area, which is a directory containing a default kettle.properties file
*/
public static void createKettleHome() {
// Try to create the directory...
//
String directory = Const.getKettleDirectory();
File dir = new File(directory);
try {
dir.mkdirs();
// Also create a file called kettle.properties
//
createDefaultKettleProperties(directory);
} catch (Exception e) {
}
}
public void checkTermination(Context ctxt) {
Expr no_annos = dropAnnos(ctxt).defExpandTop(ctxt);
if (no_annos.construct == FUN_TERM || no_annos.construct == CONST) return;
if (no_annos.construct != TERM_APP)
handleError(
ctxt,
"We are checking termination of a term application where"
+ " dropping\nannotations gives an unexpected form of expression."
+ "1. the term application: "
+ toString(ctxt)
+ "\n2. with annotations dropped: "
+ no_annos.toString(ctxt));
TermApp t = (TermApp) ((TermApp) no_annos).spineForm(ctxt, true, true, true);
TermApp anno_t = (TermApp) spineForm(ctxt, false, true, true);
if (ctxt.getFlag("debug_terminates")) {
ctxt.w.println(
"termTerminates checking TermApp.\n"
+ "\n1. original term, no annos: "
+ no_annos.toString(ctxt)
+ "\n2. spine form, no annos: "
+ t.toString(ctxt)
+ "\n3. spine form, with annos: "
+ anno_t.toString(ctxt)
+ "\n4. head:"
+ anno_t.head.toString(ctxt)
+ "\n5. inj:"
+ anno_t.isI(ctxt)
+ "\n5. head is ctor:"
+ (anno_t.head.construct == CONST ? ctxt.isTermCtor((Const) anno_t.head) : "false"));
ctxt.w.flush();
}
if (t.head.construct != CONST)
handleError(
ctxt,
"Checking termination, the head of an application is "
+ "not a constant.\n"
+ "1. the application in spine form: "
+ t.toString(ctxt)
+ "\n2. the head: "
+ t.head.toString(ctxt));
Const c = (Const) t.head;
boolean is_total = ctxt.isTotal(c);
if (!is_total && !ctxt.isTermCtor(c))
handleError(
ctxt,
"Checking termination, the head of an application is "
+ "neither\ndeclared total nor a term constructor.\n"
+ "1. the application in spine form: "
+ t.toString(ctxt)
+ "\n2. the head: "
+ c.toString(ctxt));
if (is_total) {
/* a little more work is needed here to check that if
totality were proved when some arguments are certain
fixed other terms, we have those arguments here. */
Collection thms = ctxt.getTotalityTheorems(c);
Iterator it = thms.iterator();
boolean problem = true;
while (it.hasNext()) {
Forall F = (Forall) it.next();
TermApp lhs = (TermApp) ((Atom) ((Exists) F.body).body).Y1;
problem = false;
for (int j = 0, jend = lhs.X.length; j < jend; j++)
if (lhs.X[j].construct != VAR)
if (!lhs.X[j].defEq(ctxt, t.X[j])) {
problem = true;
break; // out of for loop
}
if (!problem)
// we have found a matching totality theorem.
break; // out of while loop
}
if (problem) {
String s =
("Termination checking cannot find a registered totality theorem"
+ " for the\ngiven application. The number of theorems registered"
+ " for the head is "
+ (new Integer(thms.size())).toString()
+ ".\n"
+ "1. the term to termination check: "
+ t.toString());
if (thms.size() > 0) {
s = s + "2. the totality theorems registered for the head:\n";
it = thms.iterator();
while (it.hasNext()) {
Forall F = (Forall) it.next();
s = s + "-- " + F.toString(ctxt);
}
}
handleError(ctxt, s);
}
}
/* we cannot look at just t next, because terminates casts
will have been dropped computing it. */
for (int i = 0, iend = t.X.length; i < iend; i++) anno_t.X[i].checkTermination(ctxt);
}
public void run() {
while (true) {
if (_suppress) {
Thread.yield();
continue;
}
Pose oldPose = cachePose;
cachePose = Rover.poseProvider.getPose();
// get angle and distance where to travel
int[] way = wayClass.getPredefinedWay(oldPose, cachePose);
cacheWayAngle = way[1];
cacheWayDistance = way[0] * Const.BLOCK_WIDTH;
float angle = Const.normalizeAngle(cacheWayAngle - cachePose.getHeading());
if (Const.angleToByte(angle) == 2 && cacheWayDistance == Const.BLOCK_WIDTH) {
// if the robot is supposed to rotate 180 deg and travel one block, back out
Rover.pilot.travel(-cacheWayDistance, true);
} else {
// otherwise rotate and travel the distance
if (Math.abs(angle) > 2) {
// float cacheGyro = Rover.getGyro();
boolean touched = false;
Rover.pilot.rotate(angle, true);
while (Rover.pilot.isMoving()) {
boolean[] touch = Rover.getTouch();
if (touch[0] || touch[1]) touched = true;
Thread.yield();
}
Pose newPose = Rover.poseProvider.getPose();
/*
float angleGyro = Rover.getGyro() - cacheGyro;
// if the turn is incomplete, rotate by the difference
// TODO maybe turn only when the difference is bigger
if (angle > angleGyro + 5 || angle < angleGyro - 5) {
Rover.pilot.travel(touched ? -5 : 5);
cacheWayDistance += touched ? 5 : -5;
Rover.pilot.rotate(angle - angleGyro);
newPose = Rover.poseProvider.getPose();
} */
// update pose with normalize heading
Rover.poseProvider.setPose(new Pose(newPose.getX(), newPose.getY(), cacheWayAngle));
}
float blockAngle = (float) Math.toRadians(cacheWayAngle);
byte sin = (byte) Math.round(Math.sin(blockAngle)),
cos = (byte) Math.round(Math.cos(blockAngle));
byte
blockX =
(byte)
(Const.roundToMapArea(Const.middlePointToBlock(cachePose.getX()), "X")
+ cos * (way[0] + 1)),
blockY =
(byte)
(Const.roundToMapArea(Const.middlePointToBlock(cachePose.getY()), "Y")
+ sin * (way[0] + 1));
// extend the distance to hit a wall
if (blockX < 0
|| blockY < 0
|| blockX > 8
|| blockY > 5
|| wayClass.getOccupancyMap(blockX, blockY) > 0) {
cacheWayDistance += Const.BLOCK_WIDTH;
// TODO no way of updating pose if it doesn't hit wall
}
Rover.pilot.travel(cacheWayDistance, true);
}
// while the rover is traveling
while (Rover.pilot.isMoving()) {
boolean[] touch = Rover.getTouch();
if (touch[0] || touch[1]) {
Rover.pilot.quickStop();
float sonic = Rover.getSonic();
Pose hitPose = Rover.poseProvider.getPose();
if (hitTheWall(hitPose)) {
Rover.pilot.travel(-4);
} else if (sonic > 15 && !(touch[0] && touch[1])) {
// if the rover touches the obstacle by just one sensor
// and distance is more than 15 cm
float distanceTraveled = Rover.pilot.getMovement().getDistanceTraveled();
Rover.pilot.travel(-4);
Rover.pilot.rotate(touch[0] ? -20 : 20);
Pose newPose = Rover.poseProvider.getPose();
Rover.poseProvider.setPose(new Pose(newPose.getX(), newPose.getY(), cacheWayAngle));
cacheWayDistance -= distanceTraveled - 4;
Rover.pilot.travel(cacheWayDistance, true);
} else {
// if the obstacle touched by both sensors
// or the distance is less than 15 cm
Pose p = Rover.poseProvider.getPose();
byte blockX = Const.roundToMapArea(Const.middlePointToBlock(p.getX()), "X"),
blockY = Const.roundToMapArea(Const.middlePointToBlock(p.getY()), "Y");
float blockAngle = (float) Math.toRadians(Const.angleToByte(p.getHeading()) * 90);
byte sin = (byte) Math.round(Math.sin(blockAngle)),
cos = (byte) Math.round(Math.cos(blockAngle));
boolean isFree;
try {
isFree = wayClass.getOccupancyMap((byte) (blockX + cos), (byte) (blockY + sin)) < 0;
} catch (ArrayIndexOutOfBoundsException e) {
isFree = false;
}
if (!(touch[0] && touch[1]) && isFree) {
Rover.pilot.rotate(touch[0] ? -90 : 90);
Rover.pilot.travel(15);
Rover.pilot.rotate(touch[0] ? 90 : -90);
Rover.poseProvider.setPose(
new Pose(
(blockX + 1 / 2) * Const.BLOCK_WIDTH,
(blockY + 1 / 2) * Const.BLOCK_WIDTH,
Const.angleToByte(p.getHeading()) * 90));
float distanceTraveled = Rover.pilot.getMovement().getDistanceTraveled();
cacheWayDistance -= distanceTraveled;
Rover.pilot.travel(cacheWayDistance, true);
} else {
System.out.println("ERROR: TRAVEL CORRECTION");
// This shouldn't happen any more
Button.LEDPattern(5);
}
}
}
Thread.yield();
}
}
}
