hirromu takaki / Mbed OS moveTest

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?

UserRevisionLine numberNew 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 }