1、程序源码
1）反解模块
double Fanjie_cita_1(double X,double Y){
double cita_1 = 0.0;
double JuLi = sqrt(X*X + Y*Y);
cita_1 = (1/3.14) * 180 * ( atan(Y/X) - acos((JuLi*JuLi + l1*l1 l2*l2)/(2*l1*JuLi)) );
return cita_1;
}
double Fanjie_cita_2(double X,double Y){
double cita_2 = 0.0;
double JuLi = sqrt(X*X + Y*Y);
cita_2 = 180 - (1/3.14) * 180 * ( acos((l1*l1 + l2*l2 - JuLi*JuLi)/(2*l1*l2)) );
return cita_2;
-
}
2）按钮函数
开始按钮：
void CRobot_handDlg::OnButton6()
{
UpdateData(true);
Cita_f = Fanjie_cita_1(X2,Y2);
direc = 1;
Launch_Sifu(1); //轴一动
Cita_f = Fanjie_cita_2(X2,Y2);
direc = 0;
Launch_Sifu(2); //轴二动
Cita_f = Z2;
Launch_Bujin(1); //轴三动
stop(); //停
}
停止：
void CRobot_handDlg::OnButton5()
{
stop();
}
3) 电机运行模块
void Launch_Sifu(int ZhouNum){
int cita_Maichong = 0;
double cita_Dart = (Cita_f - Cita_0) / N;
cita_Dart = cita_Dart * 800000 /360;
cita_Maichong = (int)cita_Dart; //转换为脉冲
for(int j = 0;j < N;j++){
Sevon_Output(ZhouNum, direc, freq, cita_Maichong);
Sleep(2000);
}
}
void Launch_Bujin(int ZhouNum ){
int cita_Maichong = 0;
double cita_Dart = (Cita_f - Cita_0) / N;
cita_Dart = cita_Dart * 800000 /360;
cita_Maichong = (int)cita_Dart; //转换为脉冲
for(int j = 0;j < N;j++){
KLD_Position(ZhouNum, direc, freq, cita_Maichong);
Sleep(1000);
}
}
void stop(){
KLD_Motor_Off(1);
KLD_Motor_Off(2);
KLD_Motor_Off(3);
KLD_Motor_Off(4);
KLD_Motor_Off(5);
}
2、程序运行界面
