Create your Gitee Account
Explore and code with more than 6 million developers,Free private repositories !:)
Sign up
Clone or download
main.cpp 2.42 KB
Copy Edit Web IDE Raw Blame History
wangcongrobot authored 2018-01-10 21:16 . move .h to include.
#include "serial.h"
#include "timer.h"
#include "createFile.h"
#include "motionControl.h"
#include "pthreadCreate.h"
#include "utilities.h"
#include <vector>
const double Pi = 3.14159265358979;
unsigned char com0RecvBuf[64];
unsigned char com0SendBuf[64]; //接收与发送缓冲区
int main()
{
initSerial();
initTimer();
createFile();
pthread_t tid1, tid2;
int rc1=0, rc2=0;
rc2=pthread_create(&tid2, NULL, thread2, NULL);
usleep(10000);
if(rc2 != 0)
printf("pthread_create thread2 error!\n");
rc1 = pthread_create(&tid1, NULL, thread1, &tid2);
usleep(10000);
if(rc1 != 0)
printf("pthread_create thread1 error!\n");
printf("enter main\n");
sInit();
for(int i=0; i<5; i++)sleep(1);
//while(1){sleep(5);}
//sleep(5);
// call for getting current joint
//getCurrentJoint();
//while(1){sleep(1);}
parse();
while(get_current_joint_flag==0) // wait for get current joint
{
sleep(1);
printf("-------------\n");
continue;
}
printf("-----Start motion control!-----\n");
printf("\n-----Start IK calculation...-----\n");
//while(1){sleep(1);}
jointFromIK = IK(currentJoint, 0, 0, 0.2, 50);
cout << "Number of jointFromIK: " << jointFromIK.size() << endl;
vector<double>::iterator iter;
for( iter=jointFromIK.begin(); iter!=(jointFromIK.begin() + 6); iter++)
{
cout << *iter*180/Pi << endl;
}
sleep(5);
int IK_num=jointFromIK.size()/6; //10
cout << IK_num << endl;
//return 0;
for(int j=0; j<IK_num; j++)
{
SendEN=0; //正在设定新的关节控制信息,禁止发送数据
int k=0;
for(iter=(jointFromIK.begin()+j*6); iter!=(jointFromIK.begin()+j*6 +6); iter++)
{
Joint_AngleSet_r[k] = *iter;
cout << "Joint_AngleSet_r" << k << " -- " << Joint_AngleSet_r[k] *180/Pi<< endl;
k++;
}
usleep(50000);
mCmd.Frz=0;
AngleConvert();
//NewEnd_Pose=0; //禁止更新关节信息,直到新的逆解结果更新
memcpy(com0SendBuf,&mCmd,sizeof(MCMD)); //把新的关节控制信息传给串口数据数组
writeToSerial(com0SendBuf,24);
//SendEN=1; //发送数据
printf("Num%d IK send successfully!\n", j);
}
printf("motion control successfully!\n");
parse();
sleep(10);
printf("leave main\n");
return 0;
}

Comment ( 0 )

Sign in for post a comment