2022交流ロボコン用 バージョン2
Dependencies: mbed 2021Bcon ikarashiMDC_2byte_ver
Diff: main.cpp
- Revision:
- 0:ca9732a3ae5c
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/main.cpp Sun Mar 27 02:33:51 2022 +0000 @@ -0,0 +1,92 @@ +#include "mbed.h" +#include "ikarashiMDC.h" +#include "controller.h" +#include <math.h> + +DigitalOut stop(D13); +Bcon mycon(PA_9,PA_10,004);//FEP004はコントローラーについてる方の番号 +Serial serial(PC_10,PC_11,115200); +Serial pc(USBTX,USBRX,115200); + +ikarashiMDC motor[] = { + ikarashiMDC(0,0,SM,&serial), + ikarashiMDC(0,1,SM,&serial), + ikarashiMDC(0,2,SM,&serial), + ikarashiMDC(0,3,SM,&serial), + ikarashiMDC(1,0,SM,&serial), + ikarashiMDC(1,1,SM,&serial), + ikarashiMDC(1,2,SM,&serial) + //ikarashiMDC(1,3,SM,&serial) +}; + + +int main() +{ + mycon.StartReceive(); + stop=1; + uint8_t b[8];//ボタン b[0]~b[7]の順に 左 黄色 青 赤 緑 右 赤 青 黄色 緑 + int16_t stick[4];//スティック + double speed[7] = {0,0,0,0,0,0,0};//モータースピード + double x,y; + int i; + while(1) { + for (i=0; i<8; i++) b[i] = mycon.getButton(i); + for (i=0; i<4; i++) stick[i] = mycon.getStick(i); + x=stick[0]; + y=stick[1]; + if(x>20||x<-20||y>20||y<-20){ + for(i=0; i < 4; i++){ + speed[i] = sin(atan2(y,x)-3.14*(i*2-1) / 4); + } + } else { + for(i=0; i < 4; i++){ + speed[i] = 0; + } + } + //ここまでが全方位移動 + + if(b[0]){ + speed[4] = 1.0; + }else if(b[1]){ + speed[4] = -1.0; + }else{ + speed[4] = 0.0; + } + //左 黄色と青 + + if(b[2]){ + speed[5] = 1.0; + }else if(b[3]){ + speed[5] = -1.0; + }else{ + speed[5] = 0.0; + } + //左 赤と緑 + + if(b[4]){ + speed[6] = 1.0; + }else if(b[5]){ + speed[6] = -1.0; + }else{ + speed[6] = 0.0; + } + //右 赤と青 + + for(i=0; i < 4; i++){ + motor[i].setSpeed(speed[i]*0.5); + }//足回り + motor[4].setSpeed(speed[4]*0.3); + motor[5].setSpeed(speed[5]); + motor[6].setSpeed(speed[6]*0.5); + //モータースピードの上げ下げは*0.5を大きくしたり小さくしたりで調整 + + for (int i=0; i<8; i++) pc.printf("%d ", b[i]); + pc.printf(" | "); + for (int i=0; i<4; i++) pc.printf("%3d ", stick[i]); + pc.printf(" | "); + if (mycon.status) pc.printf("received\r\n"); + else pc.printf("anything error...\r\n"); + //コントローラーの状態を表示 + + } +}