/*
 * manipulator_contronl.c
 *
 *  Created on: 2023年10月7日
 *      Author: Administrator
 */

// Robot joint zero position
#include "MSP/msp_manipulator_contronl.h"


int Weld_Start_Position[4]={1466570,-7656470,1322021,3140335};//机器人开始焊接位置
int Shutdown_Position[4]={1466571,-9814715,2471855,3140335};//机器人停机位置



double One_Cri_count_101=6619136.0;
double Welding_Swing_Angle=6619136.0/18*1;

double Welding_Count_Inc_max=6619136.0/360*2;

double Welding_swing_speed=400;


int Joint_Init_count[4]={1496969,  -8407211, 85796,  1716376  };      //机器人初始位置


//int  Joint_Init_count[4]={1496969,  -7486090, 1188157, 3003015  };  // 调试用关节位置；




double Motor_5_max=3151753;  double Motor_5_min= -157815;

double Motor_6_max=-6752427;
double Motor_7_max=3237800;
double Motor_8_max=4868300;

double One_Deg_rad=0.017453292519943;
double Sol_angle_error=1.0/180*Pi;

double Dersire_Angle;

// Robot kinematic parameters
double seta1_offset=0;
double d1_N=0;
double a1_N=2;

double seta2_offset=0*-90/180*Pi+-1.535889741755010;
double d2_N=14;
double a2_N=262;

double seta3_offset=0.034906585039887;
double d3_N=2;
double a3_N=202;

double seta4_offset=0.034906585039887;
double d4_N=37;
double a4_N=202;
double Y4_N=2;


double YY1;
double ZZ1;
double S_seta3F3;
double Wel_M_Joint_1[4]={0,0,0,0};


int Motor_Flag=1;

int Motor_5_Pos  =1;
int  Motor_6_Pos =1;
int  Motor_7_Pos =1;
int  Motor_8_Pos =1;
int BM_Wel_Con_State=2;

void Manipulator_Count_Angle(double Manip_Count[], double *Manip_Angle )
{
	Manip_Angle[0]=(double)(1.0*(Manip_Count[0]-Joint_Init_count[0])/(One_Cri_count_101)*360.0)*One_Deg_rad/1.0;


	Manip_Angle[1]=(double)(1.0*(Manip_Count[1]-Joint_Init_count[1])/(One_Cri_count_101)*360.0)*One_Deg_rad/1.0;
	Manip_Angle[2]=(double)(1.0*(Manip_Count[2]-Joint_Init_count[2])/(One_Cri_count_101)*360.0)*One_Deg_rad/1.0;
	Manip_Angle[3]=(double)(1.0*(Manip_Count[3]-Joint_Init_count[3])/(One_Cri_count_101)*360.0)*One_Deg_rad/1.0;

}


void Manipulator_angle_count(double Manip_Angle[], double *Manip_Count )
{

	Manip_Count[0]=(int)(Manip_Angle[0]/Pi*180/360*One_Cri_count_101+Joint_Init_count[0]);
	Manip_Count[1]=(int)(Manip_Angle[1]/Pi*180/360*One_Cri_count_101+Joint_Init_count[1]);
	Manip_Count[2]=(int)(Manip_Angle[2]/Pi*180/360*One_Cri_count_101+Joint_Init_count[2]);
	Manip_Count[3]=(int)(Manip_Angle[3]/Pi*180/360*One_Cri_count_101+Joint_Init_count[3]);
}

void Manipulator_FK(double Manip_Angle[], double *Manip_Position )
{
double seta1_N=Manip_Angle[0]+seta1_offset;
double seta2_N=Manip_Angle[1]+seta2_offset;
double seta3_N=Manip_Angle[2]+seta3_offset;
double seta4_N=Manip_Angle[3]+seta4_offset;

double A11=cos(seta1_N)*cos(seta2_N+seta3_N+seta4_N);
double A12=-cos(seta1_N)*sin(seta2_N+seta3_N+seta4_N);
double A13=-sin(seta1_N);
double A14=cos(seta1_N)*(a1_N+a2_N*cos(seta2_N)+a3_N*cos(seta2_N+seta3_N))-(d2_N+d3_N)*sin(seta1_N);
double A21=sin(seta1_N)*cos(seta2_N+seta3_N+seta4_N);
double A22=-sin(seta1_N)*sin(seta2_N+seta3_N+seta4_N);
double A23=cos(seta1_N);
double A24=sin(seta1_N)*(a1_N+a2_N*cos(seta2_N)+a3_N*cos(seta2_N+seta3_N))+(d2_N+d3_N)*cos(seta1_N);
double A31=-sin(seta2_N+seta3_N+seta4_N);
double A32=-cos(seta2_N+seta3_N+seta4_N);
double A33=0;
double A34=d1_N-a2_N*sin(seta2_N)-a3_N*sin(seta2_N+seta3_N);

Manip_Position[0]=(A11*a4_N+A12*Y4_N+A13*d4_N+A14)*1.0;
Manip_Position[1]=(A21*a4_N+A22*Y4_N+A23*d4_N+A24)*1.0;
Manip_Position[2]=(A31*a4_N+A32*Y4_N+A33*d4_N+A34)*1.0;

}


