Diff: main.cpp

Revision:

1:a692014d8e41

Parent:

0:c0e9bbc27454

Child:

2:0be4cfbdf408

--- a/main.cpp	Wed Mar 11 05:54:39 2020 +0000
+++ b/main.cpp	Thu Mar 12 05:52:39 2020 +0000
@@ -7,7 +7,14 @@
 #define RESOLUTION 500
 
 CAN can1(PB_5,PB_13);
-
+/*
+0 start
+1 to 4 , 6 to 9
+5 stop
+10 clock move
+11 not clock move
+12 speed change
+*/
 Ec2multi ec[]= {
     Ec2multi(PC_5,PB_2,RESOLUTION),
     Ec2multi(PA_11,PB_1,RESOLUTION),
@@ -38,10 +45,10 @@
     ,HCSR04(PC_3,PB_10)//B
 };
 
-Ticker ticker;
+//自己位置取得
+double theta;
+double angle;
 
-//自己位置取得
-double theta=0;
 class Location
 {
 public:
@@ -177,7 +184,7 @@
 //超音波
 class Sonic
 {
-private:
+public:
     Sonic()
     {
         for(int i=0; i<4; i++) {
@@ -203,10 +210,118 @@
     double sonic_cm[4];
 };
 
+class CAN_ticker
+{
+public:
+    CAN_ticker():x(0),data(0) {}
+    void can_read()
+    {
+        CANMessage msg;
+        if(can1.read(msg)) {
+            if(msg.id == 1) {
+                x=(short)(msg.data);
+            }
+        }
+    }
+    int get_xCAN()
+    {
+        return x;
+    }
+private:
+    char data;
+    int x;
+};
+//手動出力
+void calOmega_CAN(int canx)
+{
+    int a=10,count=0;
+    double canOmega[4];
+    switch(canx) {
+        case 1:
+            canOmega[0]=0;
+            canOmega[1]=-a;
+            canOmega[2]=0;
+            canOmega[3]=a;
+            break;
+        case 2:
+            canOmega[0]=-a*1.4;
+            canOmega[1]=-a*1.4;
+            canOmega[2]=a*1.4;
+            canOmega[3]=a*1.4;
+            break;
+        case 3:
+            canOmega[0]=-a;
+            canOmega[1]=0;
+            canOmega[2]=a;
+            canOmega[3]=0;
+            break;
+        case 4:
+            canOmega[0]=a*1.4;
+            canOmega[1]=-a*1.4;
+            canOmega[2]=-a*1.4;
+            canOmega[3]=a*1.4;
+            break;
+        case 5:
+            for(int i=0; i<4; i++) {
+                canOmega[i]=0;
+            }
+            break;
+        case 6:
+            canOmega[0]=-a*1.4;
+            canOmega[1]=a*1.4;
+            canOmega[2]=a*1.4;
+            canOmega[3]=-a*1.4;
+            break;
+        case 7:
+            canOmega[0]=a;
+            canOmega[1]=0;
+            canOmega[2]=-a;
+            canOmega[3]=0;
+            break;
+        case 8:
+            canOmega[0]=a*1.4;
+            canOmega[1]=a*1.4;
+            canOmega[2]=-a*1.4;
+            canOmega[3]=-a*1.4;
+            break;
+        case 9:
+            canOmega[0]=0;
+            canOmega[1]=a;
+            canOmega[2]=0;
+            canOmega[3]=-a;
+            break;
+        case 10:
+            for(int i=0; i<4; i++) {
+                canOmega[i]=-a;
+            }
+            break;
+        case 11:
+            for(int i=0; i<4; i++) {
+                canOmega[i]=a;
+            }
+            break;
+        case 12:
+            if(count%2==1) {
+                a=20;
+            } else {
+                a=10;
+            }
+            break;
+
+
+    }
+    for(int i=0; i<4; i++) {
+        motor[i].Sc(canOmega[i]);
+    }
+
+}
+
+//ticker に入れる関数
+Ticker ticker;
+Ticker canTicker;
+Ticker locTicker;
 
 //出力
-//int a=0;
-//int j=0;
 void motorOut()
 {
     for(int i=0; i<4; i++) {
@@ -214,13 +329,24 @@
     }
 }
 
+CAN_ticker canx;
+void ticker_CanRead()
+{
+    canx.can_read();
+}
+
+Location location;
+void calLoc()
+{
+    theta=gyro.getAngle()-angle;    //角度受け取り
+    location.calXY();
+}
 int main()
 {
+    angle=gyro.getAngle();//角度初期化
     can1.frequency(1000000);
     gyro.initialize();    //main関数の最初に一度だけ実行
     gyro.acc_offset();
-    double angle;
-    angle=gyro.getAngle();
     double z;
     printf("start\r\n");
     motor[0].setEquation(0.008031,-0.022300,-0.008839,-0.016290);
@@ -238,25 +364,53 @@
     motor[2].setPDparam( 0.1790, 0.00620);
     motor[3].setPDparam( 0.1680, 0.00560);
 
-    while(1) {
+    int first=0;
+    while(first==0) {
         printf("waiting\r\n");
         if(button==0) {
             wait(1);
-            ticker.attach(motorOut,0.05);
+            ticker.attach(&motorOut,0.05);
+            first++;
             break;
         }
     }
 
+    int canX=20;//can変数
     int n=1,dx,dy,aimX,aimY;
     double vx_,vy_,vx,vy,newtime,distance;
-    Location location;
     Timer time;
     time.start();
+    locTicker.attach(&calLoc,0.05);
+    canTicker.attach(&ticker_CanRead,0.1);//can読み込み
     while(1) {
+        canX=canx.get_xCAN();//０で手動化
+        if(canX==0) {//手動化
+            ticker.detach();
+            while(1) {
+                canX=canx.get_xCAN();
+                calOmega_CAN(canX);
+                //自己位置取得
+                x=location.getX();
+                y=location.getY();
+                //目的地決定(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) {
+                    n++;
+                    printf("reach%d\r\n",n);
+                    time.reset();
+                }
+                if(canX==0) {//自動化
+                    canX=20;
+                    ticker.attach(&motorOut,0.05);
+                    break;
+                }
+            }
+        }
         //自己位置取得
-        theta=gyro.getAngle()-angle;    //角度の値を受け取る
-        location.calXY();
-
         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());
@@ -287,7 +441,7 @@
 
         if(n>=5) {
             for(int j=0; j<4; j++) {
-                motor[j].Sc(0);
+                motor[j].stop();
             }
             printf("fin\r\n");
             ticker.detach();