位置速度串级PID

/*-------------------------------------------------主控代码-------------------------------------------------*/
void HAL_GPIO_EXTI_Callback(uint16_t GPIO_Pin)
{
	//	static unsigned char timecnt;
	//	if (++timecnt >= 100)	//500ms
	//	{
	//		HAL_GPIO_TogglePin(GPIOC, GPIO_PIN_13);	//500ms翻转PC13电平,测试500ms，为了测试10ms外部中断，用示波器观测也是10ms一个下降沿
	//		timecnt = 0;
	//	}

	// 10ms进入一次外部中断   PA5
	
	
	
	// 1.10ms定时读取编码器数据 --- 实际脉冲数 --- 当做实际速度Reality_Velocity
	Encoder_Left  =  Read_Encoder_Pulse(L_ENCODER);	// 读取左编码器数据
	Encoder_Right = -Read_Encoder_Pulse(R_ENCODER); // 读取右编码器数据
	
	// 2.计算期望位置、期望速度
	Target_Velocity = Rpm_Encoder_Cnt(rpm);		/* 将转速转化为10ms的脉冲数，目标速度 */
	Target_Position = Pulse_Encoder_Cnt(num);	/* 将圈数转化为目标脉冲数，目标位置 */
	
	
	// 编码器采集的数据累加 --- 当做实际位置Reality_Position
	Cumulative_Pulse_L += Encoder_Left;		 /* 实际位置脉冲数 */
	Cumulative_Pulse_R += Encoder_Right;
	
//	// 计算左右轮 当前转速
//	Moto_Speed_L = Motor_Real_Speed(Encoder_Left);		//用单位时间内的脉冲来计算
//	Moto_Speed_R = Motor_Real_Speed(Encoder_Right);	
	//计算当前转数
	real_num = Num_AllEncoder(Cumulative_Pulse_L);
	
	// MPU6050读取
	// MPU_Get_Gyroscope(&gx, &gy, &gz);
	mpu_dmp_get_data(&pitch, &roll, &yaw);

	PWM_OUT_L = Position_PID_Left(Cumulative_Pulse_L, Target_Position);	//左轮 实际位置，期望位置
	PWM_OUT_R = Position_PID_Right(Cumulative_Pulse_R, Target_Position);	//右轮 实际位置，期望位置
	
	PWM_OUT_L = Xianfu(PWM_OUT_L, Target_Velocity);                    /* 位置环输出限幅，经过手动计算，这里的限幅，假设100转，限幅=1000，假设200转，限幅=2000*/
	PWM_OUT_L = Xianfu(PWM_OUT_L, Rpm_Encoder_Cnt(Rpm_Max)); 			//限幅在最大转速以内
	PWM_OUT_R = Xianfu(PWM_OUT_R, Target_Velocity);
	PWM_OUT_R = Xianfu(PWM_OUT_R, Rpm_Encoder_Cnt(Rpm_Max)); 			//限幅在最大转速以内
	
	//Yaw角度修正
	Yaw_PWM = MPU6050_Yaw_correct(yaw, 0.0);
	
	/* 左轮 - 滤除部分干扰 */
	if ( My_ABS(Target_Position - Cumulative_Pulse_L) <= 3  )	
	{
		PWM_OUT_L = 0;
		Load_PWMA_L(PWM_OUT_L - Yaw_PWM);
	}
	else
	{
		PWM_OUT_L = Incremental_PID_Left(Encoder_Left, PWM_OUT_L);      /* 增量式速度控制 */   //脉冲当做实际速度，期望速度是位置式PID计算出来的PWM
        Load_PWMA_L(PWM_OUT_L - Yaw_PWM);
	}
	

	
	 /* 右轮 - 滤除部分干扰 */
	if ( My_ABS(Target_Position - Cumulative_Pulse_R) <= 3 )	
	{
		PWM_OUT_R = 0;
		Load_PWMB_R(PWM_OUT_R + Yaw_PWM);
	}
	else
	{
		PWM_OUT_R = Incremental_PID_Right(Encoder_Right, PWM_OUT_R);      /* 增量式速度控制 */
        Load_PWMB_R(PWM_OUT_R + Yaw_PWM);                                   	/* 赋值 */
	}

//	
//	// PWM装载
//	Load_PWM(MOTO_L, MOTO_R);
}
/*-------------------------------------------------主控代码-------------------------------------------------*/