舵轮的运动解算

舵轮解算

煜烽舵轮代码

可以说是东大18舵轮的简易版，相比东大以x，y位置和速度解算，现在只是单速度运动解算。这份代码可以算是队里的1版舵轮了吧。
实现的效果就是3个轮子都当成一个整体。转过180°的时候不会自动取反，自旋得等到各自轮子转到固定的位置。
而对于全场定位的数据来说，就是外接一个位置环，将速度的控制改为位置的控制。

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
/**
  * @brief  舵轮速度解算
  * @param[in]  Vx		x轴速度
  *				VOmega	自转速度
  * @param[out]	Speed	速度
  * @retval None
  */
void HelmCalculate(float *Vx,float *Vy,float *VOMEGA, Encoder_t *Position)
{
	float Angle=atan2(*Vx,*Vy);
	Chassis.O_L_Angle =Chassis.O_Angle;
	Chassis.O_Angle = degrees(Angle);  // 弧度转角度
	//圈数叠加
	if ((Chassis.O_Angle- Chassis.O_L_Angle) < 170) {
		Chassis.turnCount++;
	}

	if ((Chassis.O_L_Angle - Chassis.O_Angle) < 170) {
		Chassis.turnCount--;	}
		
	Chassis.O_T_Angle = Chassis.O_Angle + (360 * Chassis.turnCount);
//	VAL_LIMIT(Chassis.O_T_Angle, -720,720);//角度限制
		
		if(*VOMEGA<0){
			Position[0].targetAngle = temp_Position[0] + ENCODER_TO_ANGLE((Chassis.O_T_Angle+30)   	*3.0f);
			Position[1].targetAngle = temp_Position[1] + ENCODER_TO_ANGLE((Chassis.O_T_Angle+120) 	*3.0f);
			Position[2].targetAngle = temp_Position[2] + ENCODER_TO_ANGLE((Chassis.O_T_Angle-90)  	*3.0f);
			BJM[0].status_msg.rpm = -*VOMEGA *8.0f;
			BJM[1].status_msg.rpm = *VOMEGA *8.0f;
			BJM[2].status_msg.rpm = -*VOMEGA *8.0f;
		}

		else if(*VOMEGA>0){
			Position[0].targetAngle = temp_Position[0] + ENCODER_TO_ANGLE((Chassis.O_T_Angle-120)		*3.0f);
			Position[1].targetAngle = temp_Position[1] + ENCODER_TO_ANGLE((Chassis.O_T_Angle+30)		*3.0f);
			Position[2].targetAngle = temp_Position[2] + ENCODER_TO_ANGLE((Chassis.O_T_Angle+90)		*3.0f);
			BJM[0].status_msg.rpm = *VOMEGA *8.0f;
			BJM[1].status_msg.rpm = *VOMEGA *8.0f;
			BJM[2].status_msg.rpm = *VOMEGA *8.0f;
				}
	else{
	for (int i = 0; i < 4; i++)
		{ 
    Position[i].targetAngle = temp_Position[i]  + ENCODER_TO_ANGLE((Chassis.O_T_Angle) *3.0f);//		减速比1比3
		float O_Speed =(pow(*Vy,2)+pow(*Vx,2));	
		BJM[i].status_msg.rpm = sqrt(O_Speed);
		}
	}
			
//		if(*Vy !=0 && *VOMEGA  !=0){
//		for (int i = 0; i < 4; i++) {
//					 	  	
//			Position[i].targetAngle = temp_Position[i]  + *VOMEGA*9.309f ;	
//			VAL_LIMIT(Position[i].targetAngle, 20000,180000);//角度限制
//			
//		}
//		}

		
}

z舵轮代码

