Revision 0:620b24ebccae, committed 2020-03-26

Comitter:

ttrist

Date:

Thu Mar 26 04:40:39 2020 +0000

Commit message:

aaa

Changed in this revision

diff -r 000000000000 -r 620b24ebccae BusSerial.lib
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/BusSerial.lib	Thu Mar 26 04:40:39 2020 +0000
@@ -0,0 +1,1 @@
+https://os.mbed.com/users/ttrist/code/BusSerial/#8eba674c3e88

diff -r 000000000000 -r 620b24ebccae FourOmniMecha.cpp
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/FourOmniMecha.cpp	Thu Mar 26 04:40:39 2020 +0000
@@ -0,0 +1,125 @@
+#include "mbed.h"
+#include "BusSerial.h"
+#include "MotorControler.h"
+#include "PID.h"
+#include "QEI.h"
+#include "TetraOmni.h"
+
+//ハード定数
+const int PPR = 250;
+const double MaxRPS = 12;
+
+//ソフト定数
+const uint8_t HeadNum = 250;                        //基板ID
+const double Kp=0.05,Ki=0.058,Kd=0.005;             //ホイール速度PID
+const double MoveRatio=0.7,RotationRatio=0.3;       //移動と回転の比
+const double DiffRange = 0.5;                       //Fake目標値の真目標への変化範囲
+const double StopThresholdRPS = 1.5;                //目標値が0の場合，このRPSなら一瞬で出力を0にする
+const int TargetRPSStage = 100;                      //目標値の偏差を分割する個数
+
+//変数
+
+//インスタンス生成
+Timer timer;
+Ticker ticker0,ticker1,ticker2,ticker3;
+BusSerial BS(PA_0,PA_1,&timer);
+PID MOT_PID[4]= {PID(&timer),PID(&timer),PID(&timer),PID(&timer)};
+TetraOmni Omni = TetraOmni(MoveRatio,RotationRatio,MaxRPS,DiffRange);
+MotorControler MOT[4]= {MotorControler( PA_8, PC_6, PA_7),MotorControler( PA_9, PC_7, PA_7),
+                        MotorControler( PA_10, PC_8, PA_7),MotorControler( PA_11, PC_9, PA_7)
+                       };
+QEI ENC[4]= {QEI(PB_2, PA_6,PPR,&timer,&ticker0), QEI( PB_5, PB_4,PPR,&timer,&ticker1),
+             QEI(PB_8, PB_7,PPR,&timer,&ticker2), QEI( PB_13, PB_12,PPR,&timer,&ticker3)
+            };
+
+
+//プロトタイプ宣言
+void classSetting();
+
+//ピン設定
+DigitalOut RBG[3]= {DigitalOut(PA_15),DigitalOut(PB_0),DigitalOut(PA_4)};
+
+int main()
+{
+    printf("boot\n");
+    //クラス一括設定
+    classSetting();
+
+    for(int i=0; i<4; i++)MOT[i].setSpeed(0);  //初期モーター速度は1にしとく
+
+
+    for(int i =0; i<3; i++) {
+        RBG[i]=1;
+        wait(1);
+        RBG[i]=0;
+    }
+    RBG[0]=1;
+
+
+    while(1) {
+        //値直接入力によるデバッグ部分
+        //uint8_t RecieveData[5] = {250,0,0,0,0b10000};
+
+        //データ受信
+        uint8_t RecieveData[5];
+        BS.GetBusSerial(&RecieveData[0],HeadNum,5,100);
+
+        //***データがちゃんと受信できた場合***
+        if(RecieveData[0]==HeadNum) {
+            //受信データ処理
+            double MoveDirection = RecieveData[1]*2;
+            double YawDeg = (int)RecieveData[2] << 8 | (int)RecieveData[3];
+            bool StickBool = RecieveData[4] >> 4 & 0b1;
+            int SpeedBool = RecieveData[4] >> 2 & 0b11;;
+            int RotBool = RecieveData[4] & 0b11;
+
+            //オムニターゲット速度計算
+            Omni.wheelTGTRPScalc(StickBool,MoveDirection,YawDeg,RotBool,SpeedBool);
+            Omni.FakeRPScalc();
+
+            //モーター回転
+            for(int i =0; i<4; i++) {
+                double ThisWheelRPS = ENC[i].getRPS();
+                if(Omni.Wheel_FakeTGTRPS(i)==0 && fabs(ThisWheelRPS)<StopThresholdRPS) MOT[i].setSpeed(0);
+                else MOT[i].setSpeed(MOT_PID[i].controlPID(Omni.Wheel_FakeTGTRPS(i),ThisWheelRPS));
+
+            }
+
+            //デバッグエリア
+            //for(int i =0; i<5; i++)   printf("%d  ",RecieveData[i]); //受信データ確認
+            //for(int i =0; i<4; i++) printf("%.2f  ",ENC[i].getRPS());//ホイールの現在のPRS確認
+            //for(int i =0; i<4; i++) printf("%.2f  ",Omni.Wheel_TrueTGTRPS(i));//ホイールの真目標のPRS確認
+            //for(int i =0; i<4; i++) printf("%.2f  ",Omni.Wheel_FakeTGTRPS(i));//ホイールの偽目標のPRS確認
+
+            //for(int i =0; i<4; i++) printf("%.2f -> %.2f  | ",ENC[i].getRPS(),Omni.Wheel_TrueTGTRPS(i));//ホイールの現在のPRS確認
+            //printf("\n");//改行
+        }
+    }
+}
+
+
+
+/*********************************************
+**********************************************
+******************関数*************************
+**********************************************
+**********************************************/
+
+void classSetting()
+{
+    //Omni.SpeedStageSetting(20,35,50);
+    for(int i =0; i<4; i++) {
+        //MotorControler設定
+        MOT[i].setMotorDirection(CW); //setspeedの正の値の回転方向を決める．CWでDIR=0,CCWでDIR=1の動き
+        MOT[i].setPwmFrequency(20000);  //PWM周波数
+        MOT[i].enableDriver();  //モータ起動
+
+        //QEI設定
+        ENC[i].rotReverce();
+        ENC[i].useAvePRS();  //ticker使用
+
+        //***PID設定***
+        MOT_PID[i].setParameter(Kp,Ki,Kd);
+    }
+    Omni.TGTRPS_SwitchingStage = TargetRPSStage;//デフォはclass中で75
+}
\ No newline at end of file

