Fixed bug in SemiAutomaticReverseParking app

sics-sse · Jan 19, 2015 · 2a039c0 · 2a039c0
1 parent 2d136e9
commit 2a039c0
Showing 1 changed file with 5 additions and 4 deletions.
diff --git a/plugins/SemiAutomaticReverseParking/src/main/java/plugins/SemiAutomaticReverseParking.java b/plugins/SemiAutomaticReverseParking/src/main/java/plugins/SemiAutomaticReverseParking.java
@@ -4,7 +4,7 @@
 import sics.port.PluginPPort;
 import sics.port.PluginRPort;
 
-import com.sun.squawk.VM;
+//import com.sun.squawk.VM;
 
 public class SemiAutomaticReverseParking extends PlugInComponent {
 
@@ -76,7 +76,7 @@ private void gotoState(int s) {
 	public void run() {
 		init();
 
-		VM.println("SemiAutomaticReverseParking is running");
+//		VM.println("SemiAutomaticReverseParking is running");
 		System.out.println("SemiAutomaticReverseParking is running");
 
 		distance = 0;
@@ -86,6 +86,7 @@ public void run() {
 			double speed = (double)((Integer) wheelSpeed.readInt());
 //			distance += Math.round((speed * TIME_STEP) / 1000.0);
 			distance += round(speed * TIME_STEP / 1000.0);			//Math.round() is not included in basic Squawk (however MathUtils.round exists)
+			System.out.println("SARP: state = " + state + ", distance = " + distance + " at speed = " + speed);
 
 			switch (state) {
 			case STARTING:
@@ -112,8 +113,8 @@ public void run() {
 			    // Turn the wheels left (-100) and wait until the distance TURN_LEFT_DIST has been moved
 				steeringAngle.write(-100);
 				if (distance > TURN_LEFT_DIST)
-//					gotoState(State.GOING_STRAIGHT);
-					gotoState(GOING_STRAIGHT);
+//					gotoState(State.ALIGNING);
+					gotoState(ALIGNING);
 				break;
 			case ALIGNING:
 			    // Turn the wheels straight (0)