ROBOSTEP4期 / Mbed 2 deprecated NHK_kaida_ros0329

射出(3/29用)

Dependencies:   mbed ros_lib_kinetic

Fork of NHK_kaida_ros0322_4 by ROBOSTEP4期

uw.cpp

Committer:
Arare
Date:
2018-04-09
Revision:
23:ffe4f63bee36
Parent:
0:f6fa58c56955

File content as of revision 23:ffe4f63bee36:

#include "uw.h"
#include "mbed.h"

Uw::Uw(PinName uw)   :   uw_(uw) {
    dist = 0;
}

double Uw::get_dist(int rank){
    double res;  
    
    uw_.output();
    uw_=0;
    wait_us(10);
    uw_=1;
    wait_us(5);
    uw_=0;
    
    uw_.input();    
    t.start();
    while(!uw_){
        t.reset();
        t.start();
        }
    do{
        res=t.read_us();        
    }while(uw_);
    t.reset();
    t.stop();
    
    
    dist=(res/2)/29.1;  //[cm]=[m/us]/[1/(us*100)]
    //  =       *343.6  //[cm]=[m/us]*[us*100]
    
    switch(rank){
        case 1:
        return dist*10;
        case 2:
        return dist*0.01;
        default:
        return dist;
    }
}