void Manipulator_IK(double Manip_Position[], double Dersire_Angle_234 , double *Manip_Angle )
{
	double PX_E=Manip_Position[0];double PY_E=Manip_Position[1];double PZ_E=Manip_Position[2];


	double DDD_End=d2_N+d3_N+d4_N;
	double DDD_O4=d2_N+d3_N;

	//Angle_1 solve

	double Fai_y_M=atan2(PY_E,PX_E);
	double S_seta1F=asin(DDD_End/sqrt(PX_E*PX_E+PY_E*PY_E));
	double S_seta1F_M1=-S_seta1F+Fai_y_M;
	double seta1_true=S_seta1F_M1-seta1_offset;

	//Angle_3 solve

	//double PX_O4=PX_E-(cos(seta1_true)*cos(Dersire_Angle_234)*a4_N- cos(seta1_true)*sin(Dersire_Angle_234)*Y4_N -sin(seta1_true)*d4_N);
	double PY_O4=PY_E-(sin(seta1_true)*cos(Dersire_Angle_234)*a4_N- sin(seta1_true)*sin(Dersire_Angle_234)*Y4_N +cos(seta1_true)*d4_N);
	double PZ_O4=PZ_E-(-sin(Dersire_Angle_234)*a4_N- cos(Dersire_Angle_234)*Y4_N );

	double YY1=(PY_O4-DDD_O4*cos(seta1_true))/sin(seta1_true)-a1_N;
	double ZZ1=d1_N-PZ_O4;

	double seta3_1=acos((YY1*YY1+ZZ1*ZZ1-a2_N*a2_N-a3_N*a3_N)/(2*a2_N*a3_N));
	double seta3_ture=seta3_1;

	//Angle_2 solve
	double fai_y22=atan2(ZZ1,YY1);
	double seta2_1_dou=asin(-a3_N*sin(seta3_ture)/sqrt(YY1*YY1+ZZ1*ZZ1)  );
	double seta2_S1= seta2_1_dou+ fai_y22 ;
	double seta2_ture=seta2_S1;

	//Angle_4 solve
	double seta4_ture=Dersire_Angle_234-seta2_ture-seta3_ture;

	Manip_Angle[0]=seta1_true-seta1_offset;
	Manip_Angle[1]=seta2_ture-seta2_offset;
	Manip_Angle[2]=seta3_ture-seta3_offset;
	Manip_Angle[3]=seta4_ture-seta4_offset;

}



double Motor_Count[4];
double Manip_Velocity[4]={0,0,0,0};
double Manip_Pos_Count[4]={0,0,0,0};

double Manip_Angle_Rad[4];
double Manip_Pos_MM[3];


double Manip_Angle_New[4];

double Motor_5_Pos_last=0;
double Motor_6_Pos_last=0;
double Motor_7_Pos_last=0;
double Motor_8_Pos_last=0;

double Dersire_Angle_flag=1;
double Dersire_Angle_Rad_DEG=0;

double Manip_Count_NEW[4];
double Dersire_Angle_Rad;

double Manip_Count_New[4];

double Weld_Gun_Activ=0;

double Manip_Count_Inc[4]={0,0,0,0};

int MASK_flag=1;

