Fix counter output interval and sampling timing (#11)

* Fix counter output interval and sampling timing

* Fix encoder value buffering timing

* Fix PWM rate sum

Author: at-wat

tfrog-motordriver/controlVelocity.c

@@ -28,6 +28,8 @@ DriverParam driver_param;
 short com_cnts[COM_MOTORS];
 short com_pwms[COM_MOTORS];
 char com_en[COM_MOTORS];
+int pwm_sum[2] = { 0, 0 };
+int pwm_num[2] = { 0, 0 };
 
 extern int watchdog;
 extern int velcontrol;
@@ -56,16 +58,11 @@ void timer0_vel_calc( ) RAMFUNC;
 void ISR_VelocityControl(  )
 {
 	// volatile unsigned int status;
-	static int pwm_sum[2] = { 0, 0 };
 	int i;
 	int64_t toq[2];
 	int64_t out_pwm[2];
 	int64_t acc[2];
 
-	if( motor[0].servo_level > SERVO_LEVEL_STOP ||
-			motor[1].servo_level > SERVO_LEVEL_STOP )
-		driver_param.cnt_updated++;
-
 	for( i = 0; i < 2; i++ )
 	{
 		if( motor[i].servo_level >= SERVO_LEVEL_TORQUE )
@@ -233,12 +230,7 @@ void ISR_VelocityControl(  )
 			motor[i].ref.rate = out_pwm[i];
 
 			pwm_sum[i] += out_pwm[i];
-
-			if( driver_param.cnt_updated == 5 )
-			{
-				motor[i].ref.rate_buf = pwm_sum[i];
-				pwm_sum[i] = 0;
-			}
+      pwm_num[i]++;
 		}
 	}
 }
@@ -256,7 +248,9 @@ void timer0_vel_calc( )
 			motor[1].servo_level > SERVO_LEVEL_STOP )
 	{
 		driver_param.watchdog ++;
+		driver_param.cnt_updated++;
 	}
+
 
 	dummy = AT91C_BASE_TC0->TC_SR;
 	dummy = dummy;
@@ -322,6 +316,19 @@ void timer0_vel_calc( )
 		__enc[i] = enc[i];
 		motor[i].enc_buf = enc[i] >> motor_param[i].enc_div;
 	}
+	for( i = 0; i < 2; i++ )
+	{
+		if( motor[i].servo_level >= SERVO_LEVEL_TORQUE )
+		{
+			if( driver_param.cnt_updated == 5 )
+			{
+				motor[i].ref.rate_buf = pwm_sum[i] * 5 / pwm_num[i];
+				motor[i].enc_buf2 = motor[i].enc_buf;
+				pwm_sum[i] = 0;
+				pwm_num[i] = 0;
+			}
+		}
+	}
 
 	LED_off(1);
 

tfrog-motordriver/controlVelocity.h

@@ -16,6 +16,7 @@ typedef struct _MotorState
 	int pos;									// count
 	unsigned int posc;							// count
 	int enc_buf;								// count
+	int enc_buf2;								// count
 	int spd;
 	int spd_cnt;
 	unsigned short enc;

tfrog-motordriver/main.c

@@ -910,6 +910,9 @@ int main(  )
 			int i;
 			// static long cnt = 0;
 			/* 約5msおき */
+			driver_param.cnt_updated -= 5;
+			if( driver_param.cnt_updated >= 5 )
+				driver_param.cnt_updated -= 5;
 
 			mask = driver_param.admask;			// analog_mask;
 			if( driver_param.io_mask[0] )
@@ -939,14 +942,14 @@ int main(  )
 			com_pwms[1] = motor[1].ref.rate_buf;
 			if( driver_param.ifmode == 0 )
 			{
-				com_cnts[0] = motor[0].enc_buf;
-				com_cnts[1] = motor[1].enc_buf;
+				com_cnts[0] = motor[0].enc_buf2;
+				com_cnts[1] = motor[1].enc_buf2;
 				data_send( com_cnts, com_pwms, com_en, analog, mask );
 			}
 			else
 			{
-				com_cnts[0] = ( short )motor[0].enc_buf;
-				com_cnts[1] = ( short )motor[1].enc_buf;
+				com_cnts[0] = ( short )motor[0].enc_buf2;
+				com_cnts[1] = ( short )motor[1].enc_buf2;
 				data_send485( com_cnts, com_pwms, com_en, analog, mask );
 			}
 			for(i = 2; i < COM_MOTORS; i ++)
@@ -979,7 +982,6 @@ int main(  )
 				com_index[i][1] = index_f;
 			}
 
-			driver_param.cnt_updated = 0;
 			driver_param.vsrc = Filter1st_Filter( &voltf, (int)( analog[ 7 ] & 0x0FFF ) );
 			ADC_Start();