File content as of revision 1:006debaed874:

#include "mbed.h"

#define DATA_NUM 10 //DATA_NUM[byte]通信
#define MY_KEYCODE 0xAA//keycode
#define EN_KEYCODE 0xAA//keycode
#define KEY 0//number of first data
#define CHECK DATA_NUM-1//number of last data
#define TIME_LIMIT 1.0//腕が閉まっても動けるのはTIME_LIMIT[s]まで
#define ARM_REV .25

volatile uint8_t INdata[DATA_NUM]={0}, EXdata[DATA_NUM]={0};
volatile uint8_t count=0;

enum Logic{OFF=0, ON};//殆どはモーターのON，OFFに使う
enum Arm{R=0, L, C};//右腕と左腕
enum Clip{Open=0, Close};//開ける動作と閉める動作

bool GetBit(uint8_t n, uint8_t bit){//n=8[bit]data, bit = 1-8(8bitのデータから指定bitの数値を読み取る)
    return (n>>(bit-1))%2;
}

BusOut LED(LED1, LED2, LED3, LED4);//確認用
BusIn mt(p5, p6, p7, p8, p9, p10);//Switch
InterruptIn HandSW[3]={p15, p16, p17};//p14, p15, p16//R, L, C
PwmOut HandSpeed[2]={p25, p26};
DigitalOut Hand[2][2]={{p30, p29}, {p28, p27}};//{p17, p18}, {p19, p20}
//ex)Hand[R][Close]=ON;
Serial pc(USBTX, USBRX);
Ticker flick;
Timeout Touch;//腕で掴むときの安全対策

void StopCatching(void);//literally
void StopLeaving(void);//literally
void StartCatching(void);//literally
void StartLeaving(void);//literally
void RightCatch(void);//右腕のリミットスイッチ
void LeftCatch(void);//左腕のリミットスイッチ
void CenterCatch(void);//中央のリミットスイッチ
void illumination(void);//literally
void SetUp(void);//literally

int main(void) {
    
    SetUp();
    
    while(1) {
        
        INdata[1]= ~mt;
        
        if(GetBit(INdata[1], 2)==OFF) 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);
            }
        }
        
        if(((!HandSW[R])&&(!HandSW[L]))&&((Hand[R][Close]&&Hand[L][Close])==ON)){//どちらも押されたら停止
            StopCatching();//停止
            Touch.detach();//ワンショットタイマー停止
        }
        
    }
}

void StopCatching(void){//停止
    Hand[R][Close]=Hand[L][Close]=OFF;
}
void StopLeaving(void){//停止
    Hand[R][Open]=Hand[L][Open]=OFF;
}
void StartCatching(void){//掴む
    Hand[R][Close]=Hand[L][Close]=ON;
}
void StartLeaving(void){//離す
    Hand[R][Open]=Hand[L][Open]=ON;
}
void RightCatch(void){//右腕のリミットスイッチ
    if((!HandSW[R])&&(!HandSW[L])){//どちらも押されたら停止
        StopCatching();//停止
        Touch.detach();//ワンショットタイマー停止
    }
}
void LeftCatch(void){//左腕のリミットスイッチ
    if((!HandSW[R])&&(!HandSW[L])){//どちらも押されたら停止
        StopCatching();//停止
        Touch.detach();//ワンショットタイマー停止
    }
}
void CenterCatch(void){//中央のリミットスイッチ
    Touch.attach(StopCatching, TIME_LIMIT);//TIME_LIMIT[s]で停止
    StartCatching();
}
void illumination(void){//literally
    count++;
    LED = count;
}
void SetUp(void){//literally
    
    mt.mode(PullUp);
    
    HandSpeed[R] = HandSpeed[L] = ARM_REV;
    
    flick.attach(illumination, .5);
    
    HandSW[R].mode(PullUp);
    HandSW[L].mode(PullUp);
    HandSW[C].mode(PullUp);
    HandSW[R].fall(RightCatch);
    HandSW[L].fall(LeftCatch);
    HandSW[C].fall(CenterCatch);
}