Revision 3:73a1daf5ed2b, committed 21 months ago

Comitter:

yn_4009

Date:

Sat Sep 17 11:23:52 2022 +0000

Parent:

2:81b5ecedbae8

Commit message:

a

Changed in this revision

--- a/syudouki_sensor.cpp	Thu Sep 15 06:39:45 2022 +0000
+++ b/syudouki_sensor.cpp	Sat Sep 17 11:23:52 2022 +0000
@@ -8,6 +8,19 @@
 
 #define RESOLUTION 2048 //AMT102の分解能は2048
 #define AIMTHETA_MAX 120
+
+
+const int PS_CAN_ID = 0x12; // 自身のCAN ID(0~2047の間の好きな数値．）
+const int CAN_Hz = 1000000;//CANに使用するクロック周波数[Hz]. CAN通信相手と共通させる
+const int TARGET_CAN_ID = 0x10; //データを届けたい相手のCAN ID
+const int CAN_LENGTH = 4; //送信データの配列数.最高8まで
+CAN can(p30, p29);//CAN_RD, CAN_TDの順
+
+
+int8_t RX, RY, LX, LY;
+uint8_t R2, L2, up, dwn, lft, rit, sqr, crss, cicl, tri, R1, L1, R3, L3, shr, opt, PSb, touc;
+
+
 const float Dimameter_sokuteiWheel = 47.15; //測定輪直径[mm]
 const int Teibai = 4; //エンコーダの逓倍
 // const double r_sokuteiWheel = 40.0; //機体回転中心から測定輪の車軸までの長さ[mm]
@@ -26,42 +39,53 @@
 
 BNO055 BNO055(p28,p27); //ジャイロ(SDA,SCL)
 //ジャイロからフィールド座標に対する機体の回転[rad]を求める
-double getGyroYaw(){
-    //BNO055(yaw 時計回りに0~360[degree])
-    double Y_ = BNO055.euler.yaw;
-    if(Y_>180) Y_-=360;
-    return Y_; //-180~180[degree]
-}
+
 //割り込みタイマー
 Ticker ticker;
 
 //機体の回転速度のPID 前3つが係数なのでこれを調整 P→D→Iの順
 CalPID aimtheta_pid(0.0,0.0,0.0,DELTA_T,AIMTHETA_MAX);
 
+const double pi = atan(1.0)*4;
 //機体のフィールド座標系での移動速度の目標
 double f_vx = 0;
 double f_vy = 0;
-//機体の角度
+//機体の角度(度)
 double theta = 0;
+//機体の角度（ラジアン）
+double rad_theta = 0;
 //機体の回転角度の目標
 double target_theta=0;
 //機体の機体座標系での移動速度の目標
 double vx = 0;
 double vy = 0;
-//機体の回転速度[degree/s] 時計回りが正
+//足回りへ送る機体の回転速度[degree/s] 時計回りが正
 double aimtheta = 0;
 
+//コントローラーからの機体の回転速度指令
+double ctrl_aimtheta = 0;
+
+//機体の最高速度（mm/s)
+double maxspeed = 511;
+
 void saveData();
 void timercallback();
 void displayData();
 //機体の移動速度をフィールド座標系から機体座標系に変換
 void field_kitai();
+//コントローラー情報を受信、データ変換
+void unzipControl();
+//足回りへの送信処理
+void asimawari_can_send();
+double getGyroYaw();
 
 int main ()
 {
-    //waitしている間に緊急停止スイッチ回す
-    printf("\r\n");
-    wait(3);
+    BNO055.reset();
+    wait(0.5);
+    can.frequency(CAN_Hz);//CANのクロック周波数設定
+    can.filter(PS_CAN_ID, 0xFFF, CANStandard, 0);////MY_CAN_ID以外のデータを受け取らないよう設定．後半の0xFFF, CANStandard, 0);は基本変えない
+
     //STARTが書かれた時点で開始する
     printf("START\r\n");
     //機体のフィールド座標系での移動速度を設定
@@ -71,10 +95,13 @@
     target_theta=0;
     //割り込みタイマーをオンにする
     ticker.attach(&timercallback,DELTA_T);
+    /*
     wait(10);
     ticker.detach();
     displayData();
-    printf("\r\n");
+    */
+    while(1) {
+    }
 }
 
 void saveData()
@@ -89,12 +116,25 @@
         data_count++;
     }
 }
+
 void timercallback() //自己位置を取得し,一定の間隔で配列に保存
 {
     theta = getGyroYaw(); //初期状態の角度からのずれを計算 [rad]
-
+    printf("theta:%11lf, ",theta);
+    unzipControl();
     field_kitai();
 
+    aimtheta = ctrl_aimtheta;
+
+    printf("vx:%11lf, ",vx);
+    printf("vy:%11lf, ",vy);
+    printf("f_vx:%11lf, ",f_vx);
+    printf("f_vy:%11lf, ",f_vy);
+    printf("aimtheta:%11lf, ",aimtheta);
+
+    asimawari_can_send();
+    printf("\r\n");
+        
     double rotate = target_theta-theta;
     double aimtheta = aimtheta_pid.calPID(rotate);
     if(save_count>=SAVE_COUNT_THRESHOLD) {
@@ -102,6 +142,7 @@
         save_count=0;
     }
 }
