Diff: main.cpp

Revision:

0:bc1f87f34e45

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp	Fri Mar 17 08:46:44 2017 +0000
@@ -0,0 +1,67 @@
+#include "mbed.h"
+#include "ta7291p.h"
+#include "Ping.h"
+
+//超音波センサー
+#define SAFE_DISTANCE 80.0
+#define TURN_DISTANCE 60.0
+#define STOP_DISTANCE 40.0
+
+//モータードライバー
+#define FAST_SPEED 1.0
+#define NORMAL_SPEED 0.7
+#define TURN_SPEED 0.4
+#define SLOW_SPEED 0.2
+
+ta7291p mortor1(p25,p24,p26);
+ta7291p mortor2(p22,p21,p23);
+Ping Pinger(p30);
+
+
+int main() {
+    int i;
+    float range;
+    
+    while(1) {
+        Pinger.Send();    
+        wait_ms(30);
+        range = Pinger.Read_cm()/2.0;
+        printf("range = %f\r\n",range);
+        //wait(1.0);
+        
+        if(range <= STOP_DISTANCE){
+                                printf("ok1\r\n");
+                                mortor1.rotstop();
+                                mortor2.rotstop();
+                                wait(2.0);
+                                
+                                for(i=0; i<100; i++)
+                                {
+                                    mortor1.rotf(NORMAL_SPEED);
+                                   // mortor2.rotf(FAST_SPEED);
+                                   //mortor2.rotb(FAST_SPEED);
+                                    wait_ms(30);
+                                }
+                                }
+        /*else if( (range > STOP_DISTANCE) && (range <= TURN_DISTANCE) ){
+                                printf("ok2\r\n");
+                                mortor1.rotf(NORMAL_SPEED);
+                                mortor2.rotf(TURN_SPEED);
+                                
+                                }*/
+        else if( (range > TURN_DISTANCE) && (range <= SAFE_DISTANCE) ){
+                                printf("ok3\r\n");
+                                mortor1.rotf(NORMAL_SPEED);
+                                mortor2.rotf(NORMAL_SPEED);
+                                //mortor1.rotb(NORMAL_SPEED);
+                                //mortor2.rotb(NORMAL_SPEED);
+                                }
+        else{
+                                printf("ok4\r\n");
+                                mortor1.rotf(FAST_SPEED);
+                                mortor2.rotf(FAST_SPEED);
+                                //mortor1.rotb(FAST_SPEED);
+                                //mortor2.rotb(FAST_SPEED);
+                                }    
+    }
+}