智也 大野 / Mbed 2 deprecated avoidance_program

障害物回避アルゴリズム

Dependencies:   TA7291P mbed Ping

main.cpp@0:bc1f87f34e45, 2017-03-17 (annotated)

Committer:
tomoya123
Date:
Fri Mar 17 08:46:44 2017 +0000
Revision:
0:bc1f87f34e45
avoidance;

Who changed what in which revision?

UserRevisionLine numberNew contents of line
tomoya123 0:bc1f87f34e45 1 #include "mbed.h"
tomoya123 0:bc1f87f34e45 2 #include "ta7291p.h"
tomoya123 0:bc1f87f34e45 3 #include "Ping.h"
tomoya123 0:bc1f87f34e45 4
tomoya123 0:bc1f87f34e45 5 //超音波センサー
tomoya123 0:bc1f87f34e45 6 #define SAFE_DISTANCE 80.0
tomoya123 0:bc1f87f34e45 7 #define TURN_DISTANCE 60.0
tomoya123 0:bc1f87f34e45 8 #define STOP_DISTANCE 40.0
tomoya123 0:bc1f87f34e45 9
tomoya123 0:bc1f87f34e45 10 //モータードライバー
tomoya123 0:bc1f87f34e45 11 #define FAST_SPEED 1.0
tomoya123 0:bc1f87f34e45 12 #define NORMAL_SPEED 0.7
tomoya123 0:bc1f87f34e45 13 #define TURN_SPEED 0.4
tomoya123 0:bc1f87f34e45 14 #define SLOW_SPEED 0.2
tomoya123 0:bc1f87f34e45 15
tomoya123 0:bc1f87f34e45 16 ta7291p mortor1(p25,p24,p26);
tomoya123 0:bc1f87f34e45 17 ta7291p mortor2(p22,p21,p23);
tomoya123 0:bc1f87f34e45 18 Ping Pinger(p30);
tomoya123 0:bc1f87f34e45 19
tomoya123 0:bc1f87f34e45 20
tomoya123 0:bc1f87f34e45 21 int main() {
tomoya123 0:bc1f87f34e45 22 int i;
tomoya123 0:bc1f87f34e45 23 float range;
tomoya123 0:bc1f87f34e45 24
tomoya123 0:bc1f87f34e45 25 while(1) {
tomoya123 0:bc1f87f34e45 26 Pinger.Send();
tomoya123 0:bc1f87f34e45 27 wait_ms(30);
tomoya123 0:bc1f87f34e45 28 range = Pinger.Read_cm()/2.0;
tomoya123 0:bc1f87f34e45 29 printf("range = %f\r\n",range);
tomoya123 0:bc1f87f34e45 30 //wait(1.0);
tomoya123 0:bc1f87f34e45 31
tomoya123 0:bc1f87f34e45 32 if(range <= STOP_DISTANCE){
tomoya123 0:bc1f87f34e45 33 printf("ok1\r\n");
tomoya123 0:bc1f87f34e45 34 mortor1.rotstop();
tomoya123 0:bc1f87f34e45 35 mortor2.rotstop();
tomoya123 0:bc1f87f34e45 36 wait(2.0);
tomoya123 0:bc1f87f34e45 37
tomoya123 0:bc1f87f34e45 38 for(i=0; i<100; i++)
tomoya123 0:bc1f87f34e45 39 {
tomoya123 0:bc1f87f34e45 40 mortor1.rotf(NORMAL_SPEED);
tomoya123 0:bc1f87f34e45 41 // mortor2.rotf(FAST_SPEED);
tomoya123 0:bc1f87f34e45 42 //mortor2.rotb(FAST_SPEED);
tomoya123 0:bc1f87f34e45 43 wait_ms(30);
tomoya123 0:bc1f87f34e45 44 }
tomoya123 0:bc1f87f34e45 45 }
tomoya123 0:bc1f87f34e45 46 /*else if( (range > STOP_DISTANCE) && (range <= TURN_DISTANCE) ){
tomoya123 0:bc1f87f34e45 47 printf("ok2\r\n");
tomoya123 0:bc1f87f34e45 48 mortor1.rotf(NORMAL_SPEED);
tomoya123 0:bc1f87f34e45 49 mortor2.rotf(TURN_SPEED);
tomoya123 0:bc1f87f34e45 50
tomoya123 0:bc1f87f34e45 51 }*/
tomoya123 0:bc1f87f34e45 52 else if( (range > TURN_DISTANCE) && (range <= SAFE_DISTANCE) ){
tomoya123 0:bc1f87f34e45 53 printf("ok3\r\n");
tomoya123 0:bc1f87f34e45 54 mortor1.rotf(NORMAL_SPEED);
tomoya123 0:bc1f87f34e45 55 mortor2.rotf(NORMAL_SPEED);
tomoya123 0:bc1f87f34e45 56 //mortor1.rotb(NORMAL_SPEED);
tomoya123 0:bc1f87f34e45 57 //mortor2.rotb(NORMAL_SPEED);
tomoya123 0:bc1f87f34e45 58 }
tomoya123 0:bc1f87f34e45 59 else{
tomoya123 0:bc1f87f34e45 60 printf("ok4\r\n");
tomoya123 0:bc1f87f34e45 61 mortor1.rotf(FAST_SPEED);
tomoya123 0:bc1f87f34e45 62 mortor2.rotf(FAST_SPEED);
tomoya123 0:bc1f87f34e45 63 //mortor1.rotb(FAST_SPEED);
tomoya123 0:bc1f87f34e45 64 //mortor2.rotb(FAST_SPEED);
tomoya123 0:bc1f87f34e45 65 }
tomoya123 0:bc1f87f34e45 66 }
tomoya123 0:bc1f87f34e45 67 }