void GF_Welding_Arm_Motion_Func( double Pos_Inc[])  //, int Motor_Pos_Count[]
{
	switch(MASK_flag)
	{
	case 1:
		Motor_Count[0]=  Motor_5_Pos ;
		Motor_Count[1]=  Motor_6_Pos ;
		Motor_Count[2]=  Motor_7_Pos ;
		if(Weld_Gun_Activ==1)
		{
			Motor_Count[3]=  Wel_M_Joint_1[3];
		}
		else
		{
			Motor_Count[3]=   Motor_8_Pos ;
			Wel_M_Joint_1[3]= Motor_Count[3];
			Weld_Gun_Activ=1;
		}
		Manipulator_Count_Angle(Motor_Count,  Manip_Angle_Rad );
		Manip_Velocity[3]=Welding_swing_speed;

		Manip_Count_New[0]=Motor_Count[0];
		Manip_Count_New[1]=Motor_Count[1];
		Manip_Count_New[2]=Motor_Count[2];
		Manip_Count_New[3]=Motor_Count[3];

		Manipulator_FK( Manip_Angle_Rad,  Manip_Pos_MM );//current Position

		MASK_flag=2;

		if  ((Motor_8_Pos>Joint_Init_count[3])&(Motor_8_Pos<Motor_8_max))  //假设读取第一时刻的位置
		{
			if(Dersire_Angle_flag==1)
			{
			Dersire_Angle_Rad=Manip_Angle_Rad[1]+Manip_Angle_Rad[2]+Manip_Angle_Rad[3]+seta2_offset+seta3_offset+seta4_offset;//期望机器人空间姿态
			Dersire_Angle_flag=2;
			}
		}
		else
		{
			MASK_flag=1;
		}
		break;
	case 2:
		if((Pos_Inc[0]==0)&(Pos_Inc[1]==0)&(Pos_Inc[2]==0))
		{
			BM_Wel_Con_State=2;   //电机运动模式指令
			Motor_Count[0]=  Motor_5_Pos ;
			Motor_Count[1]=  Motor_6_Pos ;
			Motor_Count[2]=  Motor_7_Pos ;
			Motor_Count[3]=  Wel_M_Joint_1[3];
		}
		else
		{
			BM_Wel_Con_State=1;  //电机运动模式指令
			Manip_Pos_MM[0]=Manip_Pos_MM[0]+Pos_Inc[0];
			Manip_Pos_MM[1]=Manip_Pos_MM[1]+Pos_Inc[1];
			Manip_Pos_MM[2]=Manip_Pos_MM[2]+Pos_Inc[2];  //laster Position

			Manipulator_IK(Manip_Pos_MM, Dersire_Angle_Rad, Manip_Angle_New);//Inverse kinematics solution
			Manipulator_angle_count(Manip_Angle_New,  Manip_Count_New );//已经计算出最新的关节位置，下面将要发送关节位置

			Pos_Inc_verification();
			Wel_M_Joint_1[3]=Manip_Count_New[3];
		}

		if (Motor_8_Pos>(Wel_M_Joint_1[3]+Welding_Swing_Angle))
		{
			Manip_Velocity[3]=-Welding_swing_speed;
		}
		else if(Motor_8_Pos<(Wel_M_Joint_1[3]-Welding_Swing_Angle))
		{
			Manip_Velocity[3]=Welding_swing_speed;
		}

		MASK_flag=3;
		break;
	case 3:
		MASK_flag=4;
			break;
	case 4:
		MASK_flag=5;
			break;
	case 5:
		MASK_flag=6;
			break;
	case 6:
		MASK_flag=7;
			break;
	case 7:
		MASK_flag=8;
			break;
	case 8:
		MASK_flag=9;
			break;
	case 9:
		MASK_flag=10;
			break;
	case 10:
		MASK_flag=11;
			break;
	case 11:
		MASK_flag=12;
			break;
	case 12:
		MASK_flag=13;
			break;
	case 13:
		MASK_flag=14;
			break;
	case 14:
		MASK_flag=15;
			break;
	case 15:
		MASK_flag=16;
			break;
	case 16:
		MASK_flag=17;
			break;
	case 17:
		MASK_flag=18;
			break;
	case 18:
		MASK_flag=19;
			break;
	case 19:
		MASK_flag=20;
			break;
	case 20:
		MASK_flag=21;
			break;
	case 21:
		MASK_flag=2;
			break;

	default:

	break;
	}

}



void Pos_Inc_verification()
{

	Manip_Count_Inc[0]=(Manip_Count_New[0]-Motor_Count[0])/40;
	Manip_Count_Inc[1]=(Manip_Count_New[1]-Motor_Count[1])/40;
	Manip_Count_Inc[2]=(Manip_Count_New[2]-Motor_Count[2])/40;
	Manip_Count_Inc[3]=(Manip_Count_New[3]-Motor_Count[3])/40;

	if ((fabs(Manip_Count_Inc[0]))>Welding_Count_Inc_max)
	{
		Manip_Count_Inc[0]=0;
	}
	if ((fabs(Manip_Count_Inc[1]))>Welding_Count_Inc_max)
	{
		Manip_Count_Inc[1]=0;
	}
	if ((fabs(Manip_Count_Inc[2]))>Welding_Count_Inc_max)
	{
		Manip_Count_Inc[2]=0;
	}
	if ((fabs(Manip_Count_Inc[3]))>Welding_Count_Inc_max)
	{
		Manip_Count_Inc[3]=0;
	}

	Manip_Count_New[0]=Manip_Count_Inc[0]*40+Motor_Count[0];
	Manip_Count_New[1]=Manip_Count_Inc[1]*40+Motor_Count[1];
	Manip_Count_New[2]=Manip_Count_Inc[2]*40+Motor_Count[2];
	Manip_Count_New[3]=Manip_Count_Inc[3]*40+Motor_Count[3];

	double Manip_Velocity_count[4];

	Manip_Velocity_count[0]=(Manip_Count_New[0]-Motor_Count[0])/0.04;
	Manip_Velocity_count[1]=(Manip_Count_New[1]-Motor_Count[1])/0.04;
	Manip_Velocity_count[2]=(Manip_Count_New[2]-Motor_Count[2])/0.04;
	Manip_Velocity_count[3]=(Manip_Count_New[3]-Motor_Count[3])/0.04;

	Manip_Velocity[0]=Manip_Velocity_count[0]/One_Cri_count_101*360*101*100/360;
	Manip_Velocity[1]=Manip_Velocity_count[1]/One_Cri_count_101*360*101*100/360;
	Manip_Velocity[2]=Manip_Velocity_count[2]/One_Cri_count_101*360*101*100/360;

	Motor_Flag=1;


	Motor_Count[0]=Manip_Count_New[0];
	Motor_Count[1]=Manip_Count_New[1];
	Motor_Count[2]=Manip_Count_New[2];
	Motor_Count[3]=Manip_Count_New[3];


}