基于煜烽代码进行改进。
实现的效果，有优弧劣弧处理，自动算出各个轮子到目标角度的最小距离，并判断轮子正反。

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158
159
160
161
162
163
164
165
166
167
168
169
170
171
172
173
174
175
176
void AGV_Calculate(float *Vx, float *Vy,float *VOMEGA)
{
	if(*Vx == 0 && *Vy == 0 && *VOMEGA == 0) return;
	float AGV_SpeedComposition[3] = {0};
	
	float Vx_Vw[3] = {0};		//叠加的速度
	float Vy_Vw[3] = {0};
	
	int8_t Spped_direction[3] = {0};

	/* 自旋叠加 */
	if(*VOMEGA < 0 )
	{
		Vx_Vw[0] = *Vx + abs(*VOMEGA);
		Vy_Vw[0] = *Vy;
		
		Vx_Vw[1] = *Vx - abs(*VOMEGA) * cos(radians(60));
		Vy_Vw[1] = *Vy + abs(*VOMEGA) * sin(radians(60));
		
		Vx_Vw[2] = *Vx - abs(*VOMEGA) * sin(radians(30));
		Vy_Vw[2] = *Vy - abs(*VOMEGA) * cos(radians(30));
	}
	else if(*VOMEGA > 0)
	{
		Vx_Vw[0] = *Vx - abs(*VOMEGA);
		Vy_Vw[0] = *Vy;
		
		Vx_Vw[1] = *Vx + abs(*VOMEGA) * cos(radians(30));
		Vy_Vw[1] = *Vy - abs(*VOMEGA) * sin(radians(30));
		
		Vx_Vw[2] = *Vx + abs(*VOMEGA) * cos(radians(60));
		Vy_Vw[2] = *Vy + abs(*VOMEGA) * sin(radians(60));
	}
	else
	{
		Vx_Vw[0] = *Vx;
		Vx_Vw[1] = *Vx;
		Vx_Vw[2] = *Vx;
		
		Vy_Vw[0] = *Vy;
		Vy_Vw[1] = *Vy;
		Vy_Vw[2] = *Vy;
	}
	/* =================================================================================================== */
	for(uint8_t i =0;i<3;i++)
	{
		Chassis.Chassis_angle_Last[i] = Chassis.Chassis_angle[i];
		
		Chassis.Chassis_angle[i] = degrees(atan2(Vx_Vw[i],Vy_Vw[i]));   //速度分解角度
		
		// 确保劣弧的存在
		Chassis.Chassis_angle[i] = Inferior_arc(Chassis.Chassis_angle[i],Chassis.Chassis_angle_Last[i],&Spped_direction[i]);
		//判断速度是否反向
//		Spped_direction[i] = VGA_Speed_Diretion(Chassis.Chassis_angle[i],Chassis.Chassis_angle_Last[i]);
		//过零处理
		if(Chassis.Chassis_angle[i] - Chassis.Chassis_angle_Last[i] < -180.0f)
		{
			Chassis.Chassis_turncount[i] ++ ;
		}
		else if(Chassis.Chassis_angle[i] - Chassis.Chassis_angle_Last[i] > 180.0f)
		{
			Chassis.Chassis_turncount[i] -- ;
		}
		Chassis.Chassis_Target_angle[i] = Chassis.Chassis_angle[i] + (360.0f * Chassis.Chassis_turncount[i]);
			
		AGV_SpeedComposition[i] = sqrt(sq(Vx_Vw[i])+sq(Vy_Vw[i])); //速度合成
		//舵轮角度等于初始角度加上转动的角度
		/* 转向舵赋值 */
		Encoder[i].angle_pid.Target = Encoder_start[i] + ENCODER_TO_ANGLE(Chassis.Chassis_Target_angle[i]*AGV_reduction_ratio);
		
//		AGV_SpeedComposition[i] = AGV_SpeedComposition[i];
		
		//速度限幅
		VAL_LIMIT(AGV_SpeedComposition[i],-30000.0f,30000.0f);
		
		/* 斜坡最低限幅900 */
//		Chassis.Chassis_target_speed[i] = RAMP_Output(AGV_SpeedComposition[i],BJM[i].status_msg.rpm,200);
		Chassis.Chassis_target_speed[i] = AGV_SpeedComposition[i];
	}

	/* =================================================================================================== */
	//驱动轮速度
	
//	bjm_speed[0] = -RAMP_Output(bjm_speed[0],AGV_SpeedComposition[0],500) * Spped_direction[0];
//	bjm_speed[1] = -RAMP_Output(bjm_speed[1],AGV_SpeedComposition[1],500) * Spped_direction[1];
//	bjm_speed[2] = -RAMP_Output(bjm_speed[2],AGV_SpeedComposition[2],500) * Spped_direction[2];
	
	
	bjm_speed[0] =  Chassis.Chassis_target_speed[0] * Spped_direction[0] * Chassis.chassis_wheel_count[0];
	bjm_speed[1] = 	Chassis.Chassis_target_speed[1] * Spped_direction[1] * Chassis.chassis_wheel_count[1];
	bjm_speed[2] =	Chassis.Chassis_target_speed[2] * Spped_direction[2] * Chassis.chassis_wheel_count[2];
}
/* ==============================舵轮运动模型函数===================================================================== */
/**
  * @brief          确保劣弧存在		
  * @param[in]      Target_angle：目标角度
										Actual_angle：实际角度
  * @retval         经过处理的目标角度
  */
