机器狗开发——keil 建模及任务发送-到达指定点

FW_MODEL_H

//#pragma once
//#include <iostream>

#ifndef FW_MODEL_H
#define FW_MODEL_H
#endif


#include <math.h>//在C++中为<cmath>
//using namespace std;

/*
建立机器人坐标 注：以右前腿为例，电机ID分别为0,1,2

坐标系建立 以右前腿，按改进DH参数方法（坐标系原点建立在连杆的起始端）

建立0（电机0到1轴交点）,1（初始坐标与0坐标重合）2（1电机轴线与足点竖直重力方向交点）3（膝关节）

内部角度统一为弧度

*/
#define PI           3.1415926f
#define RD(radian)   ((radian) * (180.0 / M_PI)) //弧度转角度
#define DR(degree)   ((degree) * (M_PI / 180.0)) //角度转弧度

//测量参数：轴线之间的距离（公垂线距离）  单位（mm）
//小腿到轮轴线距离：150
#define m_d1  80  //坐标系1到坐标系2的连杆偏距
#define m_l1  200  //大腿长
#define m_l2  180  //小腿长


#define Reduction_Ratio  9.1f //减速比（电机轴 输出轴）的倒数   1/9.1
#define Inverse_Reduction_Ratio  0.1098901f //减速比（电机轴 输出轴）的倒数   1/9.1

//硬限位，按坐标建立的正负向，面朝Z轴指向时，逆时针为 正，单位(弧度)

/*参考值，来源github- aliengo*/
//constexpr double a1_Hip_max = 0.802;    // unit:radian ( = 46   degree)
//constexpr double a1_Hip_min = -0.802;   // unit:radian ( = -46  degree)
//constexpr double a1_Thigh_max = 4.19;     // unit:radian ( = 240  degree)
//constexpr double a1_Thigh_min = -1.05;    // unit:radian ( = -60  degree)
//constexpr double a1_Calf_max = -0.916;   // unit:radian ( = -52.5  degree)
//constexpr double a1_Calf_min = -2.7;     // unit:radian ( = -154.5 degree)
/**/
#define theta1_hard_plus_lim  50/180.0*PI  //关节角硬限位
#define theta1_hard_lose_lim  -45/180.0*PI  //关节角硬限位
#define theta2_hard_plus_lim  45/180.0*PI //关节角硬限位
#define theta2_hard_lose_lim  -235/180.0*PI  //关节角硬限位
#define theta3_hard_plus_lim  155/180.0*PI  //关节角硬限位
#define theta3_hard_lose_lim  45/180.0*PI //关节角硬限位

//软限位，按坐标建立的正负向，面朝Z轴指向时，逆时针为 正
#define theta1_soft_plus_lim      43/180.0*PI  //关节角软限位 42
#define theta1_soft_lose_lim     -41/180.0*PI   //关节角软限位 -40
#define theta2_soft_plus_lim      36/180.0*PI  //关节角软限位   35
#define theta2_soft_lose_lim    -221/180.0*PI   //关节角软限位  -220
#define theta3_soft_plus_lim     155.6/180.0*PI   //关节角软限位  155 
#define theta3_soft_lose_lim      44/180.0*PI   //关节角软限位   45

//腿初始上电位置关节角
#define InitTheta1  0.0/180.0*PI     //1关节
#define InitTheta2  -70.0/180.0*PI   //2关节
#define InitTheta3  155.0/180.0*PI   //3关节

//struct JointAngelData
//{
//	float theta1; //关节角1
//	float theta2;
//	float theta3;
//};

class DogModel
{
public:
	DogModel();
	~DogModel();
	//double get_theta1();
	//double get_theta2();
	//double get_theta3();

	/*获取关节角度（弧度）结构体*/
    //JointAngelData get_Joint_theta();

	/*足点到{0}的，运动学(弧度0，角度1)*/
	void kine_04(bool Rad0_Deg1, const float & ct1, const float & ct2, const float & ct3);

	
   /*足点笛卡尔映射到三个关节，逆运动学 输出单位（弧度）*/
	void ikine_04(const float& px, const float& py, const float& pz);
//	/*速度雅克比*/
//	void vel_jacobian();
//	/*力雅克比*/
//	void F_jacobian();

