balance

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
//-----------------------------------------------------------------------------
// PWM计算函数
//-----------------------------------------------------------------------------
void PWM_calculate(void)
{
if(fabs(angle)>0.17)	HB_Angle = 0;
//如果车体角度大于10°,禁止前后移动
Wheel_angle_dot = (Lwheel_Counter + Rwheel_Counter)*0.5;
//取编码平均值
Wheel_angle_dot_filter *= 0.85;
//
0.85-|
Wheel_angle_dot_filter += Wheel_angle_dot*0.15;
//
0.15-|-编码滤波
Wheel_angle += Wheel_angle_dot_filter;
//统计编码器数值，为下面的计算做准备
Wheel_angle_dot = Wheel_angle_dot_filter;
//赋值给车轮角速度变量，为下面的计算做准备
Wheel_angle += HB_Angle/9.5;
//根据接收到无线模块的数据改变车的前后移动
if (Wheel_angle<-768)
Wheel_angle=-768;	//
else if
(Wheel_angle>768)	Wheel_angle=768;	//防止位置误差过大导致的不稳定
if(fabs(angle)<0.78)
//控制在车体角度小于45°才有PWM输出
{
PWM_cal = kf_1*Wheel_angle*0.008*21.25
+	//车轮角度
-|
kf_2*angle*21.25
+
//车体角度(加速度测量值处理后)
-|
kf_3*Wheel_angle_dot*0.818*21.25	+	//车轮角速度
-|
kf_4*angle_dot*21.25;
//车体角速度(陀螺仪测量值处理后)
-|-弧度制单位
}
else
{
PWM_cal = 0;
LR_Angle= 0;
}
/**********************************************************************************************************/
//****
上式中
Wheel_angle * 0.008;
//计算车轮角度(0.008 = 2π/(64*12))
****//
//****
Wheel_angle_dot*0.818
//计算车轮角速度(0.818 = 100*2*π/(64*12))
****//
//****
分别乘以21.25	原因如下：
****//
//****
PWM_cal*= 255/12;
//PWM输出为255时 对应 的输出电压值12
****//
//****
//PWM输出为X
时 对应 的输出值为PWM_cal，求X
****//
/**********************************************************************************************************/
LW_PWM	=
PWM_cal - LR_Angle;
//根据接收到无线模块的数据改变车的转弯
RW_PWM	= -PWM_cal - LR_Angle;
//由于电机安装是一正一反，故PWM输出相反
}

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
ISR(TIMER0_COMP_vect)//10ms
{
//TIMSK &=~0x02;//输出比较匹配使能
sei();
gyro=ADport(0);
acc=ADport(1);
//角度校正
//n0=648000/period0;//rp60s
//n1=648000/period1;
//n0=1080/period0;//rp100ms
//n1=1080/period1;
if(f_refresh0 || f_refresh1)
{
f_refresh0=FALSE;
f_refresh1=FALSE;
speed=(1080.0/period0+1080.0/period1)*0.5;//最大15
if(dir0==POS)
{
speed=speed;
//PORTD &=0x7f;
}
else
{
speed=-speed;
//PORTD |=0x80;
}
}
else
speed=0;
speed_filter*=0.8282;
//车轮速度滤波0.7304
speed_filter+=speed*0.1718;//0.2696
position+=speed_filter;
if(!f_ok)
{
if(f_forward)
position+=0.4;
else if(f_backward)
position-=0.6;
}
else
{
f_ok=FALSE;
f_forward=FALSE;
f_backward=FALSE;
}
if(position>300)
position=300;
else if(position<-300)
position=-300;
/*
if(++ctr_pos==2)
{
ctr_pos=0;
temp1=speed_filter*10+150;
Putc(0xfd);
Putc(temp1/256);
Putc(temp1%256);
}
*/
acc=acc-1650;
acc=acc/800;//*100 求出多少g
if(acc>1)
acc=1;
else if(acc<-1)
acc=-1;
acc=180/3.1415*asin(acc);
w=(gyro-1178)/0.67;//1169
Kalman_Filter(acc,w);
if(angle>30 || angle<-30)
{
PORTB |=1<<4;
PORTB |=1<<5;
PORTB |=1<<6;
PORTB |=1<<7;
OCR1A=0;
OCR1B=0;
return;
}
ut=Kp_angle*angle + Kd_angle*angle_dot - Kp_speed*speed_filter - Ki_speed*position;
left_sp=ut;
right_sp=ut;
if(f_right)
{
PORTA &= ~(1<<4);
left_sp+=30;
right_sp-=35;
}
else if(f_left)
{
PORTA &= ~(1<<4);
right_sp+=35;
left_sp-=30;
}
if(left_sp>PER)
left_sp=PER;
else if(left_sp<-PER)
left_sp=-PER;
if(right_sp>PER)
right_sp=PER;
else if(right_sp<-PER)
right_sp=-PER;
if(left_sp>=0)
{
PORTB &=~(1<<4);//正转,窗户
PORTB |=1<<5;
}
else
{
left_sp=-left_sp;
PORTB &=~(1<<5);//反转，人
PORTB |=1<<4;
}
if(right_sp>=0)
{
PORTB &=~(1<<6);
PORTB |=1<<7;
}
else
{
right_sp=-right_sp;
PORTB &=~(1<<7);
PORTB |=1<<6;
}
OCR1B=FRICTION+left_sp;
OCR1A=FRICTION+right_sp;
}

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
//-------------------------------------------------------
//计算PWM输出值
//车辆直径：76mm; 12*64pulse/rev; 1m=3216pulses;
//-------------------------------------------------------
//static int speed_diff,speed_diff_all,speed_diff_adjust;
//static float K_speed_P,K_speed_I;
static float K_voltage,K_angle,K_angle_dot,K_position,K_position_dot;
static float K_angle_AD,K_angle_dot_AD,K_position_AD,K_position_dot_AD;
static float position,position_dot;
static float position_dot_filter;
static float PWM;
static int speed_output_LH,speed_output_RH;
static int Turn_Need,Speed_Need;
//-------------------------------------------------------
void PWM_calculate(void)
{
if ( 0==(~PINA&BIT(1)) )	//左转
{
Turn_Need=-40;
}
else if ( 0==(~PINB&BIT(2)) )
//右转
{
Turn_Need=40;
}
else	//不转
{
Turn_Need=0;
}
if ( 0==(~PINC&BIT(0)) )	//前进
{
Speed_Need=-2;
}
else if ( 0==(~PINC&BIT(1)) )	//后退
{
Speed_Need=2;
}
else	//不动
{
Speed_Need=0;
}
K_angle_AD=ADport(4)*0.007;
K_angle_dot_AD=ADport(5)*0.007;
K_position_AD=ADport(6)*0.007;
K_position_dot_AD=ADport(7)*0.007;
position_dot=(speed_real_LH+speed_real_RH)*0.5;
position_dot_filter*=0.85;
//车轮速度滤波
position_dot_filter+=position_dot*0.15;
position+=position_dot_filter;
//position+=position_dot;
position+=Speed_Need;
if (position<-768)
//防止位置误差过大导致的不稳定
{
position=-768;
}
else if
(position>768)
{
position=768;
}
PWM = K_angle*angle *K_angle_AD + K_angle_dot*angle_dot *K_angle_dot_AD +
K_position*position *K_position_AD + K_position_dot*position_dot_filter *K_position_dot_AD;
speed_output_RH = PWM - Turn_Need;
speed_output_LH = - PWM - Turn_Need ;
/*
speed_diff=speed_real_RH-speed_real_LH;
//左右轮速差PI控制；
speed_diff_all+=speed_diff;
speed_diff_adjust=(K_speed_P*speed_diff+K_speed_I*speed_diff_all)/2;
*/
PWM_output (speed_output_LH,speed_output_RH);
}

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
/*
-----------------------------------------------------------------------------
Function
: Set_PWM()
limiting and Setting the PWM Signals, and Set the Direction Pins
Date
: 01.08.2006
-----------------------------------------------------------------------------
*/
void Set_PWM()
{
if(Drive_A >
max_PWM)Drive_A =
max_PWM;
// limiting PWM Signal A
if(Drive_A < -max_PWM)Drive_A = -max_PWM;
// limiting PWM Signal A
if(Drive_A < 0)
// Check direction
{
DriveA = Drive_A * -1;
// only positive
CW_CCW_A = 1;
// CW
}
else
{
DriveA = Drive_A;
CW_CCW_A = 0;
// CCW
}
PWM_A = 255 - DriveA;
// inverse Signal to have a Phase shift from 180?to Signal PWM B
if(Drive_B >
max_PWM)Drive_B =
max_PWM;
// limiting PWM Signal B
if(Drive_B < -max_PWM)Drive_B = -max_PWM;
// limiting PWM Signal B
if(Drive_B < 0)
// Check direction
{
DriveB = Drive_B * -1;
// only positive
CW_CCW_B = 1;
// CW
}
else
{
DriveB = Drive_B;
CW_CCW_B = 0;
// CCW
}
PWM_B = DriveB;
// Set PWM
}
/*
-----------------------------------------------------------------------------
Function
: Algorythmus()
Limit top speed by tilting back, Calculate the Driving speed
Date
: 01.08.2006
-----------------------------------------------------------------------------
*/
void Algorythmus()
{
Balance_moment =
Angle_Rate*4 +
5 * (Tilt_ANGLE + Anglecorrection);
// calculate balance moment
Overspeed = lmax(0, Drive_sum - Overspeedlimit);
// get over speed
if (Overspeed > 0)
{
if(MODE == Run)Overspeed_flag = 1;
Overspeed_integral = lmin(1000, Overspeed_integral + min(500, Overspeed + 125));
// calculate over speed integral
}
else
{
Overspeed_flag = 0;
Overspeed_integral = lmax(0, Overspeed_integral - 100);
// calculate over speed integral
}
Anglecorrection = Overspeed / 200 + Overspeed_integral / 125;
// calculate Angle correction
Drive_sum += Balance_moment;
// calculate Drive_sum
// limitting
if(Drive_sum >
Drivesumlimit)Drive_sum = Drivesumlimit;
if(Drive_sum <- Drivesumlimit)Drive_sum =-Drivesumlimit;
Drivespeed = Drive_sum / 250 + Balance_moment / 40;
// calculate Drive speed
}