File content as of revision 0:f38700ce12ea:

#include "mbed.h"
#include "main.h"//ピン位置，main関数までの初期動作の詳細，変数，関数等

extern bool GetBit(void);
extern DigitalOut LED[4];
//オムニホイール関連
extern DigitalOut Omni[4][2];
extern PwmOut OmniSpeed[4];
extern void pause(void);
extern void go(void);
extern void back(void);
extern void sidleleft(void);
extern void sidleright(void);
extern void turnleft(void);
extern void turnright(void);
//上体回転関連
extern DigitalOut Turn[2];
extern DigitalIn TurnSW;
extern PwmOut TurnSpeed;
//パワーウィンドウ(腕)関連
extern bool Pushed[2];
extern DigitalOut Hand[2][2];
extern InterruptIn HandSW[3];
extern Timeout Touch;

extern void StopCatching(void);
extern void StopLeaving(void);
extern void StartCatching(void);
extern void StartLeaving(void);
extern void RightCatch(void);
extern void LeftCatch(void);
extern void CenterCatch(void);

//通信関連
extern Serial pc;
extern Serial Xbee;//p13, p14
extern volatile uint8_t INdata[DATA_NUM], EXdata[DATA_NUM];
extern void TX(void);
extern void RX(void);
int main(void) {
    //簡単そうなコードの説明は省く
    int i;
    BusIn mt(p21, p22, p23, p24, p25, p26);//調整用スイッチ
    mt.mode(PullUp);
    //オムニホイール関連
    for(i=0; i<4; i++) OmniSpeed[i] = OmniRev;//オムニホイールのPWM
    //上体回転関連
    TurnSpeed = TurnRev;//上体回転のPWM
    TurnSW.mode(PullUp);
    //パワーウィンドウ(腕)関連
    HandSW[R].mode(PullUp);
    HandSW[L].mode(PullUp);
    HandSW[C].mode(PullUp);
    HandSW[R].rise(RightCatch);//立ち上がり
    HandSW[L].rise(LeftCatch);//立ち上がり
    HandSW[C].rise(CenterCatch);//立ち上がり
    //通信関連
    Xbee.attach(&RX, Serial::RxIrq);
    Xbee.attach(&TX, Serial::TxIrq);
    Xbee.putc(1);//送信開始
    i = Xbee.getc();//受信開始
    while(1) {
        //腕のプログラム
        INdata[1]= ~mt;//調整用スイッチ
        StopLeaving();//停止
        if((GetBit(INdata[1], 1)+GetBit(INdata[1], 2))==1){//同時押し防止
            if(GetBit(INdata[1], 1)==true) CenterCatch();//中央のリミットスイッチがうまく動作しなかった場合の保険
            if(GetBit(INdata[1], 2)==true){//腕を広げる動作はマニュアル操作
                StopCatching();//停止
                Touch.detach();//ワンショットタイマー停止
                Hand[R][Open] = Hand[L][Open] = GetBit(INdata[1], 2);
            }
        }
        //オムニホイール
        INdata[2] = ~mt;//調整用スイッチ
        pause();//停止
        if((GetBit(INdata[2], 1)+GetBit(INdata[2], 2)+GetBit(INdata[2], 3)+GetBit(INdata[2], 4)+GetBit(INdata[2], 5)+GetBit(INdata[2], 6))==1){//同時押し防止     
            if(GetBit(INdata[2], 1)) go();//前進
            if(GetBit(INdata[2], 2)) back();//後進
            if(GetBit(INdata[2], 3)) sidleleft();//左移動
            if(GetBit(INdata[2], 4)) sidleright();//右移動
            if(GetBit(INdata[2], 5)) turnleft();//左旋回
            if(GetBit(INdata[2], 6)) turnright();//右旋回
        }
        //上体回転
        INdata[3] = ~mt;//調整用スイッチ
        Turn[CW] = Turn[CCW] = OFF;//停止
        if((GetBit(INdata[3], 1)+GetBit(INdata[3], 2))==1){//同時押し防止
            if(GetBit(INdata[3], 1)&&(TurnSW)){
                Turn[CCW] = ON;//押されていて，かつリミットスイッチが押されていなかったら左回転
            }
            if(GetBit(INdata[3], 2)){
                Turn[CW] = ON;//押されたら右回転
            }
        }
    }
}