	/*建立机器人参数（弧度）*/
    //时变当前位置
	float theta1; //关节角1
	float theta2;
	float theta3;
	//数组形式的时变当前位置
	float theta[3];
	
	float car0X; //笛卡尔三个坐标位置
	float car0Y;
	float car0Z;
	
	//数组形式的时变当前位置（笛卡尔xyz）
	float car0[3]; //基于腿关节
	float carB[3]; //基于机器人质心位置
	float carW[3]; //基于初始平面位置的世界位置
	
	/*建立机器人目标参数（弧度）*/
	//时变目标位置
	float t_theta[3];
	
	//计算关节位置，用于笛卡尔求逆解
	float i_theta[3];


private:
};

moudle.cpp

#include "FW_Model.h"



DogModel::DogModel()
{
	this->theta1 = 0;
	this->theta2 = 0;
	this->theta3 = 0;
	this->car0X = 0;
	this->car0Y = 0;
	this->car0Z = 0;
	
	/*初始化参数（弧度）*/
	for (int i = 0; i < 3; i++) {
        t_theta[i] = 0;
		i_theta[i] = 0;
		 theta[i]  = 0;
    }
	float car0[3] = {0}; //基于腿关节
    float carB[3] = {0};//基于机器人质心位置
	float carW[3] = {0};//基于初始平面位置的世界位置
}
DogModel::~DogModel()
 {
	
 }


//JointAngelData DogModel::get_Joint_theta()
// {
//	JointAngelData data;
//	data.theta1 = this->theta1;
//	data.theta2 = this->theta2;
//	data.theta3 = this->theta3;
//	return data;
// }


/*足点到{0}的，运动学(角度输入)*/
void DogModel::kine_04(bool Rad0_Deg1,const float &ct1, const float& ct2, const float& ct3)
{
	double t1 = ct1;
	double t2 = ct2;
	double t3 = ct3;
    //输入关节角度，求出足点的相对于肩部{0}坐标的笛卡尔位置，姿态暂不考虑
	if (Rad0_Deg1)
	{
		/*先转角度为弧度*/
		t1 = ct1 / 180.0 * PI;//最好是180.0，防止整数相除，不如预期
		t2 = ct2 / 180.0 * PI;
		t3 = ct3 / 180.0 * PI;
		/**/
	}
	/*正运动学*/
	this->car0X = m_l1 * cos(t1)* cos(t2) + m_l2 * cos(t1) * cos(t2 + t3) + m_d1 * sin(t1);
	this->car0Y = m_l1 * sin(t1) * cos(t2) + m_l2 * sin(t1) * cos(t2 + t3) - m_d1 * cos(t1);
	this->car0Z = m_l1 * sin(t2) + m_l2 * sin(t2+t3);
	/**/
}

/*足点笛卡尔映射到三个关节，逆运动学 输出单位（弧度）*/
void DogModel::ikine_04(const float& px, const float& py, const float& pz)
{
	this->i_theta[2] = acos((px * px + py * py + pz * pz - m_l1 * m_l1 - m_l2 * m_l2 - m_d1 * m_d1) / (2 * m_l1 * m_l2));
	this->i_theta[1]= asin(pz / (sqrt(m_l1 * m_l1 + m_l2 * m_l2 + 2 * m_l1 * m_l2 * cos(this->i_theta[2])))) - atan((m_l2 * sin(this->i_theta[2])) / (m_l1 + m_l2 * cos(this->i_theta[2])));
	this->i_theta[0] = asin(m_d1 / (sqrt(px * px + py * py))) + atan(py / px);

}

#ifndef FW_CONTROL_H
#define FW_CONTROL_H
#endif





#include "FW_Model.h"
#include "BC_HAL.h"


struct ParThreePlan
{
    float a0;
	float a1;
	float a2;
	float a3;
};


class FwControl
{
public:
	FwControl(DogModel * dog_model, BC_HAL_c *bc_hal_c);
	~FwControl();



    /*初始化关节角度*/
    void setInitJointTheta();
    /*刷新当前关节角度*/
    void refCurJointTheta();

    /*计算单关节三次多项式轨迹规划参数（关节空间）单位（角度）*/
    void cal_Three_Plan(uint8_t thetaIndex,const float start_theta,const float end_theta,const float time_start_0);

    /*计算笛卡尔轨迹规划（笛卡尔）单位（mm）*/
    void cal_Three_Cartesion_Trajectory(const float t_x,const float t_y,const float t_z,const float time_duration);

