dynmodel.java

赛车程序：先对赛道和赛车建立模型

/**
 * 
 */
package cn.tsinghua.ag5.CarModel;

//import java.math.*;

/**
 * @author wfq
 *
 */
public class DynModel {

	/**
	 * 
	 */
	public DynModel( int p_carNumber) 
	{
		// TODO Auto-generated constructor stub
		p_carNumber %= s_carNum + 1;
		m_locationX = (s_carNum - p_carNumber ) * CtrlStrategy.s_span;
//		switch (p_carNumber)
//		{
//		case 1:
//			m_locationX = CtrlStrategy.s_span;
//			break;
//		case 2:
//			m_locationX = 0;
//			break;
//		default: 
//			System.out.println("Error car number!\n");
//		}
		
		m_locationY = 0;
		m_theta = 0;
		m_vX = 0;
		m_vY = 0;
		m_omiga = 0;
		
		m_delta = 0;
		m_faif = 0;
		m_fair = 0;
	}
	
	public static final short s_carNum = 2;	//小车的数目
	
	private double m_locationX;
	private double m_locationY;
	private double m_theta;	//车辆方向角,弧度	-pi -> pi, 与x轴正方向的夹角
	
	private double m_vX;
	private double m_vY;
	private double m_omiga;			//角速度
	
	
	private double m_delta;	//方向盘控制角度	-0.2rad -> 0.2rad 向左打方向盘是负值，向右打方向盘是正值
	private double m_faif;	//前轮纵向驱动力	-8000N -> 4000N
	
	private static final float s_cf = 50000f;
	private static final float s_cr = 64000;
	private static final float s_cd = 0.0005f;
	private static final float s_miu = 1;
	//private final float s_Fjk = 0;
	private static final float s_m = 1500;	//1500kg
	private static final float s_I = 2500;	//2500kg.m^2
	private static final float s_lf = 1.25f; //m
	private static final float s_lr = 1.5f;
	private static final float s_h = 0.5f;	//m
	private static final float s_g = 9.8f; //N/kg
	private static final float s_kb = 0.34f;
	
	private static final float s_spdL = 3.6f;	//最低速度限制
	public static final int s_uLimit = 4000;  //驱动力的上限
	public static final int s_dLimit = -8000; //制动力的下限
	public static final float s_steer = 0.2f;	//方向盘转动的左右限
	
	//中间变量
	private double m_fair;	//后轮纵向驱动力
	
	private double m_Fzf;
	private double m_Fzr;
	private double m_alfa2f;
	private double m_alfa2r;
	private double m_alfaf;
	private double m_alfar;
	private double m_Fksif;
	private double m_Fksir;
	
