Diff: main.cpp

Revision:

9:6057314dc8ec

Parent:

8:bfcfda6b38fe

Child:

10:a90935ea0a4b

--- a/main.cpp	Sat Sep 13 09:42:55 2014 +0000
+++ b/main.cpp	Sat Sep 13 11:03:52 2014 +0000
@@ -44,8 +44,62 @@
 void masterLoop(){
     float ir_m,fsr_m;   //sensor of master
     float ir_s,fsr_s;   //sensor of slave
+    char function_m;    //motor function of master
+    char function_s;    //motor function of slave
+    float power_m;      //motor power of master
+    float power_s;      //motor power of slave
+    float dst;
+    getSensor( &ir_m, &fsr_m );
     recieveSensor( &ir_s, &fsr_s );
+    
+    //距離を一定に保つ
+    /*
+    if( (dst=abs( ir_m+ir_s - center_pos*2 ))<0.1 ){
+        if( fsr_s-fsr_m>0.2 ){
+            function_m = MOTOR_OPEN;
+            power_m = 0;
+            function_s = MOTOR_PULL;
+            power_s = dst/2;
+        }
+        else if( fsr_m-fsr_s >0.2 ){
+            function_m = MOTOR_PULL;
+            power_m = dst/2;
+            function_s = MOTOR_OPEN;
+            power_s = 0;
+        }
+        else{
+            function_m = MOTOR_OPEN;
+            power_m = 0;
+            function_s = MOTOR_OPEN;
+            power_s = 0;
+        }
+    }
+    else if( ir_m+ir_s - center_pos*2 < 0 ){
+        function_m = MOTOR_LOOSE;
+        power_m = dst/2;
+        function_s = MOTOR_LOOSE;
+        power_s = dst/2;
+    }
+    else{
+        function_m = MOTOR_PULL;
+        power_m = dst/2;
+        function_s = MOTOR_PULL;
+        power_s = dst/2;
+    }
+    */
+    
+    function_m = MOTOR_PULL;
+    power_m = 0.1;
+    function_s = MOTOR_PULL;
+    power_s = 0.1;
+    
+    
+    //Master's motor
+    motor( function_m, power_s );
+    //slave's motor
+    sendMotor( function_s, power_s );
+    
     led4 = ( ir_s>0.5 ? 1 : 0 );
     led2 = ( led2 ? 0 : 1 );
-    wait(1);
+    wait(0.1);
 }
\ No newline at end of file