    /*在轨迹中修改关节角度*/
    void modif_Joint_Theta(float curTime);

    /*计算目标编码器值（弧度）*/
    void cal_T_Encoder();
	
	/*是否超过软限位*/
    bool exceeded_Soft_Limit();

    /*执行三次多项式轨迹规划*/
    //void three_Tragectory();



    //getTheta_Three_Path
	DogModel * m_dog_model;
	BC_HAL_c * m_bc_hal_c;

    float init_encoder1;
    float init_encoder2;
    float init_encoder3;
    ParThreePlan par_threePlan[3];
	float t_encoder[3];

private:
	

};

#include "FW_Control.h"


FwControl::FwControl(DogModel * dog_model, BC_HAL_c *bc_hal_c)
{
	 init_encoder1= 0;
     init_encoder2= 0;
     init_encoder3= 0;

	for (int i = 0; i < 4; i++) {
        par_threePlan[i].a0 = 0;
		par_threePlan[i].a1 = 0;
		par_threePlan[i].a2 = 0;
		par_threePlan[i].a3 = 0;
 
    }
	float t_encoder[3] = {};
		
	//ParThreePlan par_threePlan[3] = {};
	m_dog_model = dog_model;
	m_bc_hal_c = bc_hal_c;
}

FwControl::~FwControl()
{
	
}


 /*初始化单腿关节角度*/
void FwControl:: setInitJointTheta()
{
	//将编码器读取的值与关节角度绑定
	//开机时要明确既定的机器人结构位置
	//绝对值多圈编码器导致
	
   // dog_model->theta1 = bc_hal_c->ServoMotorState[0].Pos_now;   //单位：弧度
	m_dog_model->theta1 =InitTheta1;
	m_dog_model->theta2 =InitTheta2;
	m_dog_model->theta3 =InitTheta3;
	
	/*记录初始化编码器值*/
	this->init_encoder1 = m_bc_hal_c->ServoMotorState[0].Pos_now;
	this->init_encoder2 = m_bc_hal_c->ServoMotorState[1].Pos_now;
	this->init_encoder3 = m_bc_hal_c->ServoMotorState[2].Pos_now;
	
	
}

 /*刷新当前关节角度*/
void FwControl::refCurJointTheta()
{
	//利用当前编码器值与初始编码器值的差来计算当前关节角,
	//考虑减速比 9.1
	//并修改机器人自身存储的关节角度值
	m_dog_model->theta1 =InitTheta1+(m_bc_hal_c->ServoMotorState[0].Pos_now - init_encoder1)*Inverse_Reduction_Ratio; //1/9.1
	m_dog_model->theta2 =InitTheta2+(m_bc_hal_c->ServoMotorState[1].Pos_now - init_encoder2)*Inverse_Reduction_Ratio;
	m_dog_model->theta3 =InitTheta3+(m_bc_hal_c->ServoMotorState[2].Pos_now - init_encoder3)*Inverse_Reduction_Ratio;
	
	//数组刷新
	m_dog_model->theta[0] =InitTheta1+(m_bc_hal_c->ServoMotorState[0].Pos_now - init_encoder1)*Inverse_Reduction_Ratio; //1/9.1
	m_dog_model->theta[1]  =InitTheta2+(m_bc_hal_c->ServoMotorState[1].Pos_now - init_encoder2)*Inverse_Reduction_Ratio;
	m_dog_model->theta[2] =InitTheta3+(m_bc_hal_c->ServoMotorState[2].Pos_now - init_encoder3)*Inverse_Reduction_Ratio;

}

/*计算三次多项式轨迹规划参数（关节空间），一个关节(从1开始描述关节)*/
    void FwControl::cal_Three_Plan(uint8_t thetaIndex,const float start_theta,const float end_theta,const float time_duration)
	{
	    par_threePlan[thetaIndex-1].a0 = start_theta;
		par_threePlan[thetaIndex-1].a1 = 0;
		par_threePlan[thetaIndex-1].a2 = (3.0f/(time_duration*time_duration))*(end_theta-start_theta);
		par_threePlan[thetaIndex-1].a3 = (-2.0f/(time_duration*time_duration*time_duration))*(end_theta-start_theta);
	
	}
	
