hirromu takaki / Mbed OS moveTest

nhk 2019 syudou B

Dependencies:   R1370 gyro Controller ikarashiMDC

main.cpp@3:6633e752a5de, 2019-07-02 (annotated)

Committer:
THtakahiro702286
Date:
Tue Jul 02 09:31:00 2019 +0000
Revision:
3:6633e752a5de
Parent:
2:2b635b7e0bb6 
syudou b 2019;

Who changed what in which revision?

UserRevisionLine numberNew contents of line
THtakahiro702286 0:09ef6f968eef 1 /*
THtakahiro702286 1:c26a135080b1 2 * 改良しました
THtakahiro702286 0:09ef6f968eef 3 *
THtakahiro702286 1:c26a135080b1 4 *
THtakahiro702286 0:09ef6f968eef 5 *
THtakahiro702286 0:09ef6f968eef 6 * 何かわからないことがあったらその都度聞いて
THtakahiro702286 0:09ef6f968eef 7 */
THtakahiro702286 0:09ef6f968eef 8
THtakahiro702286 1:c26a135080b1 9 #include "mbed.h"
THtakahiro702286 1:c26a135080b1 10 #include "gyro.h"
THtakahiro702286 1:c26a135080b1 11 #include "ikarashiMDC.h"
THtakahiro702286 1:c26a135080b1 12 #include "R1370.h"
THtakahiro702286 1:c26a135080b1 13 #include "controller.h"
THtakahiro702286 1:c26a135080b1 14 #define PI 3.141593
THtakahiro702286 1:c26a135080b1 15
THtakahiro702286 1:c26a135080b1 16 Controller pad(PC_10, PC_11, 208);
THtakahiro702286 1:c26a135080b1 17 R1370 R1370(PB_6,PA_10);
THtakahiro702286 1:c26a135080b1 18 Serial pc(USBTX, USBRX, 115200);
THtakahiro702286 1:c26a135080b1 19 Serial serial(PC_6, PC_7, 115200);
THtakahiro702286 1:c26a135080b1 20 DigitalOut serialcontrol(D10);
THtakahiro702286 0:09ef6f968eef 21
THtakahiro702286 0:09ef6f968eef 22 gyro omni(4); //オムニホイールの数を入力
THtakahiro702286 0:09ef6f968eef 23
THtakahiro702286 1:c26a135080b1 24 /*ロボットの使うMDCに応じて変える事*/
THtakahiro702286 1:c26a135080b1 25 ikarashiMDC ikarashi[] = {
THtakahiro702286 1:c26a135080b1 26 ikarashiMDC(&serialcontrol,2,0,SM,&serial),
THtakahiro702286 1:c26a135080b1 27 ikarashiMDC(&serialcontrol,2,1,SM,&serial),
THtakahiro702286 1:c26a135080b1 28 ikarashiMDC(&serialcontrol,2,2,SM,&serial),
THtakahiro702286 1:c26a135080b1 29 ikarashiMDC(&serialcontrol,2,3,SM,&serial)
THtakahiro702286 1:c26a135080b1 30 };
THtakahiro702286 0:09ef6f968eef 31
THtakahiro702286 0:09ef6f968eef 32 int main()
THtakahiro702286 0:09ef6f968eef 33 {
THtakahiro702286 1:c26a135080b1 34 int b[11];//自作コントローラー関連
THtakahiro702286 1:c26a135080b1 35 double stickRad[2],stickNorn[2],raded[2],Norned[2];
THtakahiro702286 1:c26a135080b1 36
THtakahiro702286 3:6633e752a5de 37 bool b8 = 1,b10 = 1;//b[8]とb[10]のボタン判断
THtakahiro702286 1:c26a135080b1 38
THtakahiro702286 1:c26a135080b1 39 double acc[4] = {0,0,0,0},angle,control,con,func,gain = 0;//accはアクセラレート：加速の略　angleはジャイロセンサの値を入れる control,conは角度調整に必要
THtakahiro702286 2:2b635b7e0bb6 40 double idealAngle = 180,puls = 0.1;//funcは台形制御に必要 gainは台形制御の計算に使う idealAngleは角度セットに使う
THtakahiro702286 1:c26a135080b1 41
THtakahiro702286 1:c26a135080b1 42 ikarashi[0].braking = true;//必須
THtakahiro702286 1:c26a135080b1 43 /*タイヤ角を設定 必須
THtakahiro702286 1:c26a135080b1 44 omni.setRad(タイヤの番号,タイヤのついてる角度radian値)
THtakahiro702286 1:c26a135080b1 45 このプログラムでは4輪の設定をしている*/
THtakahiro702286 1:c26a135080b1 46 for(int i = 0; i < 4; i++)
THtakahiro702286 1:c26a135080b1 47 {
THtakahiro702286 1:c26a135080b1 48 omni.setRad(i, PI * (i*2+1) / 4);
THtakahiro702286 1:c26a135080b1 49 }
THtakahiro702286 1:c26a135080b1 50
THtakahiro702286 1:c26a135080b1 51 omni.setIdeal(0);//目的の角度を0度に設定 必須
THtakahiro702286 1:c26a135080b1 52
THtakahiro702286 1:c26a135080b1 53 omni.setErrorRange(2);//誤差範囲を設定 +-入力値分の誤差が出る ロボットが止まらない時は大きめに設定すること
THtakahiro702286 1:c26a135080b1 54
THtakahiro702286 1:c26a135080b1 55 omni.limitPower(0.5);//回転速度制限　ここで入力した速さよりも回転は速くならない 正の実数で１より大きくしないこと 必須
THtakahiro702286 0:09ef6f968eef 56 while(1)
THtakahiro702286 0:09ef6f968eef 57 {
THtakahiro702286 3:6633e752a5de 58 R1370.update();
THtakahiro702286 3:6633e752a5de 59 if(/*R1370.update() != 1 && */pad.receiveState() != 0)//ジャイロセンサが動いてかつコントローラーと通信していれば
THtakahiro702286 1:c26a135080b1 60 {
THtakahiro702286 1:c26a135080b1 61 angle = R1370.getAngle();//ジャイロの出した角度を保存 必須
THtakahiro702286 1:c26a135080b1 62
THtakahiro702286 1:c26a135080b1 63 for(int i = 0; i < 13; i++){
THtakahiro702286 1:c26a135080b1 64 b[i] = pad.getButton1(i);//0 = 左下 1 = 左上 2 = 十字左 3 = 上 4 = 右 5 = 下 6 = 右下 7 = L2 8 = L1 9 = R2 10 = R1 11 R3 12 = L3
THtakahiro702286 1:c26a135080b1 65 }
THtakahiro702286 1:c26a135080b1 66 for(int i = 0; i < 2; i++){
THtakahiro702286 1:c26a135080b1 67 stickRad[i] = PI - pad.getRadian(i);
THtakahiro702286 1:c26a135080b1 68 stickNorn[i] = pad.getNorm(i);
THtakahiro702286 1:c26a135080b1 69 }
THtakahiro702286 3:6633e752a5de 70 pc.printf("%d %d\n\r",b[8],b[10]);
THtakahiro702286 1:c26a135080b1 71 /*一度ボタンを押すとその方向に90度回転*/
THtakahiro702286 3:6633e752a5de 72 if(b[8] != b8)
THtakahiro702286 0:09ef6f968eef 73 {
THtakahiro702286 3:6633e752a5de 74 if(b8 == 1)
THtakahiro702286 1:c26a135080b1 75 {
THtakahiro702286 1:c26a135080b1 76 idealAngle -= 90;
THtakahiro702286 1:c26a135080b1 77 gain = 0;
THtakahiro702286 1:c26a135080b1 78 }
THtakahiro702286 3:6633e752a5de 79 b8 = b[8];
THtakahiro702286 0:09ef6f968eef 80 }
THtakahiro702286 3:6633e752a5de 81 if(b[10] != b10)
THtakahiro702286 0:09ef6f968eef 82 {
THtakahiro702286 3:6633e752a5de 83 if(b10 == 1)
THtakahiro702286 1:c26a135080b1 84 {
THtakahiro702286 1:c26a135080b1 85 idealAngle += 90;
THtakahiro702286 1:c26a135080b1 86 gain = 0;
THtakahiro702286 1:c26a135080b1 87 }
THtakahiro702286 3:6633e752a5de 88 b10 = b[10];
THtakahiro702286 0:09ef6f968eef 89 }
THtakahiro702286 1:c26a135080b1 90 if(idealAngle > 360)
THtakahiro702286 1:c26a135080b1 91 {
THtakahiro702286 1:c26a135080b1 92 idealAngle = 90;
THtakahiro702286 1:c26a135080b1 93 }
THtakahiro702286 1:c26a135080b1 94 if(idealAngle <= 0)
THtakahiro702286 0:09ef6f968eef 95 {
THtakahiro702286 1:c26a135080b1 96 idealAngle = 360;
THtakahiro702286 0:09ef6f968eef 97 }
THtakahiro702286 1:c26a135080b1 98 omni.setIdeal(idealAngle - 180);
THtakahiro702286 1:c26a135080b1 99
THtakahiro702286 1:c26a135080b1 100 //omni.cAngle(ジャイロの値)
THtakahiro702286 1:c26a135080b1 101 //ジャイロの角度を渡す　調整用の速度をもらう
THtakahiro702286 1:c26a135080b1 102 control = omni.cAngle(angle);
THtakahiro702286 1:c26a135080b1 103 //omni.pAngle(ジャイロの値,比例ゲイン) こっちは比例制御
THtakahiro702286 1:c26a135080b1 104 con = omni.pAngle(angle,2);
THtakahiro702286 1:c26a135080b1 105 /*比例ゲインは１が等倍速、２が倍速で、0.5が1/2倍速といった感じに速度が変わる
THtakahiro702286 1:c26a135080b1 106 ちょうどいい比例ゲインは試しながら見つける事
THtakahiro702286 1:c26a135080b1 107 この二つはどちらか一方のみを使うこと
THtakahiro702286 1:c26a135080b1 108 どっちも使ってみてから決めてほしい*/
THtakahiro702286 1:c26a135080b1 109
THtakahiro702286 3:6633e752a5de 110 if(fabs(raded[0] - stickRad[0]) > 2 || fabs(Norned[0] - stickNorn[0]) > 0.2)
THtakahiro702286 0:09ef6f968eef 111 {
THtakahiro702286 3:6633e752a5de 112 gain -= 0;//もしスティックが大きく動いたらgain = 0
THtakahiro702286 1:c26a135080b1 113 }
THtakahiro702286 1:c26a135080b1 114
THtakahiro702286 1:c26a135080b1 115 func = omni.lFunc(0.25,gain);//一次関数の値を取り出す omni.lFunc(比例定数,gain)
THtakahiro702286 1:c26a135080b1 116
THtakahiro702286 1:c26a135080b1 117 raded[0] = stickRad[0]; //値比較用
THtakahiro702286 1:c26a135080b1 118 Norned[0] = stickNorn[0];//
THtakahiro702286 1:c26a135080b1 119
THtakahiro702286 1:c26a135080b1 120 /* スティックが傾いていないまたはどちらも傾いていると平行移動しない*/
THtakahiro702286 1:c26a135080b1 121 if( ((stickNorn[0] == 0) && (stickNorn[1] == 0)) || ((stickNorn[0] != 0) && (stickNorn[1] != 0)) )
THtakahiro702286 1:c26a135080b1 122 {
THtakahiro702286 1:c26a135080b1 123 for(int i = 0; i < 4; i++)
THtakahiro702286 1:c26a135080b1 124 {
THtakahiro702286 1:c26a135080b1 125 acc[i] = (omni.hMove(10,i) + con) * func;
THtakahiro702286 1:c26a135080b1 126 }//+controlまたは +conで角度がずれたら修正 この時は比例制御を使った//omni.hMove(角度radian,タイヤ番号)　で速度を返す 角度が10なら平行移動しない 必須
THtakahiro702286 1:c26a135080b1 127 gain += puls;//どんどんモーターの値が大きくなる
THtakahiro702286 1:c26a135080b1 128 }
THtakahiro702286 1:c26a135080b1 129 else if(stickNorn[0] != 0 && stickNorn[1] == 0) //stick[0]だけを動かしているとき
THtakahiro702286 1:c26a135080b1 130 {
THtakahiro702286 1:c26a135080b1 131 for(int i = 0; i < 4; i++)
THtakahiro702286 1:c26a135080b1 132 {
THtakahiro702286 3:6633e752a5de 133 acc[i] = (omni.hMove(stickRad[0] + (angle * (PI / 180)), i) * stickNorn[0] + con) * func;//ロボットの向きに左右されずに動く
THtakahiro702286 1:c26a135080b1 134 }
THtakahiro702286 1:c26a135080b1 135 gain += puls;//どんどん（ry
THtakahiro702286 1:c26a135080b1 136 }
THtakahiro702286 1:c26a135080b1 137
THtakahiro702286 1:c26a135080b1 138 for(int i = 0; i < 4;i++) //モーターに出力 必須
THtakahiro702286 1:c26a135080b1 139 {
THtakahiro702286 3:6633e752a5de 140 if(acc[i] > 0.5)
THtakahiro702286 1:c26a135080b1 141 {
THtakahiro702286 3:6633e752a5de 142 acc[i] = 0.5;
THtakahiro702286 1:c26a135080b1 143 }
THtakahiro702286 3:6633e752a5de 144 else if(acc[i] < -0.5)
THtakahiro702286 1:c26a135080b1 145 {
THtakahiro702286 3:6633e752a5de 146 acc[i] = -0.5;
THtakahiro702286 1:c26a135080b1 147 }
THtakahiro702286 1:c26a135080b1 148 ikarashi[i].setSpeed(acc[i]);
THtakahiro702286 2:2b635b7e0bb6 149 //pc.printf("|%f| ",acc[i]);
THtakahiro702286 1:c26a135080b1 150 }
THtakahiro702286 3:6633e752a5de 151 pc.printf("|%.2f| ",acc[0]);
THtakahiro702286 1:c26a135080b1 152
THtakahiro702286 3:6633e752a5de 153 }
THtakahiro702286 3:6633e752a5de 154 if(/*R1370.update() != 0 || */pad.receiveState() == 0)//ジャイロ,コントローラーが動いていなかったら
THtakahiro702286 1:c26a135080b1 155 {
THtakahiro702286 3:6633e752a5de 156 pc.printf("e\n\r"); //errorを表示して動かない
THtakahiro702286 1:c26a135080b1 157
THtakahiro702286 1:c26a135080b1 158 for(int i = 0; i < 4;i++)
THtakahiro702286 1:c26a135080b1 159 {
THtakahiro702286 1:c26a135080b1 160 ikarashi[i].setSpeed(0);//モーターに0 必須
THtakahiro702286 1:c26a135080b1 161 }
THtakahiro702286 3:6633e752a5de 162 }
THtakahiro702286 0:09ef6f968eef 163 }
THtakahiro702286 0:09ef6f968eef 164 }