+
 void displayData()
 {
     printf("fvx\r\n");
@@ -136,8 +177,135 @@
     }
     data_count=0;
 }
-void field_kitai() 
+
+void field_kitai()
+{
+    rad_theta = theta*(pi/180);
+
+    vx = f_vx*cos(rad_theta) - f_vy*sin(rad_theta);
+    vy = f_vx*sin(rad_theta) + f_vy*cos(rad_theta);
+
+    if(vx < -(maxspeed)) {
+        vx = -(maxspeed);
+    }
+    if(vx > maxspeed) {
+        vx = maxspeed;
+    }
+    if(vy < -(maxspeed)) {
+        vy = -(maxspeed);
+    }
+    if(vy > maxspeed) {
+        vy = maxspeed;
+    }
+}
+
+void asimawari_can_send()
+{
+    if(PSb) {
+        vx=0;
+        vy=0;
+        aimtheta=0;
+    }
+
+    char can_data[CAN_LENGTH] = {};//送りたいデータを入れる箱
+    can_data[0] = vx/4 + 128; //データを入れる. charは8bitなので，ビットシフト等で1要素8bit以内に収める
+    can_data[1] = vy/4 + 128;
+    can_data[2] = aimtheta + 128;
+    can_data[3] = theta/2 +128;
+    CANMessage msg(TARGET_CAN_ID,can_data,CAN_LENGTH);//CANプロトコルに変換
+    bool result = can.write(msg);//CAN 送信
+    if(result == true) {
+        printf("send data");
+    } else {
+        printf("fail");
+    }
+}
+
+void unzipControl()
 {
-    vx = f_vx*cos(theta) - f_vy*sin(theta);
-    vy = f_vx*sin(theta) + f_vy*cos(theta);
+    CANMessage msg;// 送られてきたデータを入れる箱
+
+    if(can.read(msg)) {
+        ;//CANデータの読み取り
+
+
+        RX = msg.data[0] - 128; //<-128~127>に変換
+        RY = msg.data[1] - 128;
+        LX = msg.data[2] - 128;
+        LY = msg.data[3] - 128;
+        R2 = msg.data[4]; //トリガ値 <0~255>
+        L2 = msg.data[5];
+        up = (msg.data[6] & 0b10000000) >> 7;    //rx_buf[6]の値はボタン8個の値を合成したものだから、ビットシフトで1桁に解凍
+        dwn = (msg.data[6] & 0b01000000) >> 6;
+        lft = (msg.data[6] & 0b00100000) >> 5;
+        rit = (msg.data[6] & 0b00010000) >> 4;
+        sqr = (msg.data[6] & 0b00001000) >> 3;
+        crss = (msg.data[6] & 0b00000100) >> 2;
+        cicl = (msg.data[6] & 0b00000010) >> 1;
+        tri = msg.data[6] & 0b00000001;
+        R1 = (msg.data[7] & 0b10000000) >> 7;
+        L1 = (msg.data[7] & 0b01000000) >> 6;
+        R3 = (msg.data[7] & 0b00100000) >> 5;
+        L3 = (msg.data[7] & 0b00010000) >> 4;
+        shr = (msg.data[7] & 0b00001000) >> 3;
+        opt = (msg.data[7] & 0b00000100) >> 2;
+        PSb = (msg.data[7] & 0b00000010) >> 1;
+        touc = msg.data[7] & 0b00000001;
+
+        if(-10 < LX && LX <10) {
+            LX = 0;
+        }
+
+        if(-10 < LY && LY < 10) {
+            LY = 0;
+        }
+
+        f_vx = LX*4;
+        f_vy = LY*4;
+        ctrl_aimtheta = (R2 - L2)/3;
+
+
+
+        /*
+           printf("LX:%4d, ",LX);
+           printf("LY:%4d, ",LY);
+           printf("RX:%4d, ",RX);
+           printf("RY:%4d, ",RY);
+           printf("L2:%3d, ",L2);
+           printf("R2:%3d, ",R2);
+           printf("L1:%d, ",L1);
+           printf("R1:%d, ",R1);
+           printf("L3:%d, ",L3);
+           printf("R3:%d, ",R3);
+           printf("↑:%d, ",up);
+           printf("↓:%d, ",dwn);
+           printf("←:%d, ",lft);
+           printf("→:%d, ",rit);
+           printf("□:%d, ",sqr);
+           printf("×:%d, ",crss);
+           printf("〇:%d, ",cicl);
+           printf("△:%d, ",tri);
+           printf("shr:%d, ",shr);
+           printf("opt:%d, ",opt);
+           printf("PSb:%d, ",PSb);
+           printf("touc:%d, ",touc);
+           printf("\r\n");
+        */
+
+        //  hypot(x,y);
+    }
+    printf("LX:%4d, ",LX);
+    printf("LY:%4d, ",LY);
+    printf("ctrl_aimtheta:%11lf, ",ctrl_aimtheta);
 }
+
+double getGyroYaw()
+{
+    BNO055.setmode(OPERATION_MODE_NDOF);
+    BNO055.get_calib();
+    BNO055.get_angles();
+    //BNO055(yaw 時計回りに0~360[degree])
+    double Y_ = BNO055.euler.yaw;
+    if(Y_ > 180) Y_ -= 360;
+    return Y_; //-180~180[degree]
+}
\ No newline at end of file