/*计算笛卡尔轨迹规划（笛卡尔）单位（mm）*/
    void FwControl::cal_Three_Cartesion_Trajectory(const float t_x,const float t_y,const float t_z,const float time_duration)
	{
		//此处使用机器人当前位置计算逆解
		m_dog_model->ikine_04(t_x,t_y,t_z);
		
		//开始关节输入关节角，结束关节输入运动学逆解关节角
		//注：计算公式必须使用角度
	  	for(int i = 0;i<3;i++){
        par_threePlan[i].a0 = m_dog_model->theta[i]/PI*180.0;
		par_threePlan[i].a1 = 0;
		par_threePlan[i].a2 = (3.0f/(time_duration*time_duration))*(m_dog_model->i_theta[i]/PI*180.0-m_dog_model->theta[i]/PI*180.0);
		par_threePlan[i].a3 = (-2.0f/(time_duration*time_duration*time_duration))*(m_dog_model->i_theta[i]/PI*180.0-m_dog_model->theta[i]/PI*180.0);
		}
	}
/*在轨迹中修改单关节角度*/
    void FwControl::modif_Joint_Theta(float curTime)
	{
		
	    float theta_out[3]= {0};
		
		for(int i = 0;i<3;i++){
			//三次多项式：
		  //theta_out[i] = (par_threePlan[i].a0)-curTime/2.0f;
			//theta_out[i] = 155;
			
			theta_out[i] = (par_threePlan[i].a0) + (par_threePlan[i].a1*curTime) + (par_threePlan[i].a2*curTime*curTime)  + (par_threePlan[i].a3*curTime *curTime *curTime);
			
			//theta_out[i] =155-curTime/3.0f;
		}
		//转为弧度值，并更新目标关节角
		for(int i = 0;i<3;i++){
           m_dog_model->t_theta[i] = theta_out[i]/180.0f*PI;
		}
		
		//dog_model->t_theta[2] = theta_out[2]/180.0*PI;
		//dog_model->t_theta[2] = (155-curTime/3.0f)/180.0*PI;
		//t_encoder[2] = init_encoder3 + (dog_model->t_theta[2]-InitTheta3)*Reduction_Ratio;
		
		//t_encoder[2] = init_encoder3-curTime/2.0f;
	}
	
 /*计算目标编码器值（弧度）*/
    void FwControl::cal_T_Encoder()
	{
	    //计算发送给电机的位置参数
//		for(int i = 0;i<3;i++){
//           
//		}
		t_encoder[0] = init_encoder1 + (m_dog_model->t_theta[0]-InitTheta1)*Reduction_Ratio;
		t_encoder[1] = init_encoder2 + (m_dog_model->t_theta[1]-InitTheta2)*Reduction_Ratio;
		t_encoder[2] = init_encoder3 + (m_dog_model->t_theta[2]-InitTheta3)*Reduction_Ratio;
	}
	
	
/*是否超过软限位*/
    bool FwControl::exceeded_Soft_Limit()
	{
	   if(m_dog_model->theta1>theta1_soft_plus_lim ||m_dog_model->theta1<theta1_soft_lose_lim)
	   {
		  m_bc_hal_c->HAL_ServoMotorALL_SetStop();
	     return true;
	   }else  if(m_dog_model->theta2>theta2_soft_plus_lim ||m_dog_model->theta2<theta2_soft_lose_lim)
	   {
		   m_bc_hal_c->HAL_ServoMotorALL_SetStop();
	     return true;
	   }else  if(m_dog_model->theta3>theta3_soft_plus_lim ||m_dog_model->theta3<theta3_soft_lose_lim)
	   {
		   m_bc_hal_c->HAL_ServoMotorALL_SetStop();
	     return true;
	   }else
	   {
		   return false;
	   }
	   return false;
	}

---

指令层：用在freeRTOS 中进行任务调度   如何到达一个点。

bc_hal 是输入及输出。输入为电机参数，输出为电机编码器等参数

#ifndef FW_INSTRACT_H
#define FW_INSTRACT_H
#endif
#include "FW_Control.h"


class FwInstract
{
public:
	FwInstract(BC_HAL_c *bc_hal_c);
	~FwInstract();
 
     /*执行某一点（笛卡尔）*/
     bool goto_Point();

     BC_HAL_c *m_bc_hal_c;
     FwControl *m_fw_Control;

private:
	

};

