nhk 2019 syudou B
Dependencies: R1370 gyro Controller ikarashiMDC
main.cpp@0:09ef6f968eef, 2019-06-10 (annotated)
- Committer:
- THtakahiro702286
- Date:
- Mon Jun 10 14:25:27 2019 +0000
- Revision:
- 0:09ef6f968eef
- Child:
- 1:c26a135080b1
nhk2019 syudou .h file and explanation.
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
THtakahiro702286 | 0:09ef6f968eef | 1 | /* |
THtakahiro702286 | 0:09ef6f968eef | 2 | * // が縦に連続して並んでいたらそこはひとまとまりになっていると考えて |
THtakahiro702286 | 0:09ef6f968eef | 3 | * |
THtakahiro702286 | 0:09ef6f968eef | 4 | * 変数名は変えていいし、タイマー関連で テストで使った と書いてあるものは必要ない |
THtakahiro702286 | 0:09ef6f968eef | 5 | * |
THtakahiro702286 | 0:09ef6f968eef | 6 | * 何かわからないことがあったらその都度聞いて |
THtakahiro702286 | 0:09ef6f968eef | 7 | */ |
THtakahiro702286 | 0:09ef6f968eef | 8 | |
THtakahiro702286 | 0:09ef6f968eef | 9 | #include "mbed.h" // |
THtakahiro702286 | 0:09ef6f968eef | 10 | #include "gyro.h" // |
THtakahiro702286 | 0:09ef6f968eef | 11 | #include "ikarashiMDC.h" // |
THtakahiro702286 | 0:09ef6f968eef | 12 | #include "R1370.h" // 必須 |
THtakahiro702286 | 0:09ef6f968eef | 13 | Serial pc(USBTX, USBRX, 115200); // |
THtakahiro702286 | 0:09ef6f968eef | 14 | Serial serial(PC_6,PC_7); // |
THtakahiro702286 | 0:09ef6f968eef | 15 | DigitalOut serialcontrol(D2); // |
THtakahiro702286 | 0:09ef6f968eef | 16 | R1370 R1370(PB_6,PA_10); // |
THtakahiro702286 | 0:09ef6f968eef | 17 | |
THtakahiro702286 | 0:09ef6f968eef | 18 | gyro omni(4); //オムニホイールの数を入力 |
THtakahiro702286 | 0:09ef6f968eef | 19 | |
THtakahiro702286 | 0:09ef6f968eef | 20 | Timer rad,spin; //これは試すときに使ったやつ |
THtakahiro702286 | 0:09ef6f968eef | 21 | |
THtakahiro702286 | 0:09ef6f968eef | 22 | ikarashiMDC ikarashi[] { // |
THtakahiro702286 | 0:09ef6f968eef | 23 | ikarashiMDC(&serialcontrol,2,0,SM,&serial), // |
THtakahiro702286 | 0:09ef6f968eef | 24 | ikarashiMDC(&serialcontrol,2,1,SM,&serial), // |
THtakahiro702286 | 0:09ef6f968eef | 25 | ikarashiMDC(&serialcontrol,2,2,SM,&serial), //ロボットの使うMDCに応じて変える事 |
THtakahiro702286 | 0:09ef6f968eef | 26 | ikarashiMDC(&serialcontrol,2,3,SM,&serial) // |
THtakahiro702286 | 0:09ef6f968eef | 27 | }; // |
THtakahiro702286 | 0:09ef6f968eef | 28 | |
THtakahiro702286 | 0:09ef6f968eef | 29 | int main() |
THtakahiro702286 | 0:09ef6f968eef | 30 | { |
THtakahiro702286 | 0:09ef6f968eef | 31 | double acc[4] = {0,0,0,0},angle,control; //accはアクセラレート:加速の略 angleはジャイロセンサの値を入れる controlは角度調整に必要 |
THtakahiro702286 | 0:09ef6f968eef | 32 | |
THtakahiro702286 | 0:09ef6f968eef | 33 | serial.baud(115200); // |
THtakahiro702286 | 0:09ef6f968eef | 34 | ikarashi[0].braking = true; //必須 |
THtakahiro702286 | 0:09ef6f968eef | 35 | |
THtakahiro702286 | 0:09ef6f968eef | 36 | for(int i = 0; i < 4; i++) //タイヤ角を設定 必須 |
THtakahiro702286 | 0:09ef6f968eef | 37 | { //omni.setRad(タイヤの番号,タイヤのついてる角度radian値) |
THtakahiro702286 | 0:09ef6f968eef | 38 | omni.setRad(i, PI * (i*2+1) / 4); //このプログラムでは4輪の設定をしている |
THtakahiro702286 | 0:09ef6f968eef | 39 | } //もし4輪ならこのままでいい |
THtakahiro702286 | 0:09ef6f968eef | 40 | |
THtakahiro702286 | 0:09ef6f968eef | 41 | rad.start(); //テストするときに使ったやつ |
THtakahiro702286 | 0:09ef6f968eef | 42 | spin.start(); // |
THtakahiro702286 | 0:09ef6f968eef | 43 | |
THtakahiro702286 | 0:09ef6f968eef | 44 | omni.setIdeal(0); //目的の角度を0度に設定 必須 |
THtakahiro702286 | 0:09ef6f968eef | 45 | while(1) |
THtakahiro702286 | 0:09ef6f968eef | 46 | { |
THtakahiro702286 | 0:09ef6f968eef | 47 | if(R1370.update() == 0){ //ジャイロセンサが動いていれば |
THtakahiro702286 | 0:09ef6f968eef | 48 | angle = R1370.getAngle(); //ジャイロの出した角度を保存 必須 |
THtakahiro702286 | 0:09ef6f968eef | 49 | |
THtakahiro702286 | 0:09ef6f968eef | 50 | if(spin < 5) //ここから |
THtakahiro702286 | 0:09ef6f968eef | 51 | { |
THtakahiro702286 | 0:09ef6f968eef | 52 | omni.setIdeal(0); //目的の角度を0度に設定 |
THtakahiro702286 | 0:09ef6f968eef | 53 | } |
THtakahiro702286 | 0:09ef6f968eef | 54 | else if(spin < 10) |
THtakahiro702286 | 0:09ef6f968eef | 55 | { |
THtakahiro702286 | 0:09ef6f968eef | 56 | omni.setIdeal(90); //90度に設定 |
THtakahiro702286 | 0:09ef6f968eef | 57 | } |
THtakahiro702286 | 0:09ef6f968eef | 58 | else if (spin < 15) |
THtakahiro702286 | 0:09ef6f968eef | 59 | { |
THtakahiro702286 | 0:09ef6f968eef | 60 | omni.setIdeal(180);//180度に設定 |
THtakahiro702286 | 0:09ef6f968eef | 61 | } |
THtakahiro702286 | 0:09ef6f968eef | 62 | else if (spin < 20) |
THtakahiro702286 | 0:09ef6f968eef | 63 | { |
THtakahiro702286 | 0:09ef6f968eef | 64 | omni.setIdeal(-90); //-90度に設定 設定範囲は180 ~ -179 |
THtakahiro702286 | 0:09ef6f968eef | 65 | } |
THtakahiro702286 | 0:09ef6f968eef | 66 | else if (spin < 25) |
THtakahiro702286 | 0:09ef6f968eef | 67 | { |
THtakahiro702286 | 0:09ef6f968eef | 68 | omni.setIdeal(45); //45度に設定 |
THtakahiro702286 | 0:09ef6f968eef | 69 | } |
THtakahiro702286 | 0:09ef6f968eef | 70 | else |
THtakahiro702286 | 0:09ef6f968eef | 71 | { |
THtakahiro702286 | 0:09ef6f968eef | 72 | spin.reset(); |
THtakahiro702286 | 0:09ef6f968eef | 73 | } //ここまでテストで使ったやつ 条件式は不要だが角度設定で必要 |
THtakahiro702286 | 0:09ef6f968eef | 74 | |
THtakahiro702286 | 0:09ef6f968eef | 75 | control = omni.cAngle(angle); //ジャイロの角度を渡す 調整用の速度をもらう 必須 |
THtakahiro702286 | 0:09ef6f968eef | 76 | |
THtakahiro702286 | 0:09ef6f968eef | 77 | if(rad > (PI * 2 + 1)) // |
THtakahiro702286 | 0:09ef6f968eef | 78 | { // |
THtakahiro702286 | 0:09ef6f968eef | 79 | rad.reset(); //テストで使ったやつ |
THtakahiro702286 | 0:09ef6f968eef | 80 | } // |
THtakahiro702286 | 0:09ef6f968eef | 81 | |
THtakahiro702286 | 0:09ef6f968eef | 82 | for(int i = 0; i < 4; i++) // |
THtakahiro702286 | 0:09ef6f968eef | 83 | { // |
THtakahiro702286 | 0:09ef6f968eef | 84 | acc[i] = omni.hMove(10,i) + control; //omni.hMove(角度radian,タイヤ番号) で速度を返す 角度が10なら平行移動しない 必須 |
THtakahiro702286 | 0:09ef6f968eef | 85 | } //+controlで角度がずれたら修正 |
THtakahiro702286 | 0:09ef6f968eef | 86 | |
THtakahiro702286 | 0:09ef6f968eef | 87 | for(int i = 0; i < 4;i++) // |
THtakahiro702286 | 0:09ef6f968eef | 88 | { //モーターに出力 必須 |
THtakahiro702286 | 0:09ef6f968eef | 89 | ikarashi[i].setSpeed(acc[i]); // |
THtakahiro702286 | 0:09ef6f968eef | 90 | } // |
THtakahiro702286 | 0:09ef6f968eef | 91 | |
THtakahiro702286 | 0:09ef6f968eef | 92 | |
THtakahiro702286 | 0:09ef6f968eef | 93 | pc.printf("%f ",angle); //ジャイロセンサの出してる角度を表示 |
THtakahiro702286 | 0:09ef6f968eef | 94 | |
THtakahiro702286 | 0:09ef6f968eef | 95 | pc.printf("%f,%f,%f,%f\r\n",acc[0],acc[1],acc[2],acc[3]); //各モーターの出力を表示 |
THtakahiro702286 | 0:09ef6f968eef | 96 | } |
THtakahiro702286 | 0:09ef6f968eef | 97 | else //ジャイロが動いていなかったら |
THtakahiro702286 | 0:09ef6f968eef | 98 | { // |
THtakahiro702286 | 0:09ef6f968eef | 99 | pc.printf("error"); //errorを表示して動かない |
THtakahiro702286 | 0:09ef6f968eef | 100 | } // |
THtakahiro702286 | 0:09ef6f968eef | 101 | } |
THtakahiro702286 | 0:09ef6f968eef | 102 | } |