Diff: main.cpp

Revision:

0:87273e4cfe1c

Child:

1:974404d7741b

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp	Tue Oct 07 04:33:22 2014 +0000
@@ -0,0 +1,127 @@
+#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;
+    }
+}
\ No newline at end of file