Revision 9:6057314dc8ec, committed 2014-09-13

Comitter:

9uS7

Date:

Sat Sep 13 11:03:52 2014 +0000

Parent:

8:bfcfda6b38fe

Child:

10:a90935ea0a4b

Commit message:

can control motor

Changed in this revision

--- a/bluetooth.cpp	Sat Sep 13 09:42:55 2014 +0000
+++ b/bluetooth.cpp	Sat Sep 13 11:03:52 2014 +0000
@@ -85,7 +85,7 @@
         //PACK:     [option/function/pwm*4]
         //pwm
         for( int i=0 ; i<4 ; i++ ){
-            temp.byte[i]=buf[1+i];
+            temp.byte[i]=buf[2+i];
         }
         motor( buf[1], temp.fl );
     }
@@ -192,4 +192,12 @@
     
     pc.printf( "%f %f\n", *_ir, *_fsr );
         
+}
+
+void sendMotor( char function, float power ){
+    char f[1];
+    float p[1];
+    f[0] = function;
+    p[0] = power;
+    sync( SYNC_MOTOR, f, p );
 }
\ No newline at end of file

--- a/bluetooth.h	Sat Sep 13 09:42:55 2014 +0000
+++ b/bluetooth.h	Sat Sep 13 11:03:52 2014 +0000
@@ -25,6 +25,7 @@
 
 void slaveRecieve( void );
 void recieveSensor(float*, float*);
+void sendMotor( char function, float power );
 
 //void btLoop(int);       //btLoop(role)  role:BT_MASTER or BT_SLAVE
 

--- a/control.cpp	Sat Sep 13 09:42:55 2014 +0000
+++ b/control.cpp	Sat Sep 13 11:03:52 2014 +0000
@@ -34,14 +34,14 @@
 }
 
 void pull(float buf){
-    Sig1 = 0;
-    Sig2 = 1;
+    Sig1 = 1;
+    Sig2 = 0;
     Pwm = zeroPWM+buf/3;
 }
 
 void loose(float buf){
-    Sig1 = 1;
-    Sig2 = 0;
+    Sig1 = 0;
+    Sig2 = 1;
     Pwm = zeroPWM+buf/3;
 }
 
@@ -78,5 +78,4 @@
 void getSensor(float* _ir,float* _fsr){
     *_ir = IR;
     *_fsr = FSR;
-    c_pc.printf("%f\n",*_fsr);
 }
\ No newline at end of file

--- 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