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

//#ifndef INC_MANIPULATOR_CONTRONL_H_
//#define INC_MANIPULATOR_CONTRONL_H_

#ifndef INC_MSP_MSP_MANIPULATOR_CONTRONL_H_
#define INC_MSP_MSP_MANIPULATOR_CONTRONL_H_

#include "BSP/bsp_include.h"

extern int Weld_Start_Position[4];//机器人开始焊接位置
extern int Shutdown_Position[4];



#define  Pi  3.141592653589793
void Manipulator_Count_Angle(double Manip_Count[], double *Manip_Angle );
void Manipulator_angle_count(double Manip_Angle[], double *Manip_Count );
void Manipulator_FK(double Manip_Angle[], double *Manip_Position );
void Manipulator_IK(double Manip_Position[], double Dersire_Angle_234 , double *Manip_Angle );
//void Welding_Arm_Motion_Func(double Pos_Inc[], double Motor_Count[], double Dersire_Angle_Rad,double *Motor_Incr_Deg ,double *Motor_Incr_Count );
void GF_Welding_Arm_Motion_Func( double Pos_Inc[]);
void Read_Motor_Pos_Count();
void Pos_Inc_verification();


extern double Dersire_Angle;
extern double seta2_offset;
extern double seta3_offset;
extern double seta4_offset;
extern double Manip_Count_New[4];
extern double Wel_M_Joint_1[4];
extern double Manip_Velocity[4];
extern int MASK_flag;
extern double Manip_Count_Inc[4];
extern double Motor_Count[4];

extern double Motor_5_max; extern  double Motor_5_min;




extern double Motor_6_max;
extern double Motor_7_max;
extern double Motor_8_max;

#endif /* INC_MANIPULATOR_CONTRONL_H_ */
