📄 mpu_func.h
字号:
mpu_cp_data[0] = Sample();
data_min = mpu_cp_data[0].d[0];
data_max = mpu_cp_data[0].d[0];
for (j=0; j<3; j++) {
if (mpu_cp_data[0].d[j]<data_min) data_min = mpu_cp_data[0].d[j];
if (mpu_cp_data[0].d[j]>data_max) data_max = mpu_cp_data[0].d[j];
}
if (data_min>1200 && data_max>2200) {
i = 50;
}
else {
RS_SEND(Func_E);
}
}
for (i=0; i<3; i++) {
RS_SEND(Func_A);
delay(500);
mpu_cp_data[1] = Sample();
if (abs(mpu_cp_data[0].d[0]-mpu_cp_data[1].d[0])<150 &&
abs(mpu_cp_data[0].d[1]-mpu_cp_data[1].d[1])<150 &&
abs(mpu_cp_data[0].d[2]-mpu_cp_data[1].d[2])<150 &&
mpu_cp_data[0].d[0]<12000 &&
mpu_cp_data[0].d[1]<12000 &&
mpu_cp_data[0].d[2]<12000 ) {
state = TRUE;
}
else {
state = FALSE;
i = 2;
}
}
return state;
}
void Turn_an_alignment(void)
{
// struct turn_data turn_angle;
struct three_data mpu_turn_data;
float ratio, min_ratio, max_ratio;
int i, j, k;
mpu_turn_data = Sample();
turn_angle_input_time++;
if (turn_angle_input_time>3) turn_angle_input_time = 4;
if (RS_SEND(Func_A)==FALSE) Error_treatment(Angle_ditect);
ratio = mpu_turn_data.d[1]*1./(mpu_turn_data.d[2]*1.);
// turn_angle = origin_turn;
switch (turn_angle_input_time) {
case 1: //
turn_angle.ratio[0] = ratio;
turn_angle.angle[0] = 0.;
turn_angle.angle[2] = 179.8;
turn_angle.flag = FALSE;
break;
case 2: //
turn_angle.ratio[1] = ratio;
turn_angle.angle[1] = 179.8;
turn_angle.angle[2] = 90.;
turn_angle.flag = FALSE;
break;
case 3: //
if (ratio<1.) {
turn_angle.angle[2] = -90.;
turn_angle.angle[1] = -179.8;
turn_angle.flag = FALSE;
}
else {
if (turn_angle.ratio[0]<1.1 && turn_angle.ratio[0]>0.9 &&
turn_angle.ratio[1]<1.1 && turn_angle.ratio[1]>0.9 &&
turn_angle.ratio[2]<1.1 && turn_angle.ratio[2]>0.9) {
turn_angle.flag = TRUE;
} // theta is zero
else {
if (turn_angle.ratio[0]<turn_angle.ratio[1]) {
turn_angle.ratio[0] = ratio;
turn_angle.angle[0] = turn_angle.angle[2];
}
else {
turn_angle.ratio[1] = ratio;
turn_angle.angle[1] = turn_angle.angle[2];
}
turn_angle.angle[2] = (turn_angle.angle[0]+turn_angle.angle[1])/2.;
turn_angle.flag = FALSE;
}}
break;
case 4: //
if (turn_angle.ratio[0]<1.1 && turn_angle.ratio[0]>0.9 &&
turn_angle.ratio[1]<1.1 && turn_angle.ratio[1]>0.9 &&
turn_angle.ratio[2]<1.1 && turn_angle.ratio[2]>0.9) {
turn_angle.flag = TRUE;
} // theta is zero
else {
if (turn_angle.ratio[0]<turn_angle.ratio[1]) {
turn_angle.ratio[0] = ratio;
turn_angle.angle[0] = turn_angle.angle[2];
}
else {
turn_angle.ratio[1] = ratio;
turn_angle.angle[1] = turn_angle.angle[2];
}
if (fabs(turn_angle.angle[1]-turn_angle.angle[0])<5.) {
turn_angle.flag = TRUE;
}
else {
turn_angle.angle[2] = (turn_angle.angle[0]+turn_angle.angle[1])/2.;
turn_angle.flag = FALSE;
}}
break;
}
if (turn_angle.flag==FALSE) {
Pscara.coordinate[2] = Pscara.coordinate[2] - 15.;
Pscrewi.coordinate[2] = Pscrewi.coordinate[2] - 15.;
Scara_move(Pscara); // alpha alignment move
Pscrewi.alpha = turn_angle.angle[2]*pi/180.;
Pscrewi.coordinate[3] = turn_angle.angle[2];
Pscara = Scara_point_cal(Pscrewi);
// printf("alpha alignment move 1 ");
Scara_move(Pscara); // alpha alignment move
Pscara.coordinate[2] = Pscara.coordinate[2] + 15.;
Pscrewi.coordinate[2] = Pscrewi.coordinate[2] + 15.;
Scara_move(Pscara); // alpha alignment move
}
return;
}
int slope_data_input(void)
{
int i;
int finish_state;
for (i=0; i<3; i++) {
mpu_slope_data[i] = Sample();
RS_SEND(Func_A);
delta_y = delta_y + 0.5;
Pscrewc = Screw_center_cal(Pscrewi, -58.2+delta_x, delta_y, 0.);
Pscara = Scara_point_cal(Pscrewc);
Scara_move(Pscara);
outtextxy(200, 200,"Continue for another position");
}
Step_5_erase_print();
finish_state = TRUE;
return finish_state;
}
int search_screw_center(void)
{
int sc_sum_data[3], i, j;
int finish_state;
int x_cross_check = FALSE;
float move_step, move_factor;
float x_cross1, x_cross2, y_cross;
struct three_data former_sc_data;
move_step = 1.; delta_x = 0.;
move_factor = 1.; delta_y = 0.;
for (;;) {
delta_x = delta_x - 1.;
Pscrewc = Screw_center_cal(Pscrewi,-58.2+delta_x,delta_y,ang_dis.angle);
Pscara = Scara_point_cal(Pscrewc);
Scara_move(Pscara); // move to aligned position
if (x_cross_check==FALSE) {
screw_center_data[0] = screw_center_data[1];
}
screw_center_data[1] = Sample();
if (RS_SEND(Func_A)==FALSE) Error_treatment(Position_ditect);
if (screw_center_data[1].d[2]<screw_center_data[0].d[2]-250 &&
x_cross_check==FALSE) {
x_cross1 = delta_x;
if (ang_dis.angle>4.5) x_cross1 = x_cross1+1.5;
delta_x = delta_x - 3.0;
x_cross_check = TRUE;
Pscrewc = Screw_center_cal(Pscrewi,-58.2+delta_x,delta_y,ang_dis.angle);
Pscara = Scara_point_cal(Pscrewc);
Scara_move(Pscara); // move to aligned position
screw_center_data[0] = Sample();
if (RS_SEND(Func_A)==FALSE) Error_treatment(Position_ditect);
}
else if (screw_center_data[1].d[2]>screw_center_data[0].d[2]+250 &&
x_cross_check==TRUE) {
x_cross2 = delta_x;
x_cross_point = (x_cross1+x_cross2)/2.;
break;
}}
move_step = 1.;
delta_x = (x_cross1+x_cross2)/2. + 7.5;
move_factor = 1.; delta_y = 0.;
Pscrewc = Screw_center_cal(Pscrewi,-58.2+delta_x,delta_y,ang_dis.angle);
Pscara = Scara_point_cal(Pscrewc);
Scara_move(Pscara); // move to aligned position
screw_center_data[1] = Sample();
if (RS_SEND(Func_A)==FALSE) Error_treatment(Position_ditect);
for (;;) {
delta_y = delta_y - 1.;
Pscrewc = Screw_center_cal(Pscrewi,-58.2+delta_x,delta_y,ang_dis.angle);
Pscara = Scara_point_cal(Pscrewc);
Scara_move(Pscara); // move to aligned position
screw_center_data[0] = screw_center_data[1];
screw_center_data[1] = Sample();
if (RS_SEND(Func_A)==FALSE) Error_treatment(Position_ditect);
if (screw_center_data[1].d[0]<screw_center_data[0].d[0]-250) {
y_cross = delta_y;
break;
}}
if (RS_SEND(Func_B)==FALSE) Error_treatment(Position_ditect);
delta_x = (x_cross1+x_cross2)/2. + 7.5;
delta_y = y_cross + 3.2;
Pscrewc = Screw_center_cal(Pscrewi,-58.2+delta_x,delta_y,ang_dis.angle);
Pscara = Scara_point_cal(Pscrewc);
Scara_move(Pscara); // move to aligned position
screw_center_data[1] = Sample();
if (RS_SEND(Func_B)==FALSE) Error_treatment(Position_ditect);
if (screw_center_data[1].d[0]>2400 &&
screw_center_data[1].d[1]>2400 &&
screw_center_data[1].d[2]>2400) {
finish_state = TRUE;
}
else {
for (i=0; i<8; i++) {
delta_x = delta_x + 0.2;
Pscrewc = Screw_center_cal(Pscrewi,-58.2+delta_x,delta_y,ang_dis.angle);
Pscara = Scara_point_cal(Pscrewc);
Scara_move(Pscara); // move to aligned position
screw_center_data[0] = screw_center_data[1];
screw_center_data[1] = Sample();
if (RS_SEND(Func_B)==FALSE) Error_treatment(Position_ditect);
if ((screw_center_data[1].d[1]>2000 &&
screw_center_data[1].d[2]>2000) ||
screw_center_data[1].d[1]>2500 ||
screw_center_data[1].d[2]>2500 || i==7) {
for (j=0; j<4; j++) {
delta_y = delta_y + 0.2;
Pscrewc = Screw_center_cal(Pscrewi,-58.2+delta_x,delta_y,ang_dis.angle);
Pscara = Scara_point_cal(Pscrewc);
Scara_move(Pscara); // move to aligned position
screw_center_data[0] = screw_center_data[1];
screw_center_data[1] = Sample();
if (RS_SEND(Func_B)==FALSE) Error_treatment(Position_ditect);
if (screw_center_data[1].d[0]>1500) {
finish_state = TRUE;
break;
}}
i = 8;
}}}
return finish_state;
}
struct three_byte Cal_send_byte(float data)
{
struct three_byte bt;
int symbol;
bt.data[0] = 0;
if (data<0.) {
symbol = -1;
data = -data;
}
else symbol = 1;
if (fabs(data)<0.001) {
bt.data[0] = 64;
data = 0.5;
symbol = 1;
}
if (fabs(data-1.)<0.01) data = data + 0.02;
else if (data>1.) {
for (;;) {
data = data/2.;
bt.data[0]++;
if (data<1.) break;
}}
else if(data<0.5) {
for (;;) {
data = data*2.;
bt.data[0]--;
if (data>0.5) break;
}}
if (bt.data[0]<0) bt.data[0] = bt.data[0] + 128;
if (symbol==-1) bt.data[0] = bt.data[0] + 128;
if (bt.data[0]==0) {
bt.data[0] = 1;
data = data/2.;
}
bt.data[1] = data*256.;
bt.data[2] = (data*256.-bt.data[1])*256.;
if (bt.data[1]==0) bt.data[1] = 1;
if (bt.data[2]<1) bt.data[2] = 1;
return bt;
}
void Scara_move(struct cal_point scara_pt)
{
struct three_byte bt;
float rou;
int i;
print_coordinate(scara_pt);
if (scara_pt.coordinate[2]>max_z_coordinate) {
printf("z = %d Z direction move too deep!", scara_pt.coordinate[2]);
exit(1);
}
rou = sqrt(pow(scara_pt.coordinate[0]-x_0, 2.) +
pow(scara_pt.coordinate[1]-y_0, 2.));
if (rou>150.) {
printf("x, y out of range!");
exit(1);
}
// getch();
for (i=0; i<4; i++) {
bt = Cal_send_byte(scara_pt.coordinate[i]);
RS_SCARA_SEND(bt.data[0]);
delay(50);
RS_SCARA_SEND(bt.data[1]);
delay(50);
RS_SCARA_SEND(bt.data[2]);
delay(50);
// printf("%d %d %d \n",bt.data[0], bt.data[1], bt.data[2]);
}
RS_SCARA_RECEIVE();
return;
}
/* Known: scara end point x, y */
/* Calculate: scara's theta1, theta2 */
struct cal_point Scara_theta_cal(struct cal_point input_point)
{
struct cal_point out_point;
float A, B, C, PHY;
float L1, L2;
float x, y, theta1, theta2;
// float pi = 3.1415926;
L1 = 349.81;
L2 = 249.42;
x = input_point.coordinate[0];
y = input_point.coordinate[1];
A = (x*x+y*y+L1*L1-L2*L2)/2./L1;
B = A/sqrt(x*x+y*y);
if (fabs(y)<0.01) PHY = pi/2.;
else PHY = atan(x/y);
theta1 = asin(B) - PHY;
C = (x-L1*cos(theta1))/L2;
theta2 = acos(C) - theta1;
out_point = input_point;
out_point.theta[0] = theta1;
out_point.theta[1] = theta2;
return out_point;
}
/* Known: scara end point x, y, theta1, theta2, R */
/* and screw's small movement delta_x, delta_y */
/* Calculate: screw's center Ps_x, Ps_y */
struct cal_point Screw_center_cal(struct cal_point input_point,
float delta_x, float delta_y, float theta)
{
struct cal_point out_point;
float L = 58.2;
float alpha0 = 0.;
float beta;
float x, y, z;
beta = input_point.coordinate[3]*pi/180. + alpha0;
x = input_point.coordinate[0] + (L+delta_x)*cos(beta) - delta_y*sin(beta);
y = input_point.coordinate[1] + (L+delta_x)*sin(beta) + delta_y*cos(beta);
z = input_point.coordinate[2] + (L+delta_x)*sin(theta*pi/180.);
// movement along slop cause hight change
out_point = input_point;
out_point.coordinate[0] = x;
out_point.coordinate[1] = y;
out_point.coordinate[2] = z;
return out_point;
}
/* Known: screw's center, alpha */
/* Calculate: scara cal_point */
struct cal_point Scara_point_cal(struct cal_point input_point)
{
struct cal_point out_point;
float L = 58.2;
float alpha0 = 0.;
float x, y;
x = input_point.coordinate[0] - L*cos(input_point.alpha+alpha0);
y = input_point.coordinate[1] - L*sin(input_point.alpha+alpha0);
out_point = input_point;
out_point.coordinate[0] = x;
out_point.coordinate[1] = y;
out_point = Scara_theta_cal(out_point);
out_point.coordinate[3] = input_point.alpha*180./pi;
return out_point;
}
/* Known: scara end point x, y, and theta, alpha */
/* Calculate: scara aligned position's x, y */
struct cal_point Scara_aligned_point_cal(struct cal_point input_point,
float delta_z, float theta)
{
struct cal_point out_point;
float H = 76.0;
// float pi = 3.1415926;
float alpha0 = 0.;
float x, y, z;
float beta;
beta = input_point.coordinate[3]*pi/180. + alpha0;
x = input_point.coordinate[0] + (H-delta_z)*cos(beta)*sin(theta*pi/180.);
y = input_point.coordinate[1] + (H-delta_z)*sin(beta)*sin(theta*pi/180.);
z = (H-delta_z)*(1-cos(theta*pi/180.));
out_point = input_point;
out_point.coordinate[0] = x;
out_point.coordinate[1] = y;
out_point.coordinate[2] = out_point.coordinate[2] + z;
return out_point;
}
⌨️ 快捷键说明
复制代码
Ctrl + C
搜索代码
Ctrl + F
全屏模式
F11
切换主题
Ctrl + Shift + D
显示快捷键
?
增大字号
Ctrl + =
减小字号
Ctrl + -