19,468
社区成员
发帖
与我相关
我的任务
分享机械臂抓物不准
从老师那里得到的代码 实际拿机械臂测试的时候有一定的误差 然后尝试读的时候有困难 麻烦帮忙看下
// MFCApplication2Dlg.cpp : 实现文件
//
#include "stdafx.h"
#include "MFCApplication2.h"
#include "MFCApplication2Dlg.h"
#include "afxdialogex.h"
#include <opencv2\opencv.hpp>
#include<opencv2/highgui/highgui.hpp>
#include<opencv2/imgproc/imgproc.hpp>
#include <iostream>
#include <stdio.h>
#include <stdlib.h>
using namespace cv;
using namespace std;
RNG rng(12345);
void thresh_callback(Mat src_gray, Mat drawing );
/** @thresh_callback 函数 */
void thresh_callback(Mat src_gray, Mat drawing )
{
namedWindow( "imgThresholded", CV_WINDOW_AUTOSIZE );
imshow( "imgThresholded", src_gray );
Mat threshold_output;
vector<vector<Point> > contours;
vector<Vec4i> hierarchy;
/// 使用Threshold检测边缘
threshold( src_gray, threshold_output,100, 255, THRESH_BINARY );
/// 找到轮廓
findContours( threshold_output, contours, hierarchy, CV_RETR_TREE, CV_CHAIN_APPROX_SIMPLE, Point(0, 0) );
vector<vector<Point> > contours_poly( contours.size() );
vector<Rect> boundRect( contours.size() );
vector<Point2f>center( contours.size() );
vector<float>radius( contours.size() );
for( int i = 0; i < contours.size(); i++ )
{
approxPolyDP( Mat(contours[i]), contours_poly[i], 3, true );
boundRect[i] = boundingRect( Mat(contours_poly[i]) );//矩形逼近
minEnclosingCircle( contours_poly[i], center[i], radius[i] );//圆逼近
}
int max=0,seq=0;
for( int i = 0; i< contours.size(); i++ )
{
Scalar color = Scalar( rng.uniform(0, 255), rng.uniform(0,255), rng.uniform(0,255) );
if(max<(int)radius[i])
{
max=(int)radius[i];
seq=i;
}
}
//找出轮廓最大的圆也就是轮廓最大的矩形
rectangle( drawing, boundRect[seq].tl(), boundRect[seq].br(), CV_RGB(255, 0,0), 2, 8, 0 );//画矩形
// circle( drawing, center[seq], (int)radius[seq], CV_RGB(255, 0,0), 2, 8, 0 );//画圆
int x,y;
printf("x=%.1f,y=%.1f\n",center[seq].x,center[seq].y);
// cout<<"x="<<center[seq].x<<"y="<<center[seq].y<<endl;//输出坐标
/// 显示在一个窗口
// namedWindow( "Contours", CV_WINDOW_AUTOSIZE );
imshow( "Contours", drawing );
// waitKey(30);
}
#ifdef _DEBUG
#define new DEBUG_NEW
#endif
// 用于应用程序“关于”菜单项的 CAboutDlg 对话框
class CAboutDlg : public CDialogEx
{
public:
CAboutDlg();
// 对话框数据
enum { IDD = IDD_ABOUTBOX };
protected:
virtual void DoDataExchange(CDataExchange* pDX); // DDX/DDV 支持
// 实现
protected:
DECLARE_MESSAGE_MAP()
};
CAboutDlg::CAboutDlg() : CDialogEx(CAboutDlg::IDD)
{
}
void CAboutDlg::DoDataExchange(CDataExchange* pDX)
{
CDialogEx::DoDataExchange(pDX);
}
BEGIN_MESSAGE_MAP(CAboutDlg, CDialogEx)
END_MESSAGE_MAP()
// CMFCApplication2Dlg 对话框
CMFCApplication2Dlg::CMFCApplication2Dlg(CWnd* pParent /*=NULL*/)
: CDialogEx(CMFCApplication2Dlg::IDD, pParent)
, m_strRXData(_T(""))
, m_strTXData(_T(""))
, m_Index(0)
, m_strAngle1(_T(""))
, m_strAngle2(_T(""))
, m_strAngle3(_T(""))
, m_strYmid(_T(""))
, m_strXmid(_T(""))
, m_strAngleA(_T(""))
, m_strAngleB(_T(""))
, m_strAngleC(_T(""))
{
m_hIcon = AfxGetApp()->LoadIcon(IDR_MAINFRAME);
}
void CMFCApplication2Dlg::DoDataExchange(CDataExchange* pDX)
{
CDialogEx::DoDataExchange(pDX);
DDX_Text(pDX, IDC_EDIT_RXDATA, m_strRXData);
DDV_MaxChars(pDX, m_strRXData, 1024);
DDX_Text(pDX, IDC_EDIT_TXDATA, m_strTXData);
DDV_MaxChars(pDX, m_strTXData, 1024);
DDX_Control(pDX, IDC_MSCOMM1, m_ctrlComm);
DDX_Control(pDX, IDC_COMBO_CommSeclect, m_ComboBox);
DDX_CBIndex(pDX, IDC_COMBO_CommSeclect, m_Index);
DDX_Text(pDX, IDC_EDIT2, m_strAngle1);
DDX_Text(pDX, IDC_EDIT3, m_strAngle2);
DDX_Text(pDX, IDC_EDIT4, m_strAngle3);
DDX_Text(pDX, IDC_EDIT5, m_strYmid);
DDX_Text(pDX, IDC_EDIT6, m_strXmid);
DDX_Text(pDX, IDC_EDIT7, m_strAngleA);
DDX_Text(pDX, IDC_EDIT8, m_strAngleB);
DDX_Text(pDX, IDC_EDIT9, m_strAngleC);
DDX_Text(pDX, IDC_EDIT7, m_strAngleA);
DDX_Text(pDX, IDC_EDIT8, m_strAngleB);
DDX_Text(pDX, IDC_EDIT9, m_strAngleC);
}
BEGIN_MESSAGE_MAP(CMFCApplication2Dlg, CDialogEx)
ON_WM_SYSCOMMAND()
ON_WM_PAINT()
ON_WM_QUERYDRAGICON()
ON_BN_CLICKED(IDOK, &CMFCApplication2Dlg::OnBnClickedOk)
ON_MESSAGE(WM_COMM_RXCHAR, OnCommunication)
ON_BN_CLICKED(IDC_BUTTON1, &CMFCApplication2Dlg::OnBnClickedButton1)
ON_BN_CLICKED(IDC_BUTTON2, &CMFCApplication2Dlg::OnClickedButton2)
ON_BN_CLICKED(IDC_BUTTON3, &CMFCApplication2Dlg::OnClickedButton3)
ON_BN_CLICKED(IDC_BUTTON4, &CMFCApplication2Dlg::OnClickedButton4)
ON_BN_CLICKED(IDC_BUTTON5, &CMFCApplication2Dlg::OnClickedButton5)
ON_EN_CHANGE(IDC_EDIT3, &CMFCApplication2Dlg::OnEnChangeEdit3)
ON_EN_CHANGE(IDC_EDIT4, &CMFCApplication2Dlg::OnEnChangeEdit4)
ON_EN_CHANGE(IDC_EDIT8, &CMFCApplication2Dlg::OnEnChangeEdit8)
END_MESSAGE_MAP()
// CMFCApplication2Dlg 消息处理程序
BOOL CMFCApplication2Dlg::OnInitDialog()
{
CDialogEx::OnInitDialog();
// 将“关于...”菜单项添加到系统菜单中。
// IDM_ABOUTBOX 必须在系统命令范围内。
ASSERT((IDM_ABOUTBOX & 0xFFF0) == IDM_ABOUTBOX);
ASSERT(IDM_ABOUTBOX < 0xF000);
CMenu* pSysMenu = GetSystemMenu(FALSE);
if (pSysMenu != NULL)
{
BOOL bNameValid;
CString strAboutMenu;
bNameValid = strAboutMenu.LoadString(IDS_ABOUTBOX);
ASSERT(bNameValid);
if (!strAboutMenu.IsEmpty())
{
pSysMenu->AppendMenu(MF_SEPARATOR);
pSysMenu->AppendMenu(MF_STRING, IDM_ABOUTBOX, strAboutMenu);
}
}
// 设置此对话框的图标。当应用程序主窗口不是对话框时，框架将自动
// 执行此操作
SetIcon(m_hIcon, TRUE); // 设置大图标
SetIcon(m_hIcon, FALSE); // 设置小图标
// TODO: 在此添加额外的初始化代码
m_ComboBox.SetCurSel(0);
return TRUE; // 除非将焦点设置到控件，否则返回 TRUE
}
void CMFCApplication2Dlg::OnSysCommand(UINT nID, LPARAM lParam)
{
if ((nID & 0xFFF0) == IDM_ABOUTBOX)
{
CAboutDlg dlgAbout;
dlgAbout.DoModal();
}
else
{
CDialogEx::OnSysCommand(nID, lParam);
}
}
// 如果向对话框添加最小化按钮，则需要下面的代码
// 来绘制该图标。对于使用文档/视图模型的 MFC 应用程序，
// 这将由框架自动完成。
void CMFCApplication2Dlg::OnPaint()
{
if (IsIconic())
{
CPaintDC dc(this); // 用于绘制的设备上下文
SendMessage(WM_ICONERASEBKGND, reinterpret_cast<WPARAM>(dc.GetSafeHdc()), 0);
// 使图标在工作区矩形中居中
int cxIcon = GetSystemMetrics(SM_CXICON);
int cyIcon = GetSystemMetrics(SM_CYICON);
CRect rect;
GetClientRect(&rect);
int x = (rect.Width() - cxIcon + 1) / 2;
int y = (rect.Height() - cyIcon + 1) / 2;
// 绘制图标
dc.DrawIcon(x, y, m_hIcon);
}
else
{
CDialogEx::OnPaint();
}
}
//当用户拖动最小化窗口时系统调用此函数取得光标
//显示。
HCURSOR CMFCApplication2Dlg::OnQueryDragIcon()
{
return static_cast<HCURSOR>(m_hIcon);
}
void CMFCApplication2Dlg::OnBnClickedOk()
{
// TODO: 在此添加控件通知处理程序代码
CDialogEx::OnOK();
}
...全文
请发表友善的回复…
发表回复
may-night 2018-03-24
- 打赏
- 举报
您说的都有想到 但是主要opcv没学过 只能勉强看得懂代码 然后他这个是通过取两个点 算一个比例 最后通过一个挺奇怪的三角函数来算出三个角度
zgl7903 2018-03-24
- 打赏
- 举报
1 识别是否准确
2 发出命令是否准确
3 执行是否准确
may-night 2018-03-24
- 打赏
- 举报
好的老师 我看看
may-night 2018-03-24
- 打赏
- 举报
好的老师 我看看[/quote]
明天去实验室的时候确认一下设备上的螺丝了要
赵4老师 2018-03-24
- 打赏
- 举报
搜“空回误差”？
may-night 2018-03-23
- 打赏
- 举报
void CMFCApplication2Dlg::OnBnClickedButton1()
{
VideoCapture inputVideo(0);
Mat input;
while (1)
{
inputVideo >> input;
Mat src,Src;
Src=input.clone();
Point pt[2];
pt[0].x=320;
pt[0].y=0;
pt[1].x=320;
pt[1].y=480;
line(input,pt[0],pt[1],Scalar(0, 255, 0),1,8,0);
//Tuling
Point HD_p1=Point(200,200);
Point HD_p2=Point(200,241.5);
Point HD_p3=Point(241.5,200);
Point HD_p4=Point(241.5,241.5);
line(input,HD_p1,HD_p2,Scalar(0,255, 0),1,8,0);
line(input,HD_p1,HD_p3,Scalar(0,255, 0),1,8,0);
line(input,HD_p2,HD_p4,Scalar(0,255, 0),1,8,0);
line(input,HD_p3,HD_p4,Scalar(0,255, 0),1,8,0);
Point count,count2;
int OX=0,OY=480-320*tan(30*3.1415926/180);
count.x=OX;
count.y=OY;
count2.x=640;
count2.y=OY;
line(input,count,count2,Scalar(0, 255, 0),1,8,0);
imshow("input",input);
char c=waitKey(10);
if(c=='a'){
int xmax=0,xmin=640,ymax=0,ymin=480;
//三通道彩图转单通道黑白图像
//如果是opencv3可能宏定义不太一样，要么你下个opencv2.4.X，或者查一下把CV_BGR2GRAY换成3版本的宏
cvtColor(Src, src, CV_BGR2GRAY);
//方框滤波模糊处理平滑降噪
blur(src, src, Size(3, 3));
//运行Canny算子轮廓检测，30和60是两个滞后性阈值
//根据实际情况debug进行调整，当场的光线情况和摄像头拍摄图像清晰度都有影响
//建议一个值在另一个值得2到3倍，后面核大小3不要改
Canny(src, src, 35, 70, 3);
//轮廓信息容器,存储各个轮廓信息的所有像素点坐标
vector<vector<Point>> Contours;
vector<Vec4i>hierarchy;
//旋转矩形容器，暂时先用多边形逼近做，这个还没用到
//vector<RotatedRect> Rrects;
//闭运算部分
//内核矩阵尺寸
int g_nStructElementSize = 5;
//创建内核矩阵
Mat element = getStructuringElement(MORPH_RECT, Size(2 * g_nStructElementSize + 1, 2 * g_nStructElementSize + 1),
Point(g_nStructElementSize, g_nStructElementSize));
//闭运算修补，修复残缺轮廓连接处
morphologyEx(src, src, MORPH_CLOSE, element);
//轮廓查找
//findContours()的使用会改变原图像，故使用拷贝进行轮廓查找，使用完释放
Mat src_findContour=src.clone();
findContours(src_findContour, Contours, RETR_EXTERNAL, CHAIN_APPROX_NONE);
src_findContour.release();
//轮廓点数判断筛选，去除过大与过小的轮廓，保留七巧板积木轮廓
//还可以根据轮廓面积作为判断条件筛选，暂时没写要写我就补上去，不过应该用不到，筛选条件已经够苛刻了
//cmin,cmax具体值自己根据实际条件调整
unsigned int cmin = 80; // minimum contour length
unsigned int cmax = 800; // maximum contour length
//迭代器循环,erase掉干扰轮廓
for (vector<vector<Point>>::const_iterator ita = Contours.begin(); ita != Contours.end();)
{
if (((ita->size()) < cmin) || ((ita->size()) > cmax))
{
ita = Contours.erase(ita);
}
else
{
++ita;
}
}
//采用多边形逼近，这样来排除点数过多的干扰轮廓
for (vector<vector<int>>::size_type i = 0; i < Contours.size(); ++i)
{
//遍历每个轮廓，采用多边形逼近曲线，后面的3位逼近的精度，具体参数根据实际情况可能调整
//但是估计要调整可能性不大
vector<Point> approxPoly_contours;
approxPolyDP(Contours[i], approxPoly_contours, 3, true);
//如果多边形逼近曲线点数较多，则是不规则干扰轮廓，只保留三角形和矩形
//判断条件是否是大于7也要根据上面函数逼近的精度参数视具体情况而定
if (approxPoly_contours.size() > 7) continue;
//绘制多边形的点（绿色）
for (vector<Point>::size_type i=0; i < approxPoly_contours.size(); ++i)
{
cout<<"x="<<approxPoly_contours[i].x<<",y="<<approxPoly_contours[i].y<<endl;
if(approxPoly_contours[i].x>=xmax)
xmax=approxPoly_contours[i].x;
if(approxPoly_contours[i].x<=xmin)
xmin=approxPoly_contours[i].x;
if(approxPoly_contours[i].y>=ymax)
ymax=approxPoly_contours[i].y;
if(approxPoly_contours[i].y<=ymin)
ymin=approxPoly_contours[i].y;
circle(Src, approxPoly_contours[i], 2, Scalar(0, 255, 0), 1, 8);
}
//绘制多边形轮廓
for (vector<Point>::size_type i = 0; i < approxPoly_contours.size(); ++i)
{
if (i == approxPoly_contours.size() - 1)
line(Src, approxPoly_contours[i], approxPoly_contours[0], Scalar(0, 0, 255), 1, 8);
else line(Src, approxPoly_contours[i], approxPoly_contours[i + 1], Scalar(0, 0, 255), 1, 8);
}
}
Point mid;
mid.x=(xmax+xmin)/2;
mid.y=(ymax+ymin)/2;
cout<<"xmid="<<(xmax+xmin)/2<<"ymid="<<(ymax+ymin)/2<<endl;
circle(Src, mid, 2, Scalar(0, 255, 0), 1, 8);
CString strXmid="",strYmid="";
strXmid.Format("%d",mid.x);
strYmid.Format("%d",mid.y);
m_strXmid=strXmid;
m_strYmid=strYmid;
UpdateData(FALSE);
//窗口显示效果图
imshow("效果图", Src);
}
}
waitKey(0);
// TODO: 在此添加控件通知处理程序代码
}
void CMFCApplication2Dlg::OnClickedButton2(){
int port=((CComboBox*)GetDlgItem(IDC_COMBO_CommSeclect))->GetCurSel();
if(port1.InitPort(this,port+1,9600,'N',8,1,EV_RXFLAG | EV_RXCHAR,512)){
port1.StartMonitoring();
m_Opened=true;
AfxMessageBox("打开成功");
}else{
AfxMessageBox("没有发现此串口或被占用");
m_Opened=false;
}
}
BEGIN_EVENTSINK_MAP(CMFCApplication2Dlg, CDialogEx)
ON_EVENT(CMFCApplication2Dlg, IDC_MSCOMM1, 1, CMFCApplication2Dlg::OnOncommMscomm1, VTS_NONE)
END_EVENTSINK_MAP()
void CMFCApplication2Dlg::OnOncommMscomm1()
{
// TODO: 在此处添加消息处理程序代码
VARIANT variant_inp;
COleSafeArray safearry_inp;
LONG len,k;
BYTE rxdata[2048];
CString strtemp;
int order;
if(m_ctrlComm.get_CommEvent() == 2)//事件值为2表示接收缓冲区内有数据
{
//以下根据自己的通讯协议添加处理代码
strtemp = m_ctrlComm.get_Input();//读缓冲区
safearry_inp = variant_inp;//VARIANT转化为COleSafeArray
len = safearry_inp.GetOneDimSize();//字符长度
for(k=0;k<len;k++)
{
safearry_inp.GetElement(&k,rxdata+k);//转化为BYTE型数组
}
for(k=0;k<len;k++)//将数组转化成Cstring型变量
{
BYTE bt = *(char*)(rxdata+k);
strtemp.Format(_T("%c"),bt);//将字符送入临时变量strtemp中存放
m_strRXData+=strtemp;//加入接收编辑框对应字符串
order = _ttoi(strtemp);//order是字符转化后的int值
}
UpdateData(FALSE);//更新编辑框内容（主要是接收编辑框中的）
}
m_ComboBox.SetCurSel(m_Index);//打开软件时串口选择框默认显示COM1 子选项编号的排序是从0开始的。
}
void CMFCApplication2Dlg::OnClickedButton3(){
if(!m_Opened)
return; //检查串口是否打开
UpdateData(true);
int len= m_strTXData.GetLength();
cout<<"len="<<len;
/*CString str=m_strTXData;
char *strSend;
strSend=str.GetBuffer(str.GetLength());
cout<<strSend;
//char send[]=str.GetBuffer(str.GetLength());
if (strcmp("NOT FOUND", strSend) == 0)
return;
port1.WriteToPort(strSend); //发送数据
*/
m_strRXData="";
//--------------------------------------------------------
CString s1 = m_strAngle1+"_"+m_strAngle2+"@"+m_strAngle3+"&80$102";
char *strSend = s1.GetBuffer(s1.GetLength());
strSend=s1.GetBuffer(s1.GetLength());
cout<<strSend;
UpdateData(false);
}
LONG CMFCApplication2Dlg::OnCommunication(WPARAM ch, LPARAM port){
char a =ch;
m_strRXData.SetString(m_strRXData+a);
UpdateData(FALSE);
return 0;
}
void CMFCApplication2Dlg::OnClickedButton4()
{
// TODO: 在此添加控件通知处理程序代码
port1.ClosePort();
m_Opened=false;
}
void CalculateTurn(float Dxin,float Dyin,float Cyin,float Bxin)
{
float Dx,Dy,Cx,Cy,Bx,By;
float Lac,Ldc,Lad; //实际长度
float ldc2,ldb2,lbc2,lda2; //像素长度^2
float Cdcb,Cdca,Cturn1;
float r=0; //后期测
//测量的长度,正式测试需填
//r=0;
Dx=Dxin,Dy=Dyin; //物体的中心点，入参
Cx=320,Cy=Cyin; //中线上的矫正点，后期测
Bx=Bxin,By=480; //60度上的矫正点，后期测
Lac=r; //AC实际长度，后期测
float Pr; //每个像素点的长度；
Pr=r/sqrt(((Cx*Cx-Bx*Bx)+(Cy*Cy-Bx*By)));
ldc2=(Dx-Cx)*(Dx-Cx)+(Dy-Cy)*(Dy-Cy);
Ldc=sqrt(ldc2)*Pr; //DC实际长度
ldb2=(Dx-Bx)*(Dx-Bx)+(Dy-By)*(Dy-By);
lbc2=(Bx-Cx)*(Bx-Cx)+(By-Cy)*(By-Cy);
Cdcb=acos((ldc2+lbc2-ldb2)/(2*sqrt(ldc2)*sqrt(lbc2)))*(180/3.1415926);
Cdca=Cdcb+60;
lda2=ldc2+r*r-2*sqrt(ldc2)*r*cos(Cdca);
Lad=sqrt(lda2)+Pr; //AD实际长度
Cturn1=acos((Lac*Lac+Lad*Lad-Ldc*Ldc)/(2*Lac*Lad))*(180/3.1415926); //主轴转动角度
float L1,L2,L3,L4;//L1底座至第二舵机长度，L2第一舵机至第三舵机长度，L3第三舵机至爪尖长度的平方^2，L4为所求公共边长度
float Cturn2,Cturn3,Ca,Cb;
L1=95;
L2=160;
L4=200;
Ca=atan(Lad/L1)*(180/3.1415926);
L3=L1*L1+Lad*Lad;
Cb=acos((L2*L2+L3-L4*L4)-2*L2*sqrt(L3))*(180/3.1415926);
Cturn2=180-Ca-Cb;
Cturn3=180-acos((L2*L2+L4*L4-L3)-2*L2*L4)*(180/3.1415926);
cout<<"Cturn1="<<Cturn1<<endl;
cout<<"Cturn2="<<Cturn2<<endl;
cout<<"Cturn3="<<Cturn3<<endl;
}
void CMFCApplication2Dlg::OnClickedButton5()
{
// TODO: 在此添加控件通知处理程序代码
UpdateData(TRUE);
float Dx,Dy,Cx,Cy,Bx,By;
float Lac,Ldc,Lad; //实际长度
float ldc2,ldb2,lbc2,lda2; //像素长度^2
float Cdcb,Cdca,Cturn1;
float r=270; //后期测
//测量的长度,正式测试需填
//r=0;
Dx=atof(m_strXmid),Dy=atof(m_strYmid); //物体的中心点，入参
Cx=320,Cy=480-320*tan(30*3.1415926/180); //中线上的矫正点，后期测
Bx=640,By=480; //60度上的矫正点，后期测
ldb2=(Dx-Bx)*(Dx-Bx)+(Dy-By)*(Dy-By);
Lac=r; //AC实际长度 小写l为像素长度，大写的L为实际长度
float Pr; //每个像素点的长度；
Pr=r/sqrt((Cx-Bx)*(Cx-Bx)+(Cy-By)*(Cy-By));
ldc2=(Dx-Cx)*(Dx-Cx)+(Dy-Cy)*(Dy-Cy);
Ldc=sqrt(ldc2)*Pr; //DC实际长度
lbc2=(Bx-Cx)*(Bx-Cx)+(By-Cy)*(By-Cy);
Cdcb=acos((ldc2+lbc2-ldb2)/(2*sqrt(ldc2)*sqrt(lbc2)))*(180/3.1415926);
if(Dx>0&&Dx<640 && Dy>480-320*tan(30*3.1415926/180) && Dy<480){
Cdca=60-Cdcb;
}else{
Cdca=Cdcb+60;
}
lda2=ldc2+(r/Pr)*(r/Pr)-2*sqrt(ldc2)*(r/Pr)*cos(Cdca);
//Lad=sqrt(lda2)*Pr; //AD实际长度
Lad=sqrt(r*r+Ldc*Ldc-2*r*Ldc*cos(Cdca*(3.1415926)/180));
Cturn1=acos((Lac*Lac+Lad*Lad-Ldc*Ldc)/(2*Lac*Lad))*(180/3.1415926); //主轴转动角度
float L1,L2,L3,L4;//L1底座至第二舵机长度，L2第一舵机至第三舵机长度，L3第三舵机至爪尖长度的平方^2，L4为所求公共边长度
float Cturn2,Cturn3,Ca,Cb;
L1=95;//底座的高
L2=160;//內臂的高
L4=200;//外臂的高
Ca=atan(Lad/L1)*(180/3.1415926);
L3=L1*L1+Lad*Lad;
Cb=acos((L2*L2+L3-L4*L4)/(2*L2*sqrt(L3)))*(180/3.1415926);
Cturn2=180-Ca-Cb;
Cturn3=180-acos((L2*L2+L4*L4-L3)/(2*L2*L4))*(180/3.1415926);
if(atof(m_strXmid)>=320)
Cturn1=135-Cturn1;
else
Cturn1=Cturn1+135;
Cturn2=135+Cturn2;
Cturn3=135-Cturn3;
CString strAngle1="",strAngle2="",strAngle3="";
CString output="";
Cturn1=Cturn1+atof(m_strAngleA);
Cturn2=Cturn2+atof(m_strAngleB);
Cturn3=Cturn3-atof(m_strAngleC);
strAngle1.Format("%f",Cturn1);
strAngle2.Format("%f",Cturn2);
strAngle3.Format("%f",Cturn3);
m_strAngle1=strAngle1;
m_strAngle2=strAngle2;
m_strAngle3=strAngle3;
m_strTXData=m_strAngle1+"_"+m_strAngle2+"@"+m_strAngle3+"&80$102";
UpdateData(FALSE);
}
