Revision 2:0be4cfbdf408, committed 2020-03-27

Comitter:

maxnagazumi

Date:

Fri Mar 27 06:31:20 2020 +0000

Parent:

1:a692014d8e41

Commit message:

soho;

Changed in this revision

--- a/main.cpp	Thu Mar 12 05:52:39 2020 +0000
+++ b/main.cpp	Fri Mar 27 06:31:20 2020 +0000
@@ -8,7 +8,7 @@
 
 CAN can1(PB_5,PB_13);
 /*
-0 start
+0 start/stop
 1 to 4 , 6 to 9
 5 stop
 10 clock move
@@ -38,12 +38,12 @@
 Serial pc(USBTX, USBRX); // tx, rx
 R1370P gyro(PC_10,PC_11);   // tx, rx
 
-HCSR04 echo[]= {
+/*HCSR04 echo[]= {
     HCSR04(PC_0,PC_12)//A
     ,HCSR04(PA_15,PB_7)//A
     ,HCSR04(PH_1,PB_0)// B
     ,HCSR04(PC_3,PB_10)//B
-};
+};*/
 
 //自己位置取得
 double theta;
@@ -97,10 +97,14 @@
 //目的地決定
 int plot[][2]= {
     {0,0}
-    ,{800,13500}
-    ,{1000,15500}
-    ,{8654,16500}
-    ,{16000,16500}
+    ,{2000,16000}
+    ,{8000,16000}
+    ,{4000,0}
+    ,{0,0}
+    ,{0,4000}
+    ,{4000,4000}
+    ,{4000,0}
+    ,{0,0}
 };
 
 double aimTheta[]= {//目標角度を指定
@@ -108,7 +112,7 @@
 };
 
 double zMin[]= { //速度の最少を指定
-    2,2,2, 2,2,2, 5,5,5, 5,5,5
+    0,0,0,0,0,0,0,0,0,0,0,0
 };
 
 //出力を計算
@@ -182,7 +186,7 @@
 }
 
 //超音波
-class Sonic
+/*class Sonic
 {
 public:
     Sonic()
@@ -210,16 +214,20 @@
     double sonic_cm[4];
 };
 
+*/
 class CAN_ticker
 {
 public:
-    CAN_ticker():x(0),data(0) {}
-    void can_read()
+    CAN_ticker():x(0)
+    {
+        data[0]=0;
+    }
+    void canmsg_read()
     {
         CANMessage msg;
         if(can1.read(msg)) {
             if(msg.id == 1) {
-                x=(short)(msg.data);
+                x=(short)(msg.data[0]);
             }
         }
     }
@@ -228,92 +236,111 @@
         return x;
     }
 private:
-    char data;
+    char data[0];
     int x;
 };
 //手動出力
+double canOmega[4]= {
+    0,0,0,0
+};
 void calOmega_CAN(int canx)
 {
-    int a=10,count=0;
-    double canOmega[4];
+    static double a=0.1;
+    static int count=0;
     switch(canx) {
         case 1:
             canOmega[0]=0;
-            canOmega[1]=-a;
+            canOmega[1]=a;
             canOmega[2]=0;
-            canOmega[3]=a;
+            canOmega[3]=-a;
+            printf("   1");
             break;
         case 2:
-            canOmega[0]=-a*1.4;
-            canOmega[1]=-a*1.4;
-            canOmega[2]=a*1.4;
-            canOmega[3]=a*1.4;
+            canOmega[0]=a*1.4;
+            canOmega[1]=a*1.4;
+            canOmega[2]=-a*1.4;
+            canOmega[3]=-a*1.4;
+            printf("   2");
             break;
         case 3:
-            canOmega[0]=-a;
+            canOmega[0]=a;
             canOmega[1]=0;
-            canOmega[2]=a;
+            canOmega[2]=-a;
             canOmega[3]=0;
+            printf("   3");
             break;
         case 4:
-            canOmega[0]=a*1.4;
-            canOmega[1]=-a*1.4;
-            canOmega[2]=-a*1.4;
-            canOmega[3]=a*1.4;
+            canOmega[0]=-a*1.4;
+            canOmega[1]=a*1.4;
+            canOmega[2]=a*1.4;
+            canOmega[3]=-a*1.4;
+            printf("   4");
             break;
         case 5:
             for(int i=0; i<4; i++) {
                 canOmega[i]=0;
             }
+            printf("   5");
             break;
         case 6:
-            canOmega[0]=-a*1.4;
-            canOmega[1]=a*1.4;
-            canOmega[2]=a*1.4;
-            canOmega[3]=-a*1.4;
+            canOmega[0]=a*1.4;
+            canOmega[1]=-a*1.4;
+            canOmega[2]=-a*1.4;
+            canOmega[3]=a*1.4;
+            printf("   6");
             break;
         case 7:
-            canOmega[0]=a;
-            canOmega[1]=0;
-            canOmega[2]=-a;
-            canOmega[3]=0;
+            canOmega[0]=0;
+            canOmega[1]=-a;
+            canOmega[2]=0;
+            canOmega[3]=a;
+            printf("   7");
             break;
         case 8:
-            canOmega[0]=a*1.4;
-            canOmega[1]=a*1.4;
-            canOmega[2]=-a*1.4;
-            canOmega[3]=-a*1.4;
+            canOmega[0]=-a*1.4;
+            canOmega[1]=-a*1.4;
+            canOmega[2]=a*1.4;
+            canOmega[3]=a*1.4;
+            printf("   8");
             break;
         case 9:
-            canOmega[0]=0;
-            canOmega[1]=a;
-            canOmega[2]=0;
-            canOmega[3]=-a;
+            canOmega[0]=-a;
+            canOmega[1]=0;
+            canOmega[2]=a;
+            canOmega[3]=0;
+            printf("   9");
             break;
         case 10:
             for(int i=0; i<4; i++) {
-                canOmega[i]=-a;
+                canOmega[i]=a;
             }
+            printf("   10");
             break;
         case 11:
             for(int i=0; i<4; i++) {
-                canOmega[i]=a;
+                canOmega[i]=-a;
             }
+            printf("   11");
             break;
         case 12:
-            if(count%2==1) {
-                a=20;
+            wait(0.2);
+            if(count==1) {
+                a=0.1;
+                count=0;
             } else {
-                a=10;
+                a=0.05;
+                count=1;
             }
+            printf("   12 Speed change");
             break;
 
-
     }
-    for(int i=0; i<4; i++) {
-        motor[i].Sc(canOmega[i]);
+    printf(" %f\r\n",a);
+    int i=0;
+    while(i<4) {
+        motor[i].turn(canOmega[i]);
+        i++;
     }
-
 }
 
 //ticker に入れる関数
@@ -328,11 +355,13 @@
         motor[i].Sc(omega.getOmega(i));
     }
 }
