diff --git a/ARDroneForP5_Sample.pde b/ARDroneForP5_Sample.pde
index 2759ff910ab2b9ca25360f9a8fcea324582c5b8c..5bec7d702636ee7a1dba58be0dbf8a182874654a 100644
--- a/ARDroneForP5_Sample.pde
+++ b/ARDroneForP5_Sample.pde
@@ -3,14 +3,16 @@ import java.util.Timer;
 
 ARDroneForP5 ardrone;
 
+//OpenCV opencv;
 boolean stop = false;
 char state = 'n';
 int start_time = 0;
+float yaw_stop = 0;
 
 void setup() {
   size(640, 360);
-
-  ardrone=new ARDroneForP5("192.168.1.1");
+  stop = false;
+  ardrone = new ARDroneForP5("192.168.1.1");
   // connect to the AR.Drone
   ardrone.connect();
   // for getting sensor information
@@ -42,25 +44,32 @@ void draw() {
   float[] velocity = ardrone.getVelocity();
   int battery = ardrone.getBatteryPercentage();
 
-  String attitude = "pitch:" + pitch + "\nroll:" + roll + "\nyaw:" + yaw + "\naltitude:" + altitude;
+  String attitude = "pitch:" + pitch + "\nroll:" + roll + "\nyaw:" + yaw + "\naltitude:" + altitude + "\nyaw_stop:" + yaw_stop;
   text(attitude, 20, 85);
   String vel = "vx:" + velocity[0] + "\nvy:" + velocity[1];
-  text(vel, 20, 140);
+  text(vel, 20, 160);
   String bat = "battery:" + battery + " %";
-  text(bat, 20, 170);
+  text(bat, 20, 190);
+  String is_stop = "stop:" + stop;
+  text(is_stop, 20, 210);
 
   float vx = velocity[0], vy = velocity[1];
   if(stop){
-    int lr = (int)vy / 15;
-    int fb = (int)vx / 15;
-    ardrone.parallel(-lr, fb);
+    int lr = (int) vy / 15;
+    int fb = (int) vx / 15;
+    int spin = (int) (yaw - yaw_stop);
+    float yaw_diff = yaw_stop - yaw;
+    //if(yaw_diff){
+      
+    //}
+    ardrone.move3D(-fb, lr, 0, spin);
   }
 
   int ms = millis();
   String time = "time:" + ms;
-  text(time, 20, 210);
+  text(time, 20, 230);
   
-  if(state=='w'){
+  if(state == 'w'){
     square(ms);
   }
 }
@@ -71,41 +80,39 @@ void keyPressed() {
   stop = false;
   if (key == CODED) {
     if (keyCode == UP) {
-      state='f';
       ardrone.forward(); // go forward
     } 
     else if (keyCode == DOWN) {
-      state='b';
       ardrone.backward(); // go backward
     } 
     else if (keyCode == LEFT) {
-      state='l';
       ardrone.goLeft(); // go left
     } 
     else if (keyCode == RIGHT) {
-      state='r';
       ardrone.goRight(); // go right
     } 
     else if (keyCode == SHIFT) {
       ardrone.takeOff(); // take off, AR.Drone cannot move while landing
+      delay(2500);
       stop = true;
+      yaw_stop = ardrone.getYaw();
     } 
     else if (keyCode == CONTROL) {
+      ardrone.landing(); // landing
       state = 'n';
-      ardrone.landing();
-      // landing
+      stop = false;
     }
   }
   else {
     if (key =='w'){
-      state='w';
+      state = 'w';
       start_time = millis();
-
     }
-    else if (key == 's') {
+    else if (key == 's') { // hovering
+      ardrone.stop();
       state = 's';
       stop = true;
-      ardrone.stop();
+      yaw_stop = ardrone.getYaw();
     } 
     else if (key == 'r') {
       ardrone.spinRight(); // spin right
@@ -135,29 +142,45 @@ void keyPressed() {
       ardrone.toggleCamera(); // set next camera setting
     }
     else if (key == 'e'){
-      ardrone.parallel(-5,5);
+      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;
+  int speed = 5;
+  if(time < 1000){ //hitsuyou??
+    yaw_stop = ardrone.getYaw();
+    stop = true;
+  } else if(time < 4000){ //0~4s go forward for 4s
+    stop = false;
     ardrone.forward(speed);
-  }else  if(time < 8000){
+  } else if(time < 7000){ //4~7s stop for 3s
+    //yaw_stop = ardrone.getYaw();
+      
+      
+    stop = true;
+  } else if (time < 11000){ //7~11s go right for 4s
+    stop = false;
+    ardrone.goRight(speed+2);
+  } else if(time < 14000){ //11~14s stop for 3s
+    //yaw_stop = ardrone.getYaw();
     stop = true;
-  } else if (time < 11000){
-    stop=false;
-    ardrone.goRight(speed);
-  } else if(time < 16000){
+  } else if(time < 18000){ //14~18s go backward for 4s
+    stop = false;
+    ardrone.backward(speed+2);
+  } else if(time < 21000){ //18~21s stop for 3s
+    //yaw_stop = ardrone.getYaw();
+    stop = true;
+  } else if(time < 25000){ //21~25s go left for 4s
+    stop = false;
+    ardrone.goLeft(speed+2);
+  } else if(time < 28000){ //25~28s stop for 3s
+    //yaw_stop = ardrone.getYaw();
     stop = true;
-  } else {
+  } else { //28s~ landing
     stop = false;
-    state = 'n';
     ardrone.landing();
   }
 }
\ No newline at end of file