智也 大野
障害物回避アルゴリズム
Dependencies: TA7291P mbed Ping
Revision 0:bc1f87f34e45, committed 2017-03-17
- Comitter:
- tomoya123
- Date:
- Fri Mar 17 08:46:44 2017 +0000
- Commit message:
- avoidance;
Changed in this revision
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/TA7291P.lib Fri Mar 17 08:46:44 2017 +0000 @@ -0,0 +1,1 @@ +http://mbed.org/users/takoyaki/code/TA7291P/#2abc17614090
--- /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);
+ }
+ }
+}
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/mbed.bld Fri Mar 17 08:46:44 2017 +0000 @@ -0,0 +1,1 @@ +http://mbed.org/users/mbed_official/code/mbed/builds/abea610beb85 \ No newline at end of file
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/ping2.lib Fri Mar 17 08:46:44 2017 +0000 @@ -0,0 +1,1 @@ +http://mbed.org/users/rosienej/code/Ping/#6996f66161d7