-
 CAN_ticker canx;
 void ticker_CanRead()
 {
-    canx.can_read();
+    canx.canmsg_read();
+    /*for(int i=0; i<4; i++) {
+       motor[i].Sc(canOmega[i]);
+    }*/
 }
 
 Location location;
@@ -343,10 +372,10 @@
 }
 int main()
 {
-    angle=gyro.getAngle();//角度初期化
     can1.frequency(1000000);
     gyro.initialize();    //main関数の最初に一度だけ実行
     gyro.acc_offset();
+    angle=gyro.getAngle();//角度初期化
     double z;
     printf("start\r\n");
     motor[0].setEquation(0.008031,-0.022300,-0.008839,-0.016290);
@@ -359,10 +388,10 @@
     motor[2].setDutyLimit(0.4);
     motor[3].setDutyLimit(0.4);
 
-    motor[0].setPDparam( 0.1790, 0.00560);
-    motor[1].setPDparam( 0.1705, 0.00620);
-    motor[2].setPDparam( 0.1790, 0.00620);
-    motor[3].setPDparam( 0.1680, 0.00560);
+    motor[0].setPDparam( 0.01790, 0.00560);
+    motor[1].setPDparam( 0.01705, 0.00620);
+    motor[2].setPDparam( 0.01790, 0.00620);
+    motor[3].setPDparam( 0.01680, 0.00560);
 
     int first=0;
     while(first==0) {
@@ -383,8 +412,10 @@
     locTicker.attach(&calLoc,0.05);
     canTicker.attach(&ticker_CanRead,0.1);//can読み込み
     while(1) {
-        canX=canx.get_xCAN();//０で手動化
-        if(canX==0) {//手動化
+        canX=canx.get_xCAN();//０で手\動化
+        if(canX==13) {//手動化
+            wait(0.2);
+            printf("change1\r\n");
             ticker.detach();
             while(1) {
                 canX=canx.get_xCAN();
@@ -392,20 +423,26 @@
                 //自己位置取得
                 x=location.getX();
                 y=location.getY();
+                printf("X=%06d ,Y=%06d",x,y);
                 //目的地決定(syuusoku check)
                 aimX = plot[n][0];
                 aimY = plot[n][1];
                 dx=aimX-x;
                 dy=aimY-y;
                 distance = sqrt((float)dx*dx+dy*dy);
-                if(distance<800) {
+                if(distance<1000) {
                     n++;
                     printf("reach%d\r\n",n);
                     time.reset();
                 }
-                if(canX==0) {//自動化
+                if(canX==13) {//自動化
                     canX=20;
+                    for(int i=0; i<4; i++) {
+                        canOmega[i]=0;
+                    }
                     ticker.attach(&motorOut,0.05);
+                    printf("change2\r\n");
+                    wait(0.1);
                     break;
                 }
             }
@@ -413,8 +450,17 @@
         //自己位置取得
         x=location.getX();
         y=location.getY();
-        printf("X=%d,Y=%d,theta=%5.3f z=%5.3f  %f\r\n",x,y,theta,z,time.read());
-
+        int i=0;
+        if(i==10) {
+            printf("X=%d,Y=%d,theta=%5.3f ,angle=%5.3f z=%5.3f  n=%d\r\n",x,y,theta,angle,z,n);
+            for(int j=0; j<4; j++) {
+                printf("%d : %d,",j,ec[j].getCount());
+            }
+            printf("\r\n");
+            i=10;
+        } else {
+            i++;
+        }
         //目的地決定(syuusoku check)
         aimX = plot[n][0];
         aimY = plot[n][1];
@@ -439,7 +485,7 @@
             time.reset();
         }
 
-        if(n>=5) {
+        if(n>=8) {
             for(int j=0; j<4; j++) {
                 motor[j].stop();
             }