舵机加上PD控制后就不转了，去掉之后就能转 - 智能车制作

#include "common.h"
#include "include.h"
//舵机相关
#define S3010_TPM TPM1
#define S3010_CH TPM_CH0 // PTB0
#define S3010_HZ (50)
uint8 CCD_BUFF[TSL1401_SIZE]; //存储CCD图像的数组
uint8 s3010_middle = 73; //舵机中心值：对应的PWM占空比为（73/1000）精度为1000u
uint8 picture_middle = 64;//图像中心值
uint8 leftline = 1; //赛道左边界
uint8 rightline = 127; //赛道右边界
struct s3010_pid //舵机PD控制
{
int err; //定义偏差值
int err_last; //定义上一个偏差值
float Kp; //定义比例系数
float Kd;//定义微分系数
}pid;
void PIT_IRQHandler();
void vcan_sendccd(uint8 *ccdaddr, uint32 ccdsize);
void bin(uint8 * bin, uint8 len);
void main(void)
{
uint8 time = 5; // 修改 PIT0 的定时时间即可修改曝光时间
uint32 s3010_pwm = s3010_middle;
int h = 0;
tsl1401_set_addrs(TSL1401_MAX,CCD_BUFF); //初始化线性CCD
tsl1401_init(time); //初始化线性CCD ，配置中断时间为 time
//配置中断函数和使能中断
set_vector_handler(PIT_VECTORn,PIT_IRQHandler);
enable_irq(PIT_IRQn);
tpm_pwm_init(S3010_TPM, S3010_CH,S3010_HZ,73); //初始化舵机 PWM
void PID_init(); //舵机PD算法初始化
uint32 PID_realize(int x);//舵机PD实现初始化
while(1)
{
tsl1401_get_img();//采集线性CCD 图像
bin((uint8 *)&CCD_BUFF[0], TSL1401_SIZE); //处理图像，找赛道边界
//问题就在这，如果把下面的代码换成tpm_pwm_duty(S3010_TPM, S3010_CH,s3010_pwm);这之类的，舵机就能转
h = 64 - picture_middle;
s3010_pwm = PID_realize( h );
tpm_pwm_duty(S3010_TPM, S3010_CH,s3010_pwm);
DELAY_MS(200);
vcan_sendccd((uint8 *)&CCD_BUFF[0],TSL1401_SIZE);//给上位机发送数据
}
}
void bin(uint8 * bin, uint8 len)
{
uint8 i;
for(i=10;i<63;i++)
{
if(ABS(bin[i - 1]-bin[i]) >= 80)
{
leftline = i;
break;
}
}
for(i=111;i>64;i--)
{
if(ABS(bin[i + 1]-bin[i]) >= 80)
{
rightline = i;
break;
}
}
#if 0
for(i = leftline; i < rightline; i++)
{
bin[i] = 255;
}
#endif
picture_middle = (uint8)((leftline + rightline) / 2);
bin[picture_middle] = 0;
}
// PIT中断服务函数由于 TSL1401_INT_TIME 配置为 PIT0 ，因而使用 PIT0
void PIT_IRQHandler()
{
if(PIT_TFLG(PIT0) == 1 ) //判断是否 PIT0 进入中断
{
tsl1401_time_isr();
PIT_Flag_Clear(PIT0);
}
}
//发送图像到上位机显示不同的上位机，不同的命令
void vcan_sendccd(uint8 *ccdaddr, uint32 ccdsize)
{
#define CMD_CCD 2
uint8 cmdf[2] = {CMD_CCD, ~CMD_CCD}; //开头命令
uint8 cmdr[2] = {~CMD_CCD, CMD_CCD}; //结尾命令
uart_putbuff(VCAN_PORT, cmdf, sizeof(cmdf)); //先发送命令
uart_putbuff(VCAN_PORT, ccdaddr, ccdsize); //再发送图像
uart_putbuff(VCAN_PORT, cmdr, sizeof(cmdr)); //再发送命令
}
void PID_init()
{
pid.err=0;
pid.err_last=0;
pid.Kp=0.7;
pid.Kd=0.3;
}
uint32 PID_realize(int x) //x为距离中间的偏差
{
uint8 out;
pid.err = x;
out = (uint8)(s3010_middle + pid.err * pid.Kp + (pid.err - pid.err_last) * pid.Kd);
pid.err_last = pid.err;
return out;
}

复制代码