树莓派RP2040开发板,智能小车速度PID控制程序硬件:树莓派 pico开发板,或微雪rp2040-pico-zero开发板,或esp8266,esp32(需要修改引脚)智能小车底盘功放模块
软件平台:micro python固件,thonny
#代码中self.target_speed=[30,30] 是目标速度
def callback_recordtime(self,s):
self.timecunt_oldold[0]=self.timecunt_old[0]
self.timecunt_old[0]=self.timecunt
print("speed:",self.speed)
def callback_recordtime2(self,s):
self.timecunt_oldold[1]=self.timecunt_old[1]
self.timecunt_old[1]=self.timecunt
print("speed:",self.speed)
def cmt_speed(self):
for i in range(2):
t1=self.timecunt_old[i]-self.timecunt_oldold[i]
t2=self.timecunt-self.timecunt_old[i]
timecost=max(t1,t2)
if timecost<1:
timecost=1
self.speed[i]=1000/timecost
if timecost>1000:
self.speed[i]=0
if self.timecunt_oldold[i]<1:
self.speed[i]=0
def cmt_current(self):
out=[0,0]
for i in range(2):
self.speed_error[i]=(self.target_speed[i]- self.speed[i])
self.speed_error_I[i]=self.speed_error_I[i] self.speed_error[i]
self.speed_error_D[i]=self.speed_error[i]-self.speed_error_old[i]
pout=self.kp*self.speed_error[i]
iout=self.ki*self.speed_error_I[i]
dout=self.kd*self.speed_error_D[i]
self.speed_error_old[i]=self.speed_error[i]
out[i]=pout iout dout
self.pwm1.duty_u16(int(out[0]))
self.pwm2.duty_u16(int(out[1]))
def showRGBLED(self):
vR=int(self.brightness self.brightness*sin(0.01*self.timecunt))
vG=int(self.brightness self.brightness*sin(0.01*self.timecunt 2.0944))
vB=int(self.brightness self.brightness*sin(0.01*self.timecunt 2.0944 2.0944))
self.np.set_pixel(0,vR,vG,vB)
self.np.show()
评论