Diff: main_ps3.cpp

Revision:

26:760f1bce8214

Parent:

24:6d2573d6f2b6

Child:

27:88863fab46c0

--- a/main_ps3.cpp	Sun Nov 08 04:26:26 2015 +0000
+++ b/main_ps3.cpp	Sun Nov 08 10:56:27 2015 +0000
@@ -13,17 +13,6 @@
 #error Caution, You should define either BLUE or RED
 #endif
 
-#ifdef MESURE
-#include "mbed.h"
-short mesureflag=0;
-LocalFileSystem local("local")
-short testflag=0;
-FILE *fp_r = fopen("/local/velocity.dat", "w");
-#endif
-/*#include "mbed.h"
-LocalFileSystem local("local");
-FILE *fp_r = fopen("/local/velocity.dat", "w");*/
-
 #include "machine_ps3.h"
 
 Serial pc(USBTX, USBRX);
@@ -54,18 +43,18 @@
 #ifdef BLUE
             autoIM920(); /*IM920 button*/
             /********************************Nomal Mode*********************************/
-            if((step==0)&&((8600.0>x)&&(x>800.0))) {
-                targ_sita=-0.04;
+            if((step==0)&&((9000.0>x)&&(x>800.0))) {
+                targ_sita=-0.07;
 //                targ_sita=0.0;
                 step=1;
             }
-//            if((step==1)&&(x>8600.0+deff)) {
-            if((step==1)&&(x>8600.0)) {
+            if((step==1)&&(x>9000.0)) {
                 targ_velocity=0.0;
-                direction_controller.setBias(0.0);
-                direction_controller.reset();
+                /*direction_controller.setBias(0.0);
+                direction_controller.reset();*/
                 velocity_controller.setBias(0.0);
                 velocity_controller.reset();
+//                dpcount=speed;
                 step=2;
             }
             if((step==2)&&((velocity<500.0)&&(velocity>-500.0))){
@@ -73,34 +62,31 @@
                 spcount=0.0;
                 flagf=0;
 //                targ_sita=0.0;
-                targ_sita=-0.04;
+                targ_sita=-0.07;
             }
             if((step==3)&&(x<1400.0)) {
                 targ_sita=PI/4;
                 step=4;
             }
-            if((step==4)&&(x<800.0)) {
+            if((step==4)&&(x<600.0)) {
                 dpcount=speed;
                 step=114;
             }
-            /*if((step==4)&&(x<10.0)) {
-                targ_velocity=0.0;
-                step=114;
-            }*/
+            
             /***Cylinder***/
-            if((x>3300.0)&&(CStep==1)) { 
+            if((x>3100.0)&&(CStep==1)) { 
                 if(!skip) sendData(1,1);
                 CStep=2; 
             }
-            if((x>6100.0)&&(CStep==2)) {
+            if((x>6000.0)&&(CStep==2)) {
                 if(!skip) sendData(1,3);
                 CStep=3;
             }
-            if((x>7880.0)&&(CStep==3)) {
+            if((x>7700.0)&&(CStep==3)) {
                 if(!skip) sendData(1,2);
                 CStep=4;
             }
-            if((x<6880.0)&&(CStep==4)) {
+            if((x<8500.0)&&(CStep==4)) {
                 if(!skip) sendData(1,5);
                 CStep=5;
             }
@@ -108,11 +94,11 @@
                 if(!skip) sendData(1,6);
                 CStep=6;
             }
-            if((x<6300.0)&&(CStep==6)) {
+            if((x<4900.0)&&(CStep==6)) {
                 if(!skip) sendData(1,4);
                 CStep=7;
             }
-            if((x<4000.0)&&(CStep==7)){
+            if((x<1000.0)&&(CStep==7)){
                 sendData(7,0);
                 CStep=114;
             }
@@ -121,7 +107,6 @@
         }
         else if(!autoflag) {
             flaga=0;
-            manualMoveIM920(); /*analogStick*/
             manualIM920();     /*IM920 button*/
 //            mesureSwing();
 //            swingFollowing();