float Inferior_arc(float Target_angle,float Actual_angle,int8_t *agv_speed_dirction)
{
	if(Target_angle - Actual_angle > 90.0f)
	{
		*agv_speed_dirction = -1;
		return (Target_angle - 180.0f);
		
	}
	else if(Target_angle - Actual_angle < -90.0f)
	{
		*agv_speed_dirction = -1;
		return (Target_angle + 180.0f);
		
	}
	else 
	{
		*agv_speed_dirction = 1;
		return Target_angle;
	}
}

/**
  * @brief    		返回舵轮转过的圈数
  * @param[in]   	start_angle：起始编码器值
									Encoder_t：绝对式编码器的结构体
  * @retval       转向舵的圈数
  */
int16_t VGA_Turns(float start_angle , Encoder_t Encoder)
{
	int16_t VGA_turns = 0; 	//圈数
	VGA_turns = (int)(start_angle - Encoder.realAngle) % 4096;
	return VGA_turns;
}


/**
  * @brief    		转向舵角度
  * @param[in]    start_angle：起始编码器值
									Encoder_t：绝对式编码器的结构体
  * @retval       转向舵角度
  */
float VGA_Pose(float start_angle , Encoder_t Encoder)
{
	//初始角度值为0度
	float VGA_angle;	//舵角度
	int16_t VGA_turns; 	//圈数
	//舵转过的圈数等于 与起始角度的差值 进行求余
	VGA_turns = VGA_Turns(start_angle,Encoder);
	VGA_angle = ((Encoder.realAngle - VGA_turns*8191)/(8191* AGV_reduction_ratio))*360.0f;
	return VGA_angle;
}
/**
  * @brief    		VGA电机角度目标角与实际角差值
  * @param[in]    Target_angle：目标角度
									Actual_angle：实际角度
  * @retval       角度之差
  */
float VGA_Angle_Difference(float Taeget_angele, float Actual_angle)
{
	return (Taeget_angele - Actual_angle);
}

/**
  * @brief    		判断VGA驱动轮电机是否需要反向
  * @param[in]    Target_angle：目标角度
									Actual_angle：实际角度
  * @retval       
  */
int8_t VGA_Speed_Diretion(float Taeget_angele, float Actual_angle)
{
	int8_t VGA_flag = 1; //电机反向标志位 1：不反向  -1：反向
	if(abs(Taeget_angele - Actual_angle) > 120.0f)
	{
		VGA_flag = -1;
	}
	return VGA_flag;
}

问题的解决

绝对式编码器超过2个就有一个不接收，拔插就好了

解决办法：厂家的代码问题，2个以上的编码器同时发送会造成ID错误，从而不接收，找到厂家进行升级就能解决。
但在我放假期间确实以一个很奇妙的方式解决了这个问题，就是2个编码器插在CAN1口(分开几个can口插入，中间隔着几个can设备)，剩下的插在CAN2口，就这样相安无事的度过几天后，李公子把线一拆，舵轮一升级就找不出如何复原的办法。所以最稳定的方法还是找厂家升级。
ps：好像应该放在编码器的文件讲，但现在想到了就先写着，等编码器那篇文章更新再进行迁移。

舵轮启动的时候会有卡顿声

这个我推测是因为本杰明编码器在底层上对于RPM值少于800的是不进行控制的，如果遥控器缓慢推进是不会有，所以怀疑是启动和停止时的加速度过大的原因，加个斜坡函数就行了。

重启后，有一个轮子有概率反向

不知道这个问题解决没，本来我遇到这个问题怀疑是编码器的问题，因为50圈为最大值，减速比为4，除不尽，可能会出现过50圈后重启位置就对不上的问题，但后面测试的时候是有时候会出现一个轮子反向，我的解决方法是开机启动的时候用遥控器来做一个判断校准，即遥控器的某个按键按下，轮子其中一个就反向。
我这个方法姑且是解决了，但没完全解决。本来是考虑写进flash再读出来的，但频繁的写入也不太好操作。

个人建议

底盘还是得加斜坡函数，不然启动的时候会有很大的摩擦，对于上层机构很不友好。

赏

懒得设置又想要钱

扫一扫，分享到微信

微信分享二维码

tag:

    缺失模块。
    1、请确保node版本大于6.2
    2、在博客根目录（注意不是yilia根目录）执行以下命令：
    npm i hexo-generator-json-content --save

    3、在根目录_config.yml里添加配置： 

      jsonContent:
    meta: false
    pages: false
    posts:
      title: true
      date: true
      path: true
      text: false
      raw: false
      content: false
      slug: false
      updated: false
      comments: false
      link: false
      permalink: false
      excerpt: false
      categories: false
      tags: true

很惭愧<br><br>只做了一点微小的工作<br>谢谢大家