diff --git a/ARDroneForP5_Sample.pde b/ARDroneForP5_Sample.pde
index 3252969d485a18f2b905611080a4afe481769397..2759ff910ab2b9ca25360f9a8fcea324582c5b8c 100644
--- a/ARDroneForP5_Sample.pde
+++ b/ARDroneForP5_Sample.pde
@@ -1,8 +1,11 @@
 import com.shigeodayo.ardrone.processing.*;
+import java.util.Timer;
 
 ARDroneForP5 ardrone;
 
-char state='n';
+boolean stop = false;
+char state = 'n';
+int start_time = 0;
 
 void setup() {
   size(640, 360);
@@ -19,8 +22,7 @@ void setup() {
 }
 
 void draw() {
-  background(204);  
-
+  background(204);
   // getting image from AR.Drone
   // true: resizeing image automatically;
   // false: not resizing
@@ -39,8 +41,6 @@ void draw() {
   float altitude = ardrone.getAltitude();
   float[] velocity = ardrone.getVelocity();
   int battery = ardrone.getBatteryPercentage();
-  
-  float vx = velocity[0], vy = velocity[1];
 
   String attitude = "pitch:" + pitch + "\nroll:" + roll + "\nyaw:" + yaw + "\naltitude:" + altitude;
   text(attitude, 20, 85);
@@ -48,12 +48,27 @@ void draw() {
   text(vel, 20, 140);
   String bat = "battery:" + battery + " %";
   text(bat, 20, 170);
+
+  float vx = velocity[0], vy = velocity[1];
+  if(stop){
+    int lr = (int)vy / 15;
+    int fb = (int)vx / 15;
+    ardrone.parallel(-lr, fb);
+  }
+
+  int ms = millis();
+  String time = "time:" + ms;
+  text(time, 20, 210);
+  
+  if(state=='w'){
+    square(ms);
+  }
 }
 
 //PC AR.Drone
 // controlling AR.Drone through key input
 void keyPressed() {
-
+  stop = false;
   if (key == CODED) {
     if (keyCode == UP) {
       state='f';
@@ -73,41 +88,24 @@ void keyPressed() {
     } 
     else if (keyCode == SHIFT) {
       ardrone.takeOff(); // take off, AR.Drone cannot move while landing
+      stop = true;
     } 
     else if (keyCode == CONTROL) {
+      state = 'n';
       ardrone.landing();
       // landing
     }
   }
   else {
-    if (key == 's') {
-      if(vx > 30){  
-        ardrone.parallel(,);
-      }
-      //switch(state){
-      //  case 'f':  //if current move is forward      
-      //    ardrone.backward();
-      //    delay(1500);
-      //    break;
-          
-      //  case 'b':  //if current move is backword      
-      //    ardrone.forward();
-      //    delay(1500);
-      //    break;
-          
-      //  case 'l':  //if current move is right      
-      //    ardrone.goRight();
-      //    delay(1500);
-      //    break;
-          
-      //  case 'r':  //if current move is left      
-      //    ardrone.goLeft();
-      //    delay(1500);
-      //    break;
-         
-      //  default:break;
-      //  }
-      //ardrone.stop(); // hovering
+    if (key =='w'){
+      state='w';
+      start_time = millis();
+
+    }
+    else if (key == 's') {
+      state = 's';
+      stop = true;
+      ardrone.stop();
     } 
     else if (key == 'r') {
       ardrone.spinRight(); // spin right
@@ -140,4 +138,26 @@ void keyPressed() {
       ardrone.parallel(-5,5);
     }
   }
+}
+
+void square(int ms){
+  int time = ms - start_time;
+  int speed = 10;
+  if(time <1000){
+    stop=true;
+  }else  if(time < 4000){
+    stop=false;
+    ardrone.forward(speed);
+  }else  if(time < 8000){
+    stop = true;
+  } else if (time < 11000){
+    stop=false;
+    ardrone.goRight(speed);
+  } else if(time < 16000){
+    stop = true;
+  } else {
+    stop = false;
+    state = 'n';
+    ardrone.landing();
+  }
 }
\ No newline at end of file