yosuke kitajima / Mbed 2 deprecated FourOmniMecha

sss

Dependencies:   mbed TetraOmni_verKi PID2 QEI_verKi BusSerial

FourOmniMecha.cpp@0:620b24ebccae, 2020-03-26 (annotated)

Committer:
ttrist
Date:
Thu Mar 26 04:40:39 2020 +0000
Revision:
0:620b24ebccae
aaa

Who changed what in which revision?

UserRevisionLine numberNew contents of line
ttrist 0:620b24ebccae 1 #include "mbed.h"
ttrist 0:620b24ebccae 2 #include "BusSerial.h"
ttrist 0:620b24ebccae 3 #include "MotorControler.h"
ttrist 0:620b24ebccae 4 #include "PID.h"
ttrist 0:620b24ebccae 5 #include "QEI.h"
ttrist 0:620b24ebccae 6 #include "TetraOmni.h"
ttrist 0:620b24ebccae 7
ttrist 0:620b24ebccae 8 //ハード定数
ttrist 0:620b24ebccae 9 const int PPR = 250;
ttrist 0:620b24ebccae 10 const double MaxRPS = 12;
ttrist 0:620b24ebccae 11
ttrist 0:620b24ebccae 12 //ソフト定数
ttrist 0:620b24ebccae 13 const uint8_t HeadNum = 250; //基板ID
ttrist 0:620b24ebccae 14 const double Kp=0.05,Ki=0.058,Kd=0.005; //ホイール速度PID
ttrist 0:620b24ebccae 15 const double MoveRatio=0.7,RotationRatio=0.3; //移動と回転の比
ttrist 0:620b24ebccae 16 const double DiffRange = 0.5; //Fake目標値の真目標への変化範囲
ttrist 0:620b24ebccae 17 const double StopThresholdRPS = 1.5; //目標値が0の場合，このRPSなら一瞬で出力を0にする
ttrist 0:620b24ebccae 18 const int TargetRPSStage = 100; //目標値の偏差を分割する個数
ttrist 0:620b24ebccae 19
ttrist 0:620b24ebccae 20 //変数
ttrist 0:620b24ebccae 21
ttrist 0:620b24ebccae 22 //インスタンス生成
ttrist 0:620b24ebccae 23 Timer timer;
ttrist 0:620b24ebccae 24 Ticker ticker0,ticker1,ticker2,ticker3;
ttrist 0:620b24ebccae 25 BusSerial BS(PA_0,PA_1,&timer);
ttrist 0:620b24ebccae 26 PID MOT_PID[4]= {PID(&timer),PID(&timer),PID(&timer),PID(&timer)};
ttrist 0:620b24ebccae 27 TetraOmni Omni = TetraOmni(MoveRatio,RotationRatio,MaxRPS,DiffRange);
ttrist 0:620b24ebccae 28 MotorControler MOT[4]= {MotorControler( PA_8, PC_6, PA_7),MotorControler( PA_9, PC_7, PA_7),
ttrist 0:620b24ebccae 29 MotorControler( PA_10, PC_8, PA_7),MotorControler( PA_11, PC_9, PA_7)
ttrist 0:620b24ebccae 30 };
ttrist 0:620b24ebccae 31 QEI ENC[4]= {QEI(PB_2, PA_6,PPR,&timer,&ticker0), QEI( PB_5, PB_4,PPR,&timer,&ticker1),
ttrist 0:620b24ebccae 32 QEI(PB_8, PB_7,PPR,&timer,&ticker2), QEI( PB_13, PB_12,PPR,&timer,&ticker3)
ttrist 0:620b24ebccae 33 };
ttrist 0:620b24ebccae 34
ttrist 0:620b24ebccae 35
ttrist 0:620b24ebccae 36 //プロトタイプ宣言
ttrist 0:620b24ebccae 37 void classSetting();
ttrist 0:620b24ebccae 38
ttrist 0:620b24ebccae 39 //ピン設定
ttrist 0:620b24ebccae 40 DigitalOut RBG[3]= {DigitalOut(PA_15),DigitalOut(PB_0),DigitalOut(PA_4)};
ttrist 0:620b24ebccae 41
ttrist 0:620b24ebccae 42 int main()
ttrist 0:620b24ebccae 43 {
ttrist 0:620b24ebccae 44 printf("boot\n");
ttrist 0:620b24ebccae 45 //クラス一括設定
ttrist 0:620b24ebccae 46 classSetting();
ttrist 0:620b24ebccae 47
ttrist 0:620b24ebccae 48 for(int i=0; i<4; i++)MOT[i].setSpeed(0); //初期モーター速度は1にしとく
ttrist 0:620b24ebccae 49
ttrist 0:620b24ebccae 50
ttrist 0:620b24ebccae 51 for(int i =0; i<3; i++) {
ttrist 0:620b24ebccae 52 RBG[i]=1;
ttrist 0:620b24ebccae 53 wait(1);
ttrist 0:620b24ebccae 54 RBG[i]=0;
ttrist 0:620b24ebccae 55 }
ttrist 0:620b24ebccae 56 RBG[0]=1;
ttrist 0:620b24ebccae 57
ttrist 0:620b24ebccae 58
ttrist 0:620b24ebccae 59 while(1) {
ttrist 0:620b24ebccae 60 //値直接入力によるデバッグ部分
ttrist 0:620b24ebccae 61 //uint8_t RecieveData[5] = {250,0,0,0,0b10000};
ttrist 0:620b24ebccae 62
ttrist 0:620b24ebccae 63 //データ受信
ttrist 0:620b24ebccae 64 uint8_t RecieveData[5];
ttrist 0:620b24ebccae 65 BS.GetBusSerial(&RecieveData[0],HeadNum,5,100);
ttrist 0:620b24ebccae 66
ttrist 0:620b24ebccae 67 //***データがちゃんと受信できた場合***
ttrist 0:620b24ebccae 68 if(RecieveData[0]==HeadNum) {
ttrist 0:620b24ebccae 69 //受信データ処理
ttrist 0:620b24ebccae 70 double MoveDirection = RecieveData[1]*2;
ttrist 0:620b24ebccae 71 double YawDeg = (int)RecieveData[2] << 8 | (int)RecieveData[3];
ttrist 0:620b24ebccae 72 bool StickBool = RecieveData[4] >> 4 & 0b1;
ttrist 0:620b24ebccae 73 int SpeedBool = RecieveData[4] >> 2 & 0b11;;
ttrist 0:620b24ebccae 74 int RotBool = RecieveData[4] & 0b11;
ttrist 0:620b24ebccae 75
ttrist 0:620b24ebccae 76 //オムニターゲット速度計算
ttrist 0:620b24ebccae 77 Omni.wheelTGTRPScalc(StickBool,MoveDirection,YawDeg,RotBool,SpeedBool);
ttrist 0:620b24ebccae 78 Omni.FakeRPScalc();
ttrist 0:620b24ebccae 79
ttrist 0:620b24ebccae 80 //モーター回転
ttrist 0:620b24ebccae 81 for(int i =0; i<4; i++) {
ttrist 0:620b24ebccae 82 double ThisWheelRPS = ENC[i].getRPS();
ttrist 0:620b24ebccae 83 if(Omni.Wheel_FakeTGTRPS(i)==0 && fabs(ThisWheelRPS)<StopThresholdRPS) MOT[i].setSpeed(0);
ttrist 0:620b24ebccae 84 else MOT[i].setSpeed(MOT_PID[i].controlPID(Omni.Wheel_FakeTGTRPS(i),ThisWheelRPS));
ttrist 0:620b24ebccae 85
ttrist 0:620b24ebccae 86 }
ttrist 0:620b24ebccae 87
ttrist 0:620b24ebccae 88 //デバッグエリア
ttrist 0:620b24ebccae 89 //for(int i =0; i<5; i++) printf("%d ",RecieveData[i]); //受信データ確認
ttrist 0:620b24ebccae 90 //for(int i =0; i<4; i++) printf("%.2f ",ENC[i].getRPS());//ホイールの現在のPRS確認
ttrist 0:620b24ebccae 91 //for(int i =0; i<4; i++) printf("%.2f ",Omni.Wheel_TrueTGTRPS(i));//ホイールの真目標のPRS確認
ttrist 0:620b24ebccae 92 //for(int i =0; i<4; i++) printf("%.2f ",Omni.Wheel_FakeTGTRPS(i));//ホイールの偽目標のPRS確認
ttrist 0:620b24ebccae 93
ttrist 0:620b24ebccae 94 //for(int i =0; i<4; i++) printf("%.2f -> %.2f | ",ENC[i].getRPS(),Omni.Wheel_TrueTGTRPS(i));//ホイールの現在のPRS確認
ttrist 0:620b24ebccae 95 //printf("\n");//改行
ttrist 0:620b24ebccae 96 }
ttrist 0:620b24ebccae 97 }
ttrist 0:620b24ebccae 98 }
ttrist 0:620b24ebccae 99
ttrist 0:620b24ebccae 100
ttrist 0:620b24ebccae 101
ttrist 0:620b24ebccae 102 /*********************************************
ttrist 0:620b24ebccae 103 **********************************************
ttrist 0:620b24ebccae 104 ******************関数*************************
ttrist 0:620b24ebccae 105 **********************************************
ttrist 0:620b24ebccae 106 **********************************************/
ttrist 0:620b24ebccae 107
ttrist 0:620b24ebccae 108 void classSetting()
ttrist 0:620b24ebccae 109 {
ttrist 0:620b24ebccae 110 //Omni.SpeedStageSetting(20,35,50);
ttrist 0:620b24ebccae 111 for(int i =0; i<4; i++) {
ttrist 0:620b24ebccae 112 //MotorControler設定
ttrist 0:620b24ebccae 113 MOT[i].setMotorDirection(CW); //setspeedの正の値の回転方向を決める．CWでDIR=0,CCWでDIR=1の動き
ttrist 0:620b24ebccae 114 MOT[i].setPwmFrequency(20000); //PWM周波数
ttrist 0:620b24ebccae 115 MOT[i].enableDriver(); //モータ起動
ttrist 0:620b24ebccae 116
ttrist 0:620b24ebccae 117 //QEI設定
ttrist 0:620b24ebccae 118 ENC[i].rotReverce();
ttrist 0:620b24ebccae 119 ENC[i].useAvePRS(); //ticker使用
ttrist 0:620b24ebccae 120
ttrist 0:620b24ebccae 121 //***PID設定***
ttrist 0:620b24ebccae 122 MOT_PID[i].setParameter(Kp,Ki,Kd);
ttrist 0:620b24ebccae 123 }
ttrist 0:620b24ebccae 124 Omni.TGTRPS_SwitchingStage = TargetRPSStage;//デフォはclass中で75
ttrist 0:620b24ebccae 125 }