	/**
	 * 
	 */
	public void update()
	{
		//此处顺序一定不能乱
		//应该先更新小车状态，再更新控制量
//		double m_fair = m_fair;
//		double m_alfaf = m_alfaf;
//		double m_alfar = m_alfar;
//		//中间变量，应该用当前计算出的新值
////		double m_Fzf = m_Fzf;
////		double m_Fzr = m_Fzr;
//		double m_alfa2f = m_alfa2f;
//		double m_alfa2r = m_alfa2r;
//		double m_Fksif = m_Fksif;
//		double m_Fksir = m_Fksir;
		
		double t_vX = m_vX;
		double t_vY = m_vY;
		double t_theta = m_theta;
		double t_omiga = m_omiga;
		
		m_fair =  m_faif > 0 ? 0 : s_kb * m_faif;
		
		//速度为0时，程序无法计算下列公式
		if(Math.abs(t_vY) < s_spdL)	t_vY = s_spdL * (t_vY >= 0 ? 1 : -1);
//		if(Math.abs(t_vX) < s_spdL)	t_vX = s_spdL * Math.signum(t_vX);
		m_alfaf = m_delta - (s_lf * t_omiga + t_vX) / t_vY;
		m_alfar = (s_lr * t_omiga - t_vX) / t_vY;
		
		m_Fzf = (s_m * s_g * s_lr - (m_faif + m_fair) * s_h) / (s_lf + s_lr);
		m_Fzr = (s_m * s_g * s_lf + (m_faif + m_fair) * s_h) / (s_lf + s_lr);
		
		//力为0时，也会坏死
		if(Math.abs(m_Fzf) < 10)	System.out.println("Danger: closing 0!!\n");
		m_alfa2f = s_cf * m_alfaf / (s_miu * m_Fzf);
		m_alfa2r = s_cr * m_alfar / (s_miu * m_Fzr);
		
		m_Fksif = (s_miu * m_Fzf * (m_alfa2f - Math.signum(m_delta) * 
				Math.pow(m_alfa2f, 2) / 3 + Math.pow(m_alfa2f, 3) / 27) * 
				Math.sqrt(1 - Math.pow(m_faif/(s_miu * m_Fzf), 2) + 
						Math.pow(m_faif / s_cf, 2)));

		m_Fksir = (s_miu * m_Fzr * (m_alfa2r - Math.signum(m_delta) * 
				Math.pow(m_alfa2r, 2) / 3 + Math.pow(m_alfa2r, 3) / 27) * 
				Math.sqrt(1 - Math.pow(m_fair/(s_miu * m_Fzr), 2) + 
						Math.pow(m_fair / s_cr, 2)));
		
		//此处对模型坐标的基准进行修正
//		m_locationX += MyGUI.s_emStep * (t_vX * Math.cos(t_theta) + t_vY * 
//				Math.sin(t_theta));
//		m_locationY += MyGUI.s_emStep * (- t_vX * Math.sin(t_theta) + t_vY * 
//				Math.cos(t_theta));
		m_locationX += MyGUI.s_emStep * (- t_vX * Math.sin(t_theta) + t_vY * 
				Math.cos(t_theta));
		m_locationY += MyGUI.s_emStep * (t_vX * Math.cos(t_theta) + t_vY * 
				Math.sin(t_theta));
		
		m_vX += MyGUI.s_emStep * (( m_faif * m_delta + m_Fksif + m_Fksir) / 
				s_m - t_vY * t_omiga - Math.signum(t_vX) * s_cd * Math.pow(t_vX, 2));
		m_vY += MyGUI.s_emStep * ((m_faif + m_fair - m_Fksif * m_delta) / 
				s_m + t_vX * t_omiga - Math.signum(t_vY) * s_cd * Math.pow(t_vY, 2));
		
		m_theta += MyGUI.s_emStep * t_omiga;
		m_omiga += MyGUI.s_emStep * ((s_lf * m_faif * m_delta + s_lf * m_Fksif - 
				s_lr * m_Fksir) / s_I);
		
	}

	/**
	 * @return the delta
	 */
	public double getDelta() {
		return this.m_delta;
	}

	/**
	 * 方向盘控制角度	-0.2rad -> 0.2rad	左负右正
	 * @param p_delta the delta to set
	 */
	public void setDelta(double p_delta) {
		if(p_delta > s_steer)	p_delta = s_steer;
		if(p_delta < - s_steer)	p_delta = -s_steer;
		this.m_delta = p_delta;
	}

	/**
	 * @return the faif
	 */
	public double getFaif() {
		return this.m_faif;
	}

	/**
	 * 驱动和制动力范围：-8000N -> 4000N
	 * @param p_faif the faif to set
	 */
	public void setFaif(double p_faif) {
		if(p_faif > s_uLimit )	p_faif = s_uLimit;
		if(p_faif < s_dLimit)	p_faif = s_dLimit;
		this.m_faif = p_faif;
	}

	/**
	 * @return the locationX
	 */
	public double getLocationX() {
		return this.m_locationX;
	}

	/**
	 * @return the locationY
	 */
	public double getLocationY() {
		return this.m_locationY;
	}
	
	public void terminate()
	{
		m_vX = 0;
		m_vY = 0;
		m_omiga = 0;
		
		m_delta = 0;
		m_faif = 0;
		m_fair = 0;
		
		//MyGUI.s_isRoadCreated = false;
	}
	
	public void reset()
	{
		m_vX = 0;
		m_vY = 0;
		m_omiga = 0;
		
		m_delta = 0;
		m_faif = 0;

		m_locationX = 0;
		m_locationY = 0;
		
	}

	/**
	 * @return the vX
	 */
	public double getVX() {
		return this.m_vX;
	}

	/**
	 * @return the vY
	 */
	public double getVY() {
		return this.m_vY;
	}

	/**
	 * @return the theta
	 */
	public double getTheta() {
		return this.m_theta;
	}

}