2015国电设风力摆练手

2015国电设风力摆练手

继续总结一下之前的经验，东西不多，随便讲讲吧。

源码链接https://download.csdn.net/download/YOUNGAAAAA/85005265

4个空心杯电机和一个陀螺仪，加上主控和驱动板。
驱动板建议用栅极驱动器或者btn，输出功率大，控制效果好

风力摆底部当然越轻越好，然后在底部放一个激光模块。
整体思路就是速度环pid控制空心杯电机，再在速度环外部加一个位置环pid，控制风力摆的位置。

代码

控制函数，放定时器中断里，

为什么设置了两套pid参数，是因为发现静止时的pid和运动中的pid差别挺大，经验之谈，用两套效果更好而已。

void mode_select(int mode,float length)
{
	static int first = 1;
	float mode1_angle = 0;
	if(first == 1)
	{
		first = 0;
		if(mode == 0)
		{
			Increment_PID_Initialize(&speed_pid[0],50,1,2000);   // x  50,1,2000
			Increment_PID_Initialize(&speed_pid[1],50,1,2000);   // y  50,1,2000
		}
		else if(mode == 1 || mode == 2)
		{
			Increment_PID_Initialize(&speed_pid[2],20,0.1,30);   // x
			Increment_PID_Initialize(&speed_pid[3],20,0.1,30);   // y
		}
	}
	switch (mode)
	{
		case 0: // 静止
			angle_control(0,5,5);
				break;
		case 1: // 直线

			straight_cal(0,length);
			straight_control(0);

				break;
		case 2:
			circle_cal(length);

				break;
	}

}

模式0 静止

这个很简单，读取当前角度值，反馈让其定点在一个固定角度

void angle_control(int mode,float angle_x,float angle_y)
{

	if(mode == 0){
		motor_pwm_y = Increment_PID(&speed_pid[1],angle_y,jy_y);
		motor_pwm_x = Increment_PID(&speed_pid[0],angle_x,jy_x);
	}
	else if(mode == 1){
		motor_pwm_y = Increment_PID(&speed_pid[3],angle_y,-an_speed_y);
		motor_pwm_x = Increment_PID(&speed_pid[2],angle_x,an_speed_x);
	}

	// 上下运动
	if(motor_pwm_y >= 0)
	{
		if(motor_pwm_y >= 400)
			motor_pwm_y = 400;
		__HAL_TIM_SET_COMPARE(&htim1,TIM_CHANNEL_1,0);
		__HAL_TIM_SET_COMPARE(&htim1,TIM_CHANNEL_2,abs(motor_pwm_y));
	}
	else
	{
		if(motor_pwm_y <= -400)
			motor_pwm_y = -400;
		__HAL_TIM_SET_COMPARE(&htim1,TIM_CHANNEL_2,0);
		__HAL_TIM_SET_COMPARE(&htim1,TIM_CHANNEL_1,abs(motor_pwm_y));
	}

	// 左右运动
	if(motor_pwm_x >= 0)
	{
		if(motor_pwm_x >= 400)
			motor_pwm_x = 400;
		__HAL_TIM_SET_COMPARE(&htim1,TIM_CHANNEL_4,0);
		__HAL_TIM_SET_COMPARE(&htim1,TIM_CHANNEL_3,abs(motor_pwm_x));
	}
	else
	{
		if(motor_pwm_x <= -400)
			motor_pwm_x = -400;
		__HAL_TIM_SET_COMPARE(&htim1,TIM_CHANNEL_3,0);
		__HAL_TIM_SET_COMPARE(&htim1,TIM_CHANNEL_4,abs(motor_pwm_x));
	}

}

模式1 直线

思路很清晰，就是利用能量守恒的原理，在任何位置重力势能加上动能应该始终不变，反馈的量应该是能量的总和。这样就能画出指定长度的直线

void straight_control(int mode)
{
	if(mode == 0){
		if( an_speed_x >= 0){
			angle_control(1,ang_speed,0);
		}
		else{
			angle_control(1,-ang_speed,0);
		}
	}
	else if(mode == 1){
		if( an_speed_y >= 0){
			angle_control(1,0,ang_speed);
		}
		else{
			angle_control(1,0,-ang_speed);
		}
	}

}

void  straight_cal(int mode,float length)
{
	//mode: 0-x   1-y
	//standard_energy
	float tan_cita= 0,cos_cita = 0,hight_diff = 0,standard_energy = 0;
	tan_cita = length/88;
	cos_cita = 1/(sqrt(tan_cita*tan_cita+1));
	hight_diff = 72 - 72*cos_cita;
	standard_energy = 9.8*hight_diff*0.01;

	//current_enerty
	float diff_energy = 0,current_high = 0;
	if(mode == 0){
		tan_cita =tan( jy_x*M_PI/180);
		cos_cita = 1/(sqrt(tan_cita*tan_cita+1));
		current_high = 72 - 72*cos_cita;
	}
	else if(mode == 1){
		tan_cita =tan( jy_x*M_PI/180);
		cos_cita = 1/(sqrt(tan_cita*tan_cita+1));
		current_high = 72 - 72*cos_cita;
	}

	diff_energy = standard_energy - 9.8*current_high*0.01;
	if(diff_energy>=0){
		ang_speed = sqrt(fabs(diff_energy)*2)/(0.72)*100;
	}
	else{
		ang_speed = -sqrt(fabs(diff_energy)*2)/(0.72)*100;
	}

}

效果如图

模式3 圆形

画指定圆形的代码较为复杂，实际上还是和物理挂钩，维持能量守恒，然后将速度分解为x和y方向，然后用x和y方向的电机分别控制，即可得到响应的结果

void circle_cal(float length)
{
	float tan_cita= 0,tan_x = 0,tan_y = 0,tan_cigma = 0,v_sum = 0,v_x = 0,v_y = 0;
	tan_cita = length / 88;
	v_sum = tan_cita*sqrt(9.8*0.72);
	tan_x = tan( jy_x*M_PI/180);
	tan_y = -tan( jy_y*M_PI/180);
	tan_cigma = sqrt(tan_x*tan_x + tan_y*tan_y);
	if (tan_cigma != 0){
		v_x = v_sum*tan_y/tan_cigma/(0.72)*100;
		v_y = v_sum*tan_x/tan_cigma/(0.72)*100;
	}
	test[0] = tan_cigma;
	test[1] = v_x;
	test[2] = v_y;
	angle_control(1,v_x,v_y);

}

视频找不到了，当时效果不错，鲁棒性也挺好的，无论是风吹还是手扰动都能自己调回来