#include "FW_Instract.h"


#include "FreeRTOS.h"
#include "task.h"


FwInstract::FwInstract(BC_HAL_c *bc_hal_c)

{
	 m_bc_hal_c = bc_hal_c;
     static DogModel           dog_Model;	
     static FwControl          fw_Control(&dog_Model, bc_hal_c);	
	 this->m_fw_Control = &fw_Control;
}


FwInstract::~FwInstract()
{
	
}
   /*执行某一点（笛卡尔）*/
  bool FwInstract::goto_Point()
 {
	 
	vTaskDelay(1000); 
	//wang_start
	uint32_t start_tic = BSP_Systic();
	
	float execute_time = 10.0;
	float cur_time = 0.0f;
	/*初始化关节角度，必须以设定位置启动*/
    m_fw_Control->setInitJointTheta();
	/*刷新一次关节位置*/
	m_fw_Control->refCurJointTheta();
	
    /*计算单关节三次多项式轨迹规划参数（关节空间）单位（角度）*/
//    fw_Control.cal_Three_Plan(1,(dog_Model.theta1)/PI*180.0f, 20.0,   10.0);
//	fw_Control.cal_Three_Plan(2,(dog_Model.theta2)/PI*180.0f, -120.0, 10.0);
//	fw_Control.cal_Three_Plan(3,(dog_Model.theta3)/PI*180.0f, 100.0,  10.0);

	
	
    m_fw_Control->cal_Three_Cartesion_Trajectory(280,-80,0,execute_time);
	//wang_end
	while(!m_fw_Control->exceeded_Soft_Limit())
	{	 
		//bc_robotCtrl.RobotControllerTask();  //运行运动控制器
		
		//wang_add
		//bc_Hal.HAL_SetServoMotor(ServoMotor3,0,0.06*9.1,0,0,25);
		
	/*刷新当前关节角度*/
    m_fw_Control->refCurJointTheta();
		
	//计算时间
    cur_time = (BSP_Systic()-start_tic)/1000.0f;
		
	/*达到目标位置后不再修改*/
		//可通过计算执行时间，或者执行位置。考虑到超过时间后轨迹反转，必须使用时间设置
	if(cur_time<execute_time){
		//时间误差 500Hz 。输出位置误差（2毫秒执行位置）
	    	/*修改当前应该输出的角度*/
           m_fw_Control->modif_Joint_Theta(cur_time);
		
	       /*修改目标编码器的值*/
	       m_fw_Control->cal_T_Encoder();
		


	}
		//发送位置
	m_bc_hal_c->HAL_SetServoMotor(ServoMotor1,0,0,m_fw_Control->t_encoder[0], 0.1,3.0);	
	m_bc_hal_c->HAL_SetServoMotor(ServoMotor2,0,0,m_fw_Control->t_encoder[1], 0.1,3.0);		
	m_bc_hal_c->HAL_SetServoMotor(ServoMotor3,0,0,m_fw_Control->t_encoder[2], 0.2,3.0);	
//	else  //尝试减少电机噪音,不再以500HZ 频率调整,会带来更大的冲击和振动，位置维持失效。
//	{
//		for(int i = 0; i<3;i++){
//				if(abs(fw_Control.m_dog_model->t_theta[i] - fw_Control.m_dog_model->theta[i])>0.05)
//					{
//		
//			bc_Hal.HAL_SetServoMotor(ServoMotor1,0,0,fw_Control.t_encoder[0], 0.1,3.0);	
//			bc_Hal.HAL_SetServoMotor(ServoMotor2,0,0,fw_Control.t_encoder[1], 0.1,3.0);		
//			bc_Hal.HAL_SetServoMotor(ServoMotor3,0,0,fw_Control.t_encoder[2], 0.2,3.0);	
//					}
//	    }
//   }


		
		//连续性测试（初始编码器读取正确，时间计数正确）
	//bc_Hal.HAL_SetServoMotor(ServoMotor3,0,0,fw_Control.init_encoder3-cur_time/5000.0f, 0.2,3.0);	
	//bc_Hal.HAL_SetServoMotor(ServoMotor3,0,0,fw_Control.init_encoder3-cur_time/2.0f, 0.2,3.0);	
		
	vTaskDelay(2);  //500Hz 1000/500
    }
}