File content as of revision 6:606507fc8852:

#include "mbed.h"
#include "MultiSerial.h"

#define DATA_NUM 1 //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 1.0//腕のPWM
#define TURN_REV 0.25//回転の初期PWM
#define TURN_RATIO 100//1000000000//なるべく大きい数
#define TURN_RATE 1//加速度の変更...1, 2, 4, 8, 16...
#define TURN_REV_LIMIT 0.5//PWMの最大数値
#define TURN_ACCELERATION 0.1//0.015625//なるべく小さい数
#define TURN_SLOPE TURN_RATE*0.1//0.015625//なるべく小さい数1/2, 4, 8,...256

#define KEYCODE 0xAA

uint8_t INdata[DATA_NUM]={0}, EXdata[DATA_NUM]={0};
uint8_t count=0;
uint8_t TURN=0, IO=0,SIG=0;
uint8_t data[DATA_NUM];

enum Logic{OFF=0, ON};//殆どはモーターのON，OFFに使う
enum Arm{R=0, L, C};//右腕と左腕
enum Clip{Open=0, Close};//開ける動作と閉める動作
enum Revolution{CW=0, CCW};//正回転，逆回転(上体回転)
enum Compass{N=0, E, W, S};//マシンの回転

BusOut LED(LED1, LED2, LED3, LED4);//確認用

/* 
    BusIn mt(p19, p20, p17, p18);//Switch
*/


BusIn mt(p7, p8, p9, p10);
//DigitalIn Ninety[2]={p5, p6};//R, L
DigitalIn OneEighty[2]={p18, p19};//R, L
InterruptIn HandSW[3]={p15, p16, p17};//p14, p15, p16//R, L, C
DigitalIn TurnSW[2]={p11, p12};//
PwmOut HandSpeed[2]={p23, p24};
PwmOut TurnSpeed(p22);//p22
DigitalOut Hand[2][2]={{p28, p27}, {p26, p25}};//{p17, p18}, {p19, p20}
//ex)Hand[R][Close]=ON;
DigitalOut Turn[2]={p29, p30};//p29, p30
//ex)Turn[CW]=ON;
Serial pc(USBTX, USBRX);
Ticker flick;
Ticker Save;
Ticker Accelerator;
Timeout Touch;//腕で掴むときの安全対策

void SW_saving(void);
void Acceleration(void);
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
uint8_t LinkBit(bool eight, bool seven, bool six, bool five, bool fore, bool three, bool two, bool one);
bool GetBit(uint8_t n, uint8_t bit);
void SetUp(void);//literally


int main(void) {
    
    wait(0.5);
    
    MultiSerial leg_mbed(p13,p14,read);
    
    SetUp();
    
    while(1) {
        
        leg_mbed.read_data(data,KEYCODE);
        
        INdata[0] = data[0] | ~mt;
        
        if(GetBit(INdata[0], 2)==OFF) StopLeaving();//停止
        
        if((GetBit(INdata[0], 1)+GetBit(INdata[0], 2))==1){//同時押し防止
            if(GetBit(INdata[0], 1)==true){
                CenterCatch();//中央のリミットスイッチがうまく動作しなかった場合の保険
            }
            if(GetBit(INdata[0], 2)==true){//腕を広げる動作はマニュアル操作
                StopCatching();//停止
                Touch.detach();//ワンショットタイマー停止
                Hand[R][Open] = GetBit(INdata[0], 2)&&OneEighty[R];
                Hand[L][Open] = GetBit(INdata[0], 2)&&OneEighty[L];
            }
        }
        
        if(((!HandSW[R])&&(!HandSW[L]))&&((Hand[R][Close]&&Hand[L][Close])==ON)){//どちらも押されたら停止
            StopCatching();//停止
            Touch.detach();//ワンショットタイマー停止
        }
        
        if(!OneEighty[R]) Hand[R][Open]=OFF;
        if(!OneEighty[L]) Hand[L][Open]=OFF;
        //if(!Ninety[R]) Hand[R][Close]=OFF;
        //if(!Ninety[L]) Hand[L][Close]=OFF;
        
        //if((!Ninety[R])&&(!Ninety[L])){//どちらも押されたら停止
        //    StopCatching();//停止
        //    Touch.detach();//ワンショットタイマー停止
        //}
        
        //LED = ((0x01 * !HandSW[R]) | (0x02 * !HandSW[L]) | (0x04 * !HandSW[C]) | (0x08 * ((!OneEighty[R])&&(!OneEighty[L])) ));
        LED = INdata[0];
        
        //上体回転
        
        //IO = GetBit(INdata[0], 3)+GetBit(INdata[0], 4);
        
        //if(GetBit(INdata[0], 3)==OFF) Turn[CW] = OFF;//停止
        //if(GetBit(INdata[0], 4)==OFF) Turn[CCW] = OFF;//停止
        
        if((GetBit(INdata[0], 3)+GetBit(INdata[0], 4)==1)&&(TurnSpeed==0)){//同時押し防止
        
            Turn[CW] = GetBit(INdata[0], 3)&&TurnSW[N];//押されていて，かつリミットスイッチが押されていなかったら右回転
            Turn[CCW] = GetBit(INdata[0], 4)&&TurnSW[E];//押されていて，かつリミットスイッチが押されていなかったら左回転
            TURN = LinkBit(0, 0, 0, 0, 0, 0, Turn[CW], Turn[CCW]);
            
        }
        if(TurnSpeed!=0){
            
            Turn[CW] = GetBit(TURN, 2)&&TurnSW[N];
            Turn[CCW] = GetBit(TURN, 1)&&TurnSW[E];
            
        }
        if((GetBit(INdata[0], 3)+GetBit(INdata[0], 4)!=1)&&(TurnSpeed==0)){
            Turn[CW] = OFF;
            Turn[CCW] = OFF;
        }
        
        //Acceleration();
        
        //float i;

        /*SIG = LinkBit(0, 0, 0, 0, 0, 0, GetBit(INdata[0], 3), GetBit(INdata[0], 4)); 
        IO = GetBit(INdata[0], 3)+GetBit(INdata[0], 4);
        
        if((IO==1)&&(0<=TurnSpeed)&&(TurnSpeed<TURN_REV_LIMIT)&&(TURN==SIG)){
            
            TurnSpeed = TurnSpeed + TURN_SLOPE;
            wait(TURN_ACCELERATION);
        }
        if(((IO==0)||(IO==2)||(TURN!=SIG))&&(0<TurnSpeed)&&(TurnSpeed<=TURN_REV_LIMIT)){
            
            TurnSpeed = TurnSpeed - TURN_SLOPE;
            wait(TURN_ACCELERATION);
        }*/
        //i = TurnSpeed;
        //pc.printf("%f, %d\n", i, IO);
        
        if(!TurnSW[N]) Turn[CW]=OFF;//リミットスイッチによる停止
        if(!TurnSW[E]) Turn[CCW]=OFF;
        
        
    }
}

void SW_saving(void){
    IO = GetBit(INdata[0], 3)+GetBit(INdata[0], 4);
    SIG = LinkBit(0, 0, 0, 0, 0, 0, GetBit(INdata[0], 3), GetBit(INdata[0], 4));
}
void Acceleration(void){
    
    float i;
    
    SIG = LinkBit(0, 0, 0, 0, 0, 0, GetBit(INdata[0], 3), GetBit(INdata[0], 4)); 
    IO = GetBit(INdata[0], 3)+GetBit(INdata[0], 4);
    
    if((IO==1)&&(((unsigned long)(TurnSpeed*TURN_RATIO)==0)||(0<(unsigned long)(TurnSpeed*TURN_RATIO)))&&((unsigned long)(TurnSpeed*TURN_RATIO)<(unsigned long)(TURN_REV_LIMIT*TURN_RATIO))&&(TURN==SIG)){
        
        TurnSpeed = TurnSpeed + TURN_SLOPE;
        wait(TURN_ACCELERATION);
    }
    if(((IO==0)||(IO==2)||(TURN!=SIG))&&(0<(unsigned long)(TurnSpeed*TURN_RATIO))&&((unsigned long)(TurnSpeed*TURN_RATIO)<=(unsigned long)(TURN_REV_LIMIT*TURN_RATIO))){
        
        TurnSpeed = TurnSpeed - TURN_SLOPE;
        wait(TURN_ACCELERATION);
    }
    i = TurnSpeed;
    pc.printf("%f, %d\n", i, IO);
}
void StopCatching(void){//停止
    Hand[R][Close]=Hand[L][Close]=OFF;
}
void StopLeaving(void){//停止
    Hand[R][Open]=Hand[L][Open]=OFF;
}
void StartCatching(void){//掴む（リミットスイッチが押されてなければ）

    if((!HandSW[R])||(!HandSW[L])){
        Hand[R][Close] = Hand[L][Close] = OFF;
    }
    else{
        
        //Hand[R][Close]=Ninety[R];
        //Hand[L][Close]=Ninety[L];
        
        Hand[R][Close]=ON;
        Hand[L][Close]=ON;
            
    }
    
}
void StartLeaving(void){//離す（リミットスイッチが押されてなければ）
    Hand[R][Open]=OneEighty[R];
    Hand[L][Open]=OneEighty[L];
}
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){//中央のリミットスイッチ

    if((!OneEighty[R])&&(!OneEighty[L])){
        Touch.attach(StopCatching, TIME_LIMIT);//TIME_LIMIT[s]で停止
        StartCatching();
        LED=15;
    }
    
}
    
void illumination(void){//literally
    count++;
    LED = count;
}
uint8_t LinkBit(bool eight, bool seven, bool six, bool five, bool fore, bool three, bool two, bool one){
    return 0x80*eight + 0x40*seven + 0x20*six + 0x10*five + 0x08*fore + 0x04*three + 0x02*two + 0x01*one;
}
bool GetBit(uint8_t n, uint8_t bit){//n=8[bit]data, bit = 1-8(8bitのデータから指定bitの数値を読み取る)-
    return (n>>(bit-1))%2;
}
void SetUp(void){//literally
    
    mt.mode(PullUp);
    
    HandSpeed[R] = HandSpeed[L] = ARM_REV;
    TurnSpeed = TURN_REV;
    
    //flick.attach(illumination, .5);
    //Accelerator.attach(Acceleration, TURN_ACCELERATION);
    //Save.attach(SW_saving, 0.015625);
    
    HandSW[R].mode(PullUp);
    HandSW[L].mode(PullUp);
    HandSW[C].mode(PullUp);
    HandSW[R].fall(RightCatch);
    HandSW[L].fall(LeftCatch);
    HandSW[C].fall(CenterCatch);
    HandSW[C].rise(StopCatching);
    
    OneEighty[R].mode(PullUp);
    OneEighty[L].mode(PullUp);
    //Ninety[R].mode(PullUp);
    //Ninety[L].mode(PullUp);
    
    TurnSW[N].mode(PullUp);
    TurnSW[E].mode(PullUp);
    
}