my_act.c
5 KB
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
177
178
179
180
181
182
183
#include "hardware.h"
#include "math.h"
#include "sys.h"
#include "all_value.h"
//#include "my_act.h"
///*******************电机停止*******************/
//void speed_set_mms(unsigned short id,signed long Velocity,signed long a,signed long d)
//{
// signed long m=0;
// signed long n=0;
// signed long x=0;
// signed long y=0;
// m=id;
// n=Velocity*60*(double)dataWheelReductionRatio/(PI*(double)dataWheelDiameter);
// x=a*(double)dataWheelReductionRatio/(PI*(double)dataWheelDiameter);
// y=d*(double)dataWheelReductionRatio/(PI*(double)dataWheelDiameter);
// speed_set_rpm(m,(signed long)n,(signed long)x,(signed long)y);
//}
//void woodman(u8 a,u8 b,u8 c) //a禁双轮 、b禁升降
//{
// if(a==1){speed_set_mms(1,0,1500,1500);speed_set_mms(2,0,1500,1500);}
// if(b==1) speed_set_rpm(3,0,25,50);
// if(c==1) speed_set_rpm(4,0,25,50);
//}
///*****************手持器控制*******************/
//void hand_control(u8 button)
//{
// switch(button)
// {
// case 1: Motor[1].PcToM; MC_TargetSpeed_set(2,500); break;//前进
// case 2: MC_TargetSpeed_set(1,-1000);MC_TargetSpeed_set(2,-500);break;//后退
//// case 3: speed_set_rpm(1,-100,4,10); speed_set_rpm(2,100,4,10); break;//左转
//// case 4: speed_set_rpm(1,100,4,10); speed_set_rpm(2,-100,4,10); break;//右转
//// case 5: if(LIMIT_2==0){speed_set_rpm(3,-200,25,200);}else speed_set_rpm(3,0,25,200);break; //上升
//// case 6: if(LIMIT_1==0){speed_set_rpm(3,200,25,200);}else speed_set_rpm(3,0,25,200);break;//下降
//// case 7: speed_set_rpm(4,1000,25,25);delay_ms(1);break; //正转
//// case 8: speed_set_rpm(4,-1000,25,25);delay_ms(1);break;//反转
//// case 10:speed_set_rpm(1,-100,8,10);
//// speed_set_rpm(2,100,8,10);
//// speed_set_rpm(4,-100*dataCouplingCoefficient,44,56);break;
//// case 11:speed_set_rpm(1,100,8,10);
//// speed_set_rpm(2,-100,8,10);
//// speed_set_rpm(4,100*dataCouplingCoefficient,44,56);break;
// default:MC_TargetSpeed_set(1,0);
// MC_TargetSpeed_set(2,0);
//// speed_set_rpm(3,0,25,25);
//// speed_set_rpm(4,0,25,25);
// break;
// }
//}
///*******************导航前进*******************/
//void guid_directiongo(double a,double b,double d,u32 acc,u32 dec)//a 导航角度、 b中心速度、c 比例、 d 目标方向
//{
// static double pid_dir=0;
// woodman(0,1,1);
// if(radarstop==0)pid_dir=PID_realize(d,a);//检测到障碍物pid不需要调节
// speed_set_mms(1,(signed long)(1000*b-1.1*b*pid_dir),acc,dec);
// speed_set_mms(2,(signed long)(1000*b+1.1*b*pid_dir),acc,dec);
//}
///*******************导航后退*******************/
//void guid_directionback(double a,double b,double d,u32 acc,u32 dec)//a 导航角度 b中心速度 c 比例 d 目标方向
//{
// woodman(0,1,1);
// speed_set_mms(1,(signed long)(-(1000*b)),acc,dec);
// speed_set_mms(2,(signed long)(-(1000*b)),acc,dec);
//}
///****************旋转到目标方向****************/
//void AGV_rot_target(u8 dir,double rot_s,double r_speed,u8 ouhe_en)//dir旋转方向 rot_speed旋转速度
//{
// woodman(0,1,0);
// /*耦合的条件
// *托盘不在低位，并且agv当前点不是整体旋转点
// *或者当前点是站台点，，站台点永远是耦合的
// */
// /*原地复位也是高位耦合*/
// if(dir==2)//顺时针旋转
// {
// speed_set_rpm(1,r_speed,8,20);
// speed_set_rpm(2,-r_speed,8,20);
// if(ouhe_en)speed_set_rpm(4,r_speed*rot_s,8*rot_s,20*rot_s);
// }
// else
// if(dir==1)//逆时针旋转
// {
// speed_set_rpm(1,-r_speed,8,20);
// speed_set_rpm(2,r_speed,8,20);
// if(ouhe_en)speed_set_rpm(4,-r_speed*rot_s,8*rot_s,20*rot_s);
// }
// else
// {
// speed_set_rpm(1,0,10,25);
// speed_set_rpm(2,0,10,25);
// speed_set_rpm(4,0,rot_s*10,rot_s*25);
// }
//}
///*******************y轴调整********************/
//void adjust_y(double s)
//{
// woodman(0,1,1);
// speed_set_mms(1,s,500,2500);
// speed_set_mms(2,s,500,2500);
//}
///********转盘调整****************/
//void adjust_tray(double a,double b)//a 转盘角度 b转盘精度 返回1 调整完成
//{
// woodman(1,1,0);
// if(a>=45&&a<(90-b)) speed_set_rpm(4,50,25,50);else
// if(a<45&&a>b) speed_set_rpm(4,-50,25,50);else
// if(a>=0&&a<=b) speed_set_rpm(4,0,25,100);else
// if(a>=(90-b)&&a<=90) speed_set_rpm(4,0,25,100);
//}
///*********************上升************************/
//void uptray(double a)
//{
// woodman(1,0,1);
// if(LIMIT_2==1)
// {
// up_flag=0;
// delay_ms(250);
// speed_set_rpm(3,0,25,100);
// }else
// {
// up_flag=1;
// if(Distanceupdown<=50)
// {
// speed_set_rpm(3,-a,25,200);
// }else
// {
// speed_set_rpm(3,-500,25,200);
// }
// }
//}
///********************‘*下降************************/
//void downtray(double a)
//{
// woodman(1,0,1);
// if(LIMIT_1==1)
// {
// down_flag=0;
//// delay_ms(50);
// speed_set_rpm(3,0,25,100);
// }else
// {
// down_flag=1;
// if(Distanceupdown<=50)
// {
// speed_set_rpm(3,a,25,200);
// }else
// {
// speed_set_rpm(3,500,25,200);
// }
// }
//}
///************************旋转托盘***************************/
//void rottray(u8 a,double b)
//{
// woodman(1,1,0);
// if(a==1)
// {
// speed_set_rpm(4,-b,100,100);
// }else
// if(a==2)
// {
// speed_set_rpm(4,b,100,100);
// }else
// {
// speed_set_rpm(4,0,100,100);
// }
//}