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?
User | Revision | Line number | New 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);//回転速度制限 ここで入力した速さよりも回転は速くならない 正の実数で1より大きくしないこと 必須 |
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 | /*比例ゲインは1が等倍速、2が倍速で、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 | } |