iic协议扩展板和pmw引脚控制舵机转向与超声测距的实验,可实现简易超声雷达

iic协议扩展板和pmw引脚控制舵机转向与超声测距的实验,可实现简易超声雷达

//
#include <ST_HW_HC_SR04.h>
#include <Wire.h>
#include <Adafruit_PWMServoDriver.h>
#include <Servo.h>
Adafruit_PWMServoDriver pwm = Adafruit_PWMServoDriver();
Servo myservo; 
// Change the pins if you wish.
ST_HW_HC_SR04 ultrasonicSensor(13, 12); // ST_HW_HC_SR04(TRIG, ECHO)
unsigned int ddd=0;
char dis=16;//分辨率1024/16=64，每个运动周期往返采样两次，期望每秒采128个点，1000/128*34.4=268.76cm（测量范围<=134cm）
void sendToIIC(byte _AAA,byte _aaa,unsigned int _ddd){
  //50hz 相当于20ms(20000us)的 周期,  4096分辨率，分辨率每单位占用时间t 为20000us/4096=4.8828125us
  //舵机控制所需脉宽Th范围0.5~2.5ms（500~2500us）因此高电平分辨率范围就是Th/t =(102.4~512)个单位。
  //取整 最小103 最大512 范围是512-103=409；
  //pwm.setAddr(0x40+_AAA);
  float per=(float)_ddd/1024.0;//参数ddd为10位正整数最大1024
  uint16_t offNum=103+int(409*per);
  //pwm.setPWM(num,on,off);num pmw的id代号,on 上升沿（在0-4096之间）开始位置,off 下降沿在（在0-4096之间）的开始位置
  pwm.setPWM(_aaa, 0, offNum);

}
void setup() {
    Serial.begin(115200);

    while(!Serial); // Wait for the Serial connection;
    ultrasonicSensor.setTimeout(23200);//23.2*34.4=798cm(8m)最大测量范围4m 
    //iic
    pwm.begin();  
    pwm.setPWMFreq(50); 
    //pmw引脚
    myservo.attach(9);  // attaches the servo on pin 9 to the servo object
}

void loop() {
    //iic
    //sendToIIC(0,0,ddd);
    //pmw引脚 scale it to use it with the servo (value between 0 and 180) 
    myservo.write(map(ddd, 0, 1024, 0, 180)); 
  
    ddd+=dis;
    if(ddd>=1024||ddd<=0){
      dis*=-1;
    }
    

  
    int hitTime = ultrasonicSensor.getHitTime(); // In microseconds
    float d=(float)hitTime/1000.0*34.4;
    String message = String(d) + "cm\n";

    /*
    #
    # This means the sensor didn't receive back the packets before the timeout
    #
    #   That usually happens when the distance between the sensor and the block-
    # ing object is greater than 86 centimeters (33.85 inches).
    #
    #   Increasing the timeout from 5000 microseconds to 23200 microseconds
    # will increase the maximum distance to ~4m, at the cost of the code being
    # stuck for 23.2 ms if no packet is received during this period (worst case
    # scenario).
    #
    #   More information (including calculations) is included in the
    # extras/HC-SR04.txt file. (Inside your libraries/ST_HW_HC_SR04 folder)
    #
    */
    if((hitTime == 0) && (ultrasonicSensor.getTimeout() == 5000)) {
        
    }
    //23.2ms声波传播 23.2*34.4=8米，测量4米
    int costTime= hitTime*2/1000;//ms
    if(costTime<7){
      delay((7-costTime)); // 128hz 期望测量周期为7.8125ms （测量范围<=134cm）
    }else{
      //超出期望周期说明测两距离过长。
     //测量周期变长，测量频率减慢 最慢1000/23.2=43hz
     }
    Serial.print(message);

    

}