diff -r 000000000000 -r 620b24ebccae MotorControler.lib
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/MotorControler.lib	Thu Mar 26 04:40:39 2020 +0000
@@ -0,0 +1,1 @@
+https://os.mbed.com/users/sgrsn/code/MotorControler/#543ff0150de1

diff -r 000000000000 -r 620b24ebccae PID2.lib
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/PID2.lib	Thu Mar 26 04:40:39 2020 +0000
@@ -0,0 +1,1 @@
+https://os.mbed.com/users/sgrsn/code/PID2/#873985df821c

diff -r 000000000000 -r 620b24ebccae QEI_verKi.lib
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/QEI_verKi.lib	Thu Mar 26 04:40:39 2020 +0000
@@ -0,0 +1,1 @@
+https://os.mbed.com/users/ttrist/code/QEI_verKi/#4f69bb55ab4a

diff -r 000000000000 -r 620b24ebccae TetraOmni_verKi.lib
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/TetraOmni_verKi.lib	Thu Mar 26 04:40:39 2020 +0000
@@ -0,0 +1,1 @@
+https://os.mbed.com/users/ttrist/code/TetraOmni_verKi/#7c3745bfc8ae

diff -r 000000000000 -r 620b24ebccae mbed.bld
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/mbed.bld	Thu Mar 26 04:40:39 2020 +0000
@@ -0,0 +1,1 @@
+https://os.mbed.com/users/mbed_official/code/mbed/builds/65be27845400
\ No newline at end of file