File content as of revision 0:87273e4cfe1c:

#include "mbed.h"

#define fsr_def 0.05
#define sence_def 0.15
#define zeroPWM 0.17
#define pullzero 0.9


AnalogIn FSR1(p19);
AnalogIn FSR2(p20);


DigitalOut Sig11(p24);
DigitalOut Sig12(p26);
PwmOut Pwm1(p25);
DigitalOut Sig21(p21);
DigitalOut Sig22(p23);
PwmOut Pwm2(p22);

DigitalOut myled1(LED1);
DigitalOut myled2(LED2);

Serial pc(USBTX, USBRX);

//引く
void Pull1(float buf){
    Sig11 = 0;
    Sig12 = 1;
    Pwm1 = 1.0-(buf);
    //Pwm1 = 0.5;
}

void Pull2(float buf){
    Sig21 = 0;
    Sig22 = 1;
    Pwm2 = 1.0-(buf);
    //Pwm2 = 0.5;
}



//ブレーキモード
void Brake1(){
    Sig11 = 0;
    Sig12 = 0;
    Pwm1 = 1;
}

void Brake2(){
    Sig21 = 0;
    Sig22 = 0;
    Pwm2 = 1;
}


//開放
void Open1(){
    Sig11 = 0;
    Sig12 = 0;
    Pwm1 = 0;
}

void Open2(){
    Sig21 = 0;
    Sig22 = 0;
    Pwm2 = 0;
}


//FSRと相手側モータを回す
void Moter2FSR1(float buf){
    if((FSR1-buf)>fsr_def && FSR2<pullzero){ //相手が引っ張ってる状態でピックってなる動き
     Pull2(FSR1);  
     Brake1(); 
     myled1=1;
    }
    else if(FSR1<FSR2 && abs(FSR1-FSR2)>sence_def){//ギューんって引っ張る動き
     Pull2(1.5*FSR1);
     Brake1(); 
     myled1=0;   
    }
    else{     
         Open2();   
         myled1=0;
    }
}

void Moter1FSR2(float buf){
    if((FSR2-buf)>fsr_def && FSR1<pullzero){
     Pull1(FSR2);
     Brake2(); 
     myled2=1;   
    }
    else if(FSR2<FSR1&& abs(FSR1-FSR2)>sence_def){
     Pull1(1.5*FSR2);
     Brake2(); 
     myled2=0;   
    }
    else{     
         Open1();   
         myled2=0;
    }
}


int main() {
    float FSR1_buf = FSR1;
    float FSR2_buf = FSR2;
    

    Pwm1.period_us(50);
    Pwm2.period_us(50);
    
    while(1) {
        pc.printf("FSR1:");
        pc.printf("%1.3f  ",FSR1.read()); //圧力センサの出力
        pc.printf("FSR2:");
        pc.printf("%1.3f  \n",FSR2.read()); //圧力センサの出力
      
        
        Moter2FSR1(FSR1_buf);
        Moter1FSR2(FSR2_buf);
     
        FSR1_buf = FSR1;
        FSR2_buf = FSR2;
    }
}