main.cpp

00001 #include "mbed.h"
00002 #include "ikarashiMDC.h"
00003 #include "controller.h"
00004 #include <math.h>
00005 
00006 DigitalOut stop(D13);
00007 Bcon mycon(PA_9,PA_10,004);//FEP004はコントローラーについてる方の番号
00008 Serial serial(PC_10,PC_11,115200);
00009 Serial pc(USBTX,USBRX,115200);
00010 
00011 ikarashiMDC motor[] = {
00012     ikarashiMDC(0,0,SM,&serial),
00013     ikarashiMDC(0,1,SM,&serial),
00014     ikarashiMDC(0,2,SM,&serial),
00015     ikarashiMDC(0,3,SM,&serial),
00016     ikarashiMDC(1,0,SM,&serial),
00017     ikarashiMDC(1,1,SM,&serial),
00018     ikarashiMDC(1,2,SM,&serial)
00019     //ikarashiMDC(1,3,SM,&serial)
00020 };
00021 
00022 
00023 int main()
00024 {
00025     mycon.StartReceive();
00026     stop=1;
00027     uint8_t b[8];//ボタン　b[0]~b[7]の順に　左 黄色 青 赤 緑　右 赤 青 黄色 緑
00028     int16_t stick[4];//スティック
00029     double speed[7] = {0,0,0,0,0,0,0};//モータースピード
00030     double x,y;
00031     int i;
00032     while(1) {
00033         for (i=0; i<8; i++) b[i] = mycon.getButton(i);
00034         for (i=0; i<4; i++) stick[i] = mycon.getStick(i);
00035         x=stick[0];
00036         y=stick[1];
00037         if(x>20||x<-20||y>20||y<-20){
00038             for(i=0; i < 4; i++){
00039                 speed[i] = sin(atan2(y,x)-3.14*(i*2-1) / 4);
00040             }
00041         } else {
00042             for(i=0; i < 4; i++){
00043                 speed[i] = 0;
00044             }
00045         }
00046         //ここまでが全方位移動
00047         
00048         if(b[0]){
00049             speed[4] = 1.0;
00050         }else if(b[1]){
00051             speed[4] = -1.0;
00052         }else{
00053             speed[4] = 0.0;
00054         }
00055         //左 黄色と青
00056         
00057         if(b[2]){
00058             speed[5] = 1.0;
00059         }else if(b[3]){
00060             speed[5] = -1.0;
00061         }else{
00062             speed[5] = 0.0;
00063         }
00064         //左 赤と緑
00065         
00066         if(b[4]){
00067             speed[6] = 1.0;
00068         }else if(b[5]){
00069             speed[6] = -1.0;
00070         }else{
00071             speed[6] = 0.0;
00072         }
00073         //右　赤と青
00074         
00075         for(i=0; i < 4; i++){
00076             motor[i].setSpeed(speed[i]*0.5);
00077         }//足回り
00078         motor[4].setSpeed(speed[4]*0.3);
00079         motor[5].setSpeed(speed[5]);
00080         motor[6].setSpeed(speed[6]*0.5);
00081         //モータースピードの上げ下げは*0.5を大きくしたり小さくしたりで調整
00082         
00083         for (int i=0; i<8; i++) pc.printf("%d ", b[i]);
00084         pc.printf(" | ");
00085         for (int i=0; i<4; i++) pc.printf("%3d ", stick[i]);
00086         pc.printf(" | ");
00087         if (mycon.status) pc.printf("received\r\n");
00088         else pc.printf("anything error...\r\n");
00089         //コントローラーの状態を表示
00090         
00091     }
00092 }