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 t
4000
he 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);

}