NagaokaRoboticsClub_mbedTeam / Mbed 2 deprecated 2022kouroboBv2

2022交流ロボコン用　バージョン2

Dependencies:   mbed 2021Bcon ikarashiMDC_2byte_ver

main.cpp@0:ca9732a3ae5c, 2022-03-27 (annotated)

Committer:
umekou
Date:
Sun Mar 27 02:33:51 2022 +0000
Revision:
0:ca9732a3ae5c
first commit;

Who changed what in which revision?

UserRevisionLine numberNew contents of line
umekou 0:ca9732a3ae5c 1 #include "mbed.h"
umekou 0:ca9732a3ae5c 2 #include "ikarashiMDC.h"
umekou 0:ca9732a3ae5c 3 #include "controller.h"
umekou 0:ca9732a3ae5c 4 #include <math.h>
umekou 0:ca9732a3ae5c 5
umekou 0:ca9732a3ae5c 6 DigitalOut stop(D13);
umekou 0:ca9732a3ae5c 7 Bcon mycon(PA_9,PA_10,004);//FEP004はコントローラーについてる方の番号
umekou 0:ca9732a3ae5c 8 Serial serial(PC_10,PC_11,115200);
umekou 0:ca9732a3ae5c 9 Serial pc(USBTX,USBRX,115200);
umekou 0:ca9732a3ae5c 10
umekou 0:ca9732a3ae5c 11 ikarashiMDC motor[] = {
umekou 0:ca9732a3ae5c 12 ikarashiMDC(0,0,SM,&serial),
umekou 0:ca9732a3ae5c 13 ikarashiMDC(0,1,SM,&serial),
umekou 0:ca9732a3ae5c 14 ikarashiMDC(0,2,SM,&serial),
umekou 0:ca9732a3ae5c 15 ikarashiMDC(0,3,SM,&serial),
umekou 0:ca9732a3ae5c 16 ikarashiMDC(1,0,SM,&serial),
umekou 0:ca9732a3ae5c 17 ikarashiMDC(1,1,SM,&serial),
umekou 0:ca9732a3ae5c 18 ikarashiMDC(1,2,SM,&serial)
umekou 0:ca9732a3ae5c 19 //ikarashiMDC(1,3,SM,&serial)
umekou 0:ca9732a3ae5c 20 };
umekou 0:ca9732a3ae5c 21
umekou 0:ca9732a3ae5c 22
umekou 0:ca9732a3ae5c 23 int main()
umekou 0:ca9732a3ae5c 24 {
umekou 0:ca9732a3ae5c 25 mycon.StartReceive();
umekou 0:ca9732a3ae5c 26 stop=1;
umekou 0:ca9732a3ae5c 27 uint8_t b[8];//ボタン　b[0]~b[7]の順に　左 黄色 青 赤 緑　右 赤 青 黄色 緑
umekou 0:ca9732a3ae5c 28 int16_t stick[4];//スティック
umekou 0:ca9732a3ae5c 29 double speed[7] = {0,0,0,0,0,0,0};//モータースピード
umekou 0:ca9732a3ae5c 30 double x,y;
umekou 0:ca9732a3ae5c 31 int i;
umekou 0:ca9732a3ae5c 32 while(1) {
umekou 0:ca9732a3ae5c 33 for (i=0; i<8; i++) b[i] = mycon.getButton(i);
umekou 0:ca9732a3ae5c 34 for (i=0; i<4; i++) stick[i] = mycon.getStick(i);
umekou 0:ca9732a3ae5c 35 x=stick[0];
umekou 0:ca9732a3ae5c 36 y=stick[1];
umekou 0:ca9732a3ae5c 37 if(x>20||x<-20||y>20||y<-20){
umekou 0:ca9732a3ae5c 38 for(i=0; i < 4; i++){
umekou 0:ca9732a3ae5c 39 speed[i] = sin(atan2(y,x)-3.14*(i*2-1) / 4);
umekou 0:ca9732a3ae5c 40 }
umekou 0:ca9732a3ae5c 41 } else {
umekou 0:ca9732a3ae5c 42 for(i=0; i < 4; i++){
umekou 0:ca9732a3ae5c 43 speed[i] = 0;
umekou 0:ca9732a3ae5c 44 }
umekou 0:ca9732a3ae5c 45 }
umekou 0:ca9732a3ae5c 46 //ここまでが全方位移動
umekou 0:ca9732a3ae5c 47
umekou 0:ca9732a3ae5c 48 if(b[0]){
umekou 0:ca9732a3ae5c 49 speed[4] = 1.0;
umekou 0:ca9732a3ae5c 50 }else if(b[1]){
umekou 0:ca9732a3ae5c 51 speed[4] = -1.0;
umekou 0:ca9732a3ae5c 52 }else{
umekou 0:ca9732a3ae5c 53 speed[4] = 0.0;
umekou 0:ca9732a3ae5c 54 }
umekou 0:ca9732a3ae5c 55 //左 黄色と青
umekou 0:ca9732a3ae5c 56
umekou 0:ca9732a3ae5c 57 if(b[2]){
umekou 0:ca9732a3ae5c 58 speed[5] = 1.0;
umekou 0:ca9732a3ae5c 59 }else if(b[3]){
umekou 0:ca9732a3ae5c 60 speed[5] = -1.0;
umekou 0:ca9732a3ae5c 61 }else{
umekou 0:ca9732a3ae5c 62 speed[5] = 0.0;
umekou 0:ca9732a3ae5c 63 }
umekou 0:ca9732a3ae5c 64 //左 赤と緑
umekou 0:ca9732a3ae5c 65
umekou 0:ca9732a3ae5c 66 if(b[4]){
umekou 0:ca9732a3ae5c 67 speed[6] = 1.0;
umekou 0:ca9732a3ae5c 68 }else if(b[5]){
umekou 0:ca9732a3ae5c 69 speed[6] = -1.0;
umekou 0:ca9732a3ae5c 70 }else{
umekou 0:ca9732a3ae5c 71 speed[6] = 0.0;
umekou 0:ca9732a3ae5c 72 }
umekou 0:ca9732a3ae5c 73 //右　赤と青
umekou 0:ca9732a3ae5c 74
umekou 0:ca9732a3ae5c 75 for(i=0; i < 4; i++){
umekou 0:ca9732a3ae5c 76 motor[i].setSpeed(speed[i]*0.5);
umekou 0:ca9732a3ae5c 77 }//足回り
umekou 0:ca9732a3ae5c 78 motor[4].setSpeed(speed[4]*0.3);
umekou 0:ca9732a3ae5c 79 motor[5].setSpeed(speed[5]);
umekou 0:ca9732a3ae5c 80 motor[6].setSpeed(speed[6]*0.5);
umekou 0:ca9732a3ae5c 81 //モータースピードの上げ下げは*0.5を大きくしたり小さくしたりで調整
umekou 0:ca9732a3ae5c 82
umekou 0:ca9732a3ae5c 83 for (int i=0; i<8; i++) pc.printf("%d ", b[i]);
umekou 0:ca9732a3ae5c 84 pc.printf(" | ");
umekou 0:ca9732a3ae5c 85 for (int i=0; i<4; i++) pc.printf("%3d ", stick[i]);
umekou 0:ca9732a3ae5c 86 pc.printf(" | ");
umekou 0:ca9732a3ae5c 87 if (mycon.status) pc.printf("received\r\n");
umekou 0:ca9732a3ae5c 88 else pc.printf("anything error...\r\n");
umekou 0:ca9732a3ae5c 89 //コントローラーの状態を表示
umekou 0:ca9732a3ae5c 90
umekou 0:ca9732a3ae5c 91 }
umekou 0:ca9732a3ae5c 92 }