Diff: main_ps3.cpp

Revision:

24:6d2573d6f2b6

Parent:

23:26f9483439fe

Child:

26:760f1bce8214

Child:

28:70e45354fbf3

diff -r 26f9483439fe -r 6d2573d6f2b6 main_ps3.cpp
--- a/main_ps3.cpp	Sat Nov 07 01:03:46 2015 +0000
+++ b/main_ps3.cpp	Sun Nov 08 04:26:26 2015 +0000
@@ -26,18 +26,18 @@
 
 #include "machine_ps3.h"
 
-#define deff 70.0
-
 Serial pc(USBTX, USBRX);
 
-
+//LocalFileSystem local("local");
 
 int main() {
+//    FILE *fp_r = fopen("/local/velocity.dat", "w");
     Com.attach(&Call,RATE);
     initializeMotors();
     initializeControllers();
     initializeRS485();
     initializeMbedSerial();
+    initializeSwing();
 #ifdef BLUE
     sita=PI/4.0,targ_sita=PI/4.0;
     IndicatorBLUE = 1;
@@ -51,10 +51,11 @@
     sendData(7,0);
     while(1) {
         if(autoflag){
+#ifdef BLUE
             autoIM920(); /*IM920 button*/
             /********************************Nomal Mode*********************************/
             if((step==0)&&((8600.0>x)&&(x>800.0))) {
-                targ_sita=-0.02;
+                targ_sita=-0.04;
 //                targ_sita=0.0;
                 step=1;
             }
@@ -71,54 +72,66 @@
                 step=3;
                 spcount=0.0;
                 flagf=0;
-//                targ_sita=0.1;
-                targ_sita=0.0;
+//                targ_sita=0.0;
+                targ_sita=-0.04;
             }
-//            if((step==3)&&(x<2000.0)) {
             if((step==3)&&(x<1400.0)) {
                 targ_sita=PI/4;
                 step=4;
             }
-            if((step==4)&&(x<600.0)) {
-                targ_velocity=0.0;
+            if((step==4)&&(x<800.0)) {
+                dpcount=speed;
                 step=114;
             }
+            /*if((step==4)&&(x<10.0)) {
+                targ_velocity=0.0;
+                step=114;
+            }*/
             /***Cylinder***/
-            if((x>3130.0+deff)&&(CStep==1)) { 
-//            if((x>2030.0+deff)&&(CStep==1)) { 
+            if((x>3300.0)&&(CStep==1)) { 
                 if(!skip) sendData(1,1);
                 CStep=2; 
             }
-            if((x>5900.0+deff)&&(CStep==2)) {
+            if((x>6100.0)&&(CStep==2)) {
                 if(!skip) sendData(1,3);
                 CStep=3;
             }
-            if((x>7680.0+deff)&&(CStep==3)) {
+            if((x>7880.0)&&(CStep==3)) {
                 if(!skip) sendData(1,2);
                 CStep=4;
             }
-            if((x<6580.0+deff)&&(CStep==4)) {
+            if((x<6880.0)&&(CStep==4)) {
                 if(!skip) sendData(1,5);
                 CStep=5;
             }
-            if((x<6100.0+deff)&&(CStep==5)) {
-                if(!skip) sendData(1,4);
+            if((x<6800.0)&&(CStep==5)) {
+                if(!skip) sendData(1,6);
                 CStep=6;
             }
-            if((x<4000.0)&&(CStep==6)){
+            if((x<6300.0)&&(CStep==6)) {
+                if(!skip) sendData(1,4);
+                CStep=7;
+            }
+            if((x<4000.0)&&(CStep==7)){
                 sendData(7,0);
                 CStep=114;
             }
+#else
+#endif
         }
         else if(!autoflag) {
             flaga=0;
-            mesureSwing();
             manualMoveIM920(); /*analogStick*/
             manualIM920();     /*IM920 button*/
+//            mesureSwing();
+//            swingFollowing();
+//            wait(RATE);
+//            fprintf(fp_r, "%f\r\n",swingRadVelocity);
+//            if(b==11) fclose(fp_r);
+            pc.printf("Swing:%f\r\n",swingRadVelocity);
         }
         /***update state***/ 
-        pc.printf("x:%f ,y:%f ,sita:%f ,r:%f\r\n",x,y,sita,Pulses_move_r);
-//        pc.printf("a2:%d ,b:%d ,X:%d ,Y:%d\r\n",a2,b,X,Y);
-//        wait(RATE);
+//        pc.printf("Swing:%f\r\n",SwingSens.getPulses());
+//        pc.printf("x:%f ,y:%f ,sita:%f ,r:%f\r\n",x,y,sita,Pulses_move_r);
     }
 }
\ No newline at end of file