Revision 3:f70ecf1e21d5, committed 2019-11-23

Comitter:

KINU

Date:

Sat Nov 23 10:06:03 2019 +0000

Parent:

2:c799e2dde99f

Child:

4:75090ff74dd0

Commit message:

moterCameraUS015sb612

Changed in this revision

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/TB6612.cpp	Sat Nov 23 10:06:03 2019 +0000
@@ -0,0 +1,57 @@
+/**
+ * Motor Driver TB6612 Control Library
+ *
+ * -- TB6612 is a device of the TOSHIBA. 
+ *
+ * Copyright (C) 2012 Junichi Katsu (JKSOFT) 
+ */
+ 
+ 
+#include "TB6612.h"
+ 
+// TB6612 Class Constructor
+TB6612::TB6612(PinName pwm, PinName fwd, PinName rev):
+        _pwm(pwm), _fwd(fwd), _rev(rev) {
+ 
+    _fwd = 0;
+    _rev = 0;
+    _pwm = 0.0;
+    _pwm.period(0.001);
+}
+ 
+// Speed Control
+//  arg
+//   int speed -100 -- 0 -- 100
+void TB6612::speed(int speed) {
+        
+    if( speed > 0 )
+    {
+        _pwm = ((float)speed) / 100.0;
+        _fwd = 1;
+        _rev = 0;
+    }
+    else if( speed < 0 )
+    {
+        _pwm = -((float)speed) / 100.0;
+        _fwd = 0;
+        _rev = 1;
+    }
+    else
+    {
+        _fwd = 1;
+        _rev = 1;
+    }
+}
+ 
+ 
+// Speed Control with time-out
+//  arg
+//   int speed -100 -- 0 -- 100
+//   int time  0
+void TB6612::move(int sspeed , int time)
+{
+    speed(sspeed);
+    wait_ms(time);
+}
+ 
+            
\ No newline at end of file

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/TB6612.h	Sat Nov 23 10:06:03 2019 +0000
@@ -0,0 +1,30 @@
+/**
+ * Motor Driver TB6612 Control Library
+ *
+ * -- TB6612 is a device of the rohm. 
+ *
+ * Copyright (C) 2012 Junichi Katsu (JKSOFT) 
+ */
+ 
+#ifndef MBED_TB6612_H
+#define MBED_TB6612_H
+ 
+#include "mbed.h"
+ 
+class TB6612 {
+public:
+    TB6612(PinName pwm, PinName fwd, PinName rev);
+    void speed(int speed);
+    void move(int speed , int time);
+    void operator= ( int value )
+    {
+        speed(value);
+    }
+    
+protected:
+    PwmOut _pwm;
+    DigitalOut _fwd;
+    DigitalOut _rev;
+};
+ 
+#endif
\ No newline at end of file

--- a/main.cpp	Sun Nov 17 10:08:05 2019 +0000
+++ b/main.cpp	Sat Nov 23 10:06:03 2019 +0000
@@ -1,24 +1,29 @@
 #include "mbed.h"
 #include "JPEGCamera.h"
 #include "us015.h"
+#include "TB6612.h"
 DigitalOut myled(LED1);
 US015 hs(p12,p11);
 DigitalOut thermo(p20);
 DigitalOut Ultra(p12);
 Serial pc(USBTX,USBRX); // tx, rx
 JPEGCamera camera(p9, p10); // TX, RX
+TB6612 left(p25,p17,p16);//モーターピン
+TB6612 right(p26,p19,p18);//モーターピン
 int main(){
   int i=1;
   float th;
   Timer tm;
   for(i=0;i<3;i++){
   pc.printf("start\r\n");
-
+  left = 100; //左モーター１００％
+  right = 100;//右モーター１００％
   bool detected=false;
     thermo=0; //焦電off
     Ultra=1;//超音波on
-    printf("超音波on 焦電off" )  ;
+    
     while(1) {
+         printf("超音波on 焦電off" )  ;
          hs.TrigerOut();
          wait(1);
          int distance;
@@ -26,50 +31,66 @@
          printf("%d\r\n",distance);//距離出力
         
         if(distance<2000){//超音波反応
+         left = 0; //左モーター０％
+         right = 0;//右モーター０％
+         printf("停止\n\r");
+         wait(1.5);
+         
          Ultra=0;//超音波off
          thermo=1;//焦電on
-          printf("音波off焦電on  ");
+          printf("超音波off焦電on  ");
           if(true)
              th = thermo;
              if(th=1 && !detected) {//焦電反応ありの場合
-              i++; 
+               i++; 
                detected=true;
-             pc.printf("human\r\n");
-             tm.reset();
-             tm.start();
+               pc.printf("human\r\n");
+               tm.reset();
+               tm.start();
           
-           LocalFileSystem local("local");
-           Timer timer;
-           timer.start();
-           camera.setPictureSize(JPEGCamera::SIZE320x240);
+               LocalFileSystem local("local");
+               Timer timer;
+               timer.start();
+               camera.setPictureSize(JPEGCamera::SIZE320x240);
 
             
-              if (camera.isReady()) {
-               char filename[32];
-               sprintf(filename, "/local/pict%03d.jpg",i);
-               printf("Picture: %s ", filename);
-                if (camera.takePicture(filename)) {
-                 while (camera.isProcessing()) {
+               if (camera.isReady()) {
+                 char filename[32];
+                 sprintf(filename, "/local/pict%03d.jpg",i);
+                 printf("Picture: %s ", filename);
+                 if (camera.takePicture(filename)) { 
+                  while (camera.isProcessing()) {
                     camera.processPicture();
                     printf("take pictuer!");
-                  }
-               } else {
+                   }
+                 }else{
                  printf("take picture failed\r\n");
+                 }
+               }else{
+                 printf("camera is not ready\r\n");
                 }
-               } else {
-               printf("camera is not ready\r\n");
-               }
+               
+               printf("time = %f\n", timer.read());
                
-              printf("time = %f\n", timer.read());
-            }
-          
-          
-          
-        }else{//焦電反応なしの場合
-             detected=false;
-             }           
+              }else{//焦電反応なしの場合
+               detected=false;
+               left = 20; //左モーター１０％
+               right = 100;//右モーター１００％(右折)
+               printf("右折\n\r");
+               wait(3.0);
+               left = 100; //左モーター１００％
+               right = 100;//右モーター１００％
+              }           
+       }else{//超音波distance>2000
+          left = 100; //左モーター100％
+          right = 20;//右モーター20％(右折)
+         }
+
+    }
+
 }
+
+
  
   }
-  }
-   
\ No newline at end of file
+