haoDIY创好电子音响电脑科技DIY小制作发明移动版

1。集Ki和Kd为零并逐步增加KP让机器人开始振荡的零位。

2。增加Ki使机器人的响应越快，当它失去了平衡。王下应足够大，使倾斜角度不增加。机器人必须回到零位，如果它是倾斜的。

三.增加KD以减少振荡。的超调量也应减少到现在。

4。重复上述步骤，通过微调每个参数以达到最好的效果。

我用超声波距离传感器是us-020。它有四个引脚分别是VCC，制动，回声，和GND。它是由一个5V电源供电。触发和回声引脚分别连接到数字引脚9和8的Arduino。我们将使用newping图书馆从传感器得到的距离值。我们将阅读距离每隔100毫秒，如果值为0和20cm之间，我们将指挥机器人进行旋转。这应该足以使机器人远离障碍。

#包括“丝。”

#包括“i2cdev。H” #包括“MPU6050。H” #包括“数学。” #包括<< newping。”

#定义leftmotorpwmpin 6 #定义leftmotordirpin 7 #定义rightmotorpwmpin 5 #定义rightmotordirpin 4

#定义trigger_pin 9 #定义echo_pin 8 #定义max_distance 75

#定义KP 40 #定义KD 0.05 #定义Ki 40 #定义sampletime 0.005 #定义targetangle - 2.5

MPU6050微处理器； newping声纳（trigger_pin，echo_pin，max_distance；

int16_t配件，accz，捷瑞士； 挥发性int电机功率，gyrorate； 挥发性浮accangle，gyroangle，currentangle，prevangle = 0，错误，preverror = 0，errorsum = 0； 挥发性的字节数= 0；

无效setmotors（int leftmotorspeed，int rightmotorspeed）{ 如果（leftmotorspeed > = 0）{ analogWrite（leftmotorpwmpin，leftmotorspeed）； digitalwrite（leftmotordirpin，低）； } { 其他值（leftmotorpwmpin，255 + leftmotorspeed）； digitalwrite（leftmotordirpin，高）； } 如果（rightmotorspeed > = 0）{ analogWrite（rightmotorpwmpin，rightmotorspeed）； digitalwrite（rightmotordirpin，低）； } { 其他值（rightmotorpwmpin，255 + rightmotorspeed）； digitalwrite（rightmotordirpin

无效init_pid() { //初始化定时器1 cli()；/ /禁用全局中断 tccr1a = 0；//设置整个tccr1a登记0 tccr1b = 0；//同tccr1b /设置比较匹配登记设定采样时间5ms ocr1a = 9999； / /打开 CTC模式tccr1b | =（1 << wgm12）； /集cs11位预分频的8 tccr1b | =（1 << cs11）； /使定时器中断 比较timsk1 | =（1 << ocie1a）； sei()；/使

无效setup() { /设定电机控制和PWM引脚输出模式 pinmode（leftmotorpwmpin，输出）； pinmode（leftmotordirpin，输出）； pinmode（rightmotorpwmpin，输出）； pinmode（rightmotordirpin，输出）； /设置状态导致输出模式 pinmode（13，输出 ）；//初始化MPU6050，设置偏移值 微处理器。initialize()； 微处理器。setyacceloffset（1593）； 微处理器。setzacceloffset（963）； 微处理器。setxgyrooffset（40）； /

无效loop() { /读值加速度和陀螺仪 配件=微处理器。getaccelerationy()； accz =微处理器。getaccelerationz()； 捷瑞士=微处理器。getrotationx()；设置电机功率经过约束它 /功率=约束（电机功率，255，255）； setmotors（电机功率，电机功率）； /测距离每100毫秒 如果（（计数为20）= = 0）{ distancecm =声纳。ping_cm()； } 如果（（distancecm <！= 0)) { setMotors(-motorPower, motorPower); } } // The ISR will be called every 5 milliseconds ISR(TIMER1_COMPA_vect) { // calculate the angle of inclination accAngle = atan2(accY, accZ)*RAD_TO_DEG; gyroRate = map(gyroX, -32768, 32767, -250, 250); gyroAngle = (float)gyroRate*sampleTime; currentAngle = 0.9934*(prevAngle + gyroAngle) + 0.0066*(accAngle); error = currentAngle - targetAngle; errorSum = errorSum + error; errorSum = constrain(errorSum, -300, 300); //calculate output from P, I and D values motorPower = Kp*(error) + Ki*(errorSum)*sampleTime - Kd*(currentAngle-prevAngle)/sampleTime; prevAngle = currentAngle; // toggle the led on pin13 every second count++; if(count == 200) { count = 0; digitalWrite(13, !//读出数字接口的值（13））；}

在调整PID常数花多一点的时间会给我们一个更好的结果。我们机器人的尺寸也限制了我们所能达到的稳定水平。这是比建立一个小的像我们这样的一个容易的建立一个全尺寸的平衡机器人。不过，我想，我们的机器人在各种表面上的平衡，如视频所示是一个相当不错的工作。