変数の計算

[C素人]

PICマイコン（コンパイラC18)の環境でライントレーサーのPID制御にチャレンジしています。

センサは、左右に4個ずつ配置。（白: Hi、黒: Low)
左右のモータをPWM制御する方式です。

センサの位置情報(sensor_val:-7～+7)からPWM Dutyを計算により算出しています。
0がライン中央に機体がある状態であり、そこからの偏差に対してPID制御を検討しています。

PWM Dutyの計算は、割り込み処理(1msec周期)内で行っています。


[質問]
P制御を行う場合、
Kp=73
Ki=0
Kd=0　で計算すると、

机上の計算では、sensor_val=1のとき、

pwm_l=585
pwm_r=439 となるはずですが、（プログラムもそのように計算されるように記述しているつもりです。）

実際、左右のPWMの波形を測定してみると、左右共に100%となってしまっています。


変数mvに直接値を代入すれば、その値に応じたpwm波形が出力されることは確認できています。
変数mvの計算方法に問題がありそうと思っているのですが、どこが問題なのかが分かりません。

コード:

void main(void){

//ポート設定など省略

//PWM設定 Timer2= 15.625kHz
	OpenPWM1(0xFF);
	OpenPWM2(0xFF);
	OpenTimer2(TIMER_INT_OFF & T2_PS_1_1 & T2_POST_1_1 );
	pwm_l=512;		//PWM Duty max1023
	pwm_r=512;		//PWM Duty max1023

//1msec割り込みタイマ設定
	OpenTimer0(TIMER_INT_ON & T0_16BIT & T0_SOURCE_INT & T0_PS_1_2);
	WriteTimer0(63536);


 while(1){
	LED_ON=1;			//センサLED ON
	SetDCPWM1(pwm_l);		//左モータ用 PWM
	SetDCPWM2(pwm_r);		//右モータ用 PWM
		}			
}


#pragma interrupt pid   		//pwm割り込みの宣言
#pragma code isr_vector = 0x08		//割込みベクタにジャンプ命令をセット
void goto_isr(void)
{
	_asm
		Goto pid
	_endasm
}

#pragma code 
void pid(void)  			//割込み処理関数
{
	sen = !(PORTA) & 0xFF;
	switch(sen){
		case 0x80:
            sensor_val=-7;
			break;
		case 0xC0:
            sensor_val=-6;
			break;            
		case 0x40:
            sensor_val=-5;
			break;
		case 0x60:
            sensor_val=-4;
			break;
		case 0x20:
            sensor_val=-3;
			break;            
		case 0x30:
            sensor_val=-2;
			break;            
		case 0x10:
            sensor_val=-1;
			break;            
		case 0x18:
            sensor_val=0;
			break;            
		case 0x08:
            sensor_val=1;
			break;            
		case 0x0C:
            sensor_val=2;
			break;
		case 0x04:
            sensor_val=3;
			break;
		case 0x06:
            sensor_val=4;
			break;
		case 0x02:
            sensor_val=5;
			break;
		case 0x03:
            sensor_val=6;
			break;
		case 0x01:
            sensor_val=7;
			break;
        }
        
        diff[0]=diff[1];
        diff[1]=sensor_val-target_val;
        integral+=(diff[1]+diff[0])/2.0*DELTA_T;	//daikei面積
   
        p=Kp*diff[1];
        i=Ki*integral;
        d=Kd *(diff[1]-diff[0])/DELTA_T;

	mv=p+i+d;
	PWM(mv);

	INTCONbits.TMR0IF = 0; 	//タイマ0割り込みフラグのクリア
	WriteTimer0(63536);
}


void PWM(int mv){

	pwm_l=512 + mv;
	pwm_r=512 - mv;

	if(pwm_l>=1023)
	{
		pwm_l=1023;
	}
	if(pwm_l<0)
	{
		pwm_l=0;
	}
	if(pwm_r>=1023)
	{
		pwm_r=1023;
	}
	if(pwm_r<0)
	{
		pwm_r=0;
	}

}

[みけCAT]

提示されたコードには定義されていない識別子が多いですが、
その中でも関係がありそうなtarget_valとは何ですか？

[C素人]

みけCATさま

返信ありがとうございます。

変数は、以下の通り宣言しています。

target_valは、目的値として0を初期値として代入しています。

コード:

/**********************************
	    定数、変数の宣言定義
***********************************/
static unsigned int sen;                 //Sensor
static signed int sensor_val,target_val=0;             //Sensor
static unsigned int pwm_l,pwm_r;       //PWM Duty

#define DELTA_T 0.001	//処理周期 10msec
#define Kp 73.0        //PID係数
#define Ki 0.0        //PID係数
#define Kd 0.0       //PID係数

//static signed double DELTA_T=0.01;        //処理周期 10msec
//static signed double Kp=2.0;        //PID係数
//static signed double Ki=0.0;        //PID係数
//static signed double Kd=0.0;        //PID係数

static signed int diff[2];
static float long integral=0;

static signed int p,i,d,mv;

[C素人]

解決できました。