steeringwheelclass.java

采用PCI1020芯片控制三个电机

package cn.com.yuzhiqiang.pcimotor;

public class SteeringWheelClass extends ControlMotor {

	// public static final int XAXIS = 0;

	// public static final int YAXIS = 1;

	// public static final int ZAXIS = 2;

	// public static final int UAXIS = 3;

	// public static final int ALLAXIS = 4;

	// public static final int DV = 0; // 定长驱动

	// public static final int LV = 1; // 连续驱动

	// public static final int CWCCW = 0; // CW/CCW方式

	// public static final int CPDIR = 1; // CP/DIR方式

	// public static final int MDIRECTION = 0;// 反方向

	// public static final int PDIRECTION = 1;// 正方向

	// public static final int LINE = 0; // 直线运动

	// public static final int CURVE = 1; // S曲线运动

	// public static final int LOGIC = 0; // 逻辑计数器

	// public static final int FACT = 1; // 实位计数器

	// public static final int SUDENSTOP = 0; // 立即停止

	// public static final int DECSTOP = 1; // 减速停止

	// public static final int AUTO = 0; // 自动减速

	// public static final int HAND = 1; // 手动减速

	public static final float STEP10_OF_ANGLE = (float)5.55556; //22.2;// 40细分

	static int ReferAngle = 0;// 默认为0 现场改 //参考原点

	private static final int MULTIPLE = 3;// 光码盘角度和实际转动角度的比例减速齿轮3:1

	// 5.5555556;//
	// 每度对应的脉冲数
	// 10细分时

	// public static final int STEP10_OF_ANGLE =;
	// 在后面的计算中除去取整

	static int[] PA_rr3 = motor.PA_RR3;// 也可以都在主程序中定义 其他类中用引用

	static int[] PA_rr0 = motor.PA_RR0;//
	
	

	// ControlMotor func_ControlMotor;

	/*
	 * 公共参数数组成员 
	 * LONG Multiple; 倍率 (1~500) 
	 * LONG StartSpeed; 初始速度(1~8000) 
	 * LONG DriveSpeed; 驱动速度(1~8000) 
	 * LONG Acceleration; 加速度(125~1000000) 
	 * LONG Deceleration; 减速度(125~1000000) 
	 * LONG AccIncRate; 加速度变化率(954~62500000) 
	 * LONG DecIncRate; 减速度变化率(954~62500000)	 * 
	 * 
	 * 直线和S曲线参数数组成员 
	 * LONG AxisNum; 轴号 (X轴 | Y轴 | X、Y轴) 
	 * LONG LV_DV; 驱动方式 (连续 | 定长 )
	 * LONG DecMode; 减速方式 (自动减速 | 手动减速) 
	 * LONG PulseMode; 脉冲方式 (CW/CCW方式 | CP/DIR方式) 
	 * LONG Line_Curve; 运动方式 (直线 | 曲线) 
	 * LONG Direction; 运动方向 (正方向 | 反方向) 
	 * LONG nPulseNum; 定量输出脉冲数(0~268435455)
	 */
	// 每个继承类私有或保护
	private static int[] ADataList = new int[7];// 公共参数数组
	private static int[] ALCData = new int[7]; // 直线和S曲线参数数组
	
	static double angle0;//new080907
	static int readangle;
	static double dif,dif_1=0, dif_2=0;//new080906
	static int []node={10,20,30};//参数及经验，node[x]需要测试后设值	
	static int []node_v={10,30};//车速node，
	static int drivecv,drivecv_1;
	static int accdrvcv,accdrvcv_1;
	static double iNextAngle;
	static double speed;
	
	// 赋值函数
	private static void SetLCDL(int p, int dir, int urg) {// 紧急度urg保留
//  private void SetLCDL(int p, int dir) {
		if(urg<=1)urg=1;//new080901
		if(urg>=3)urg=3;
		// int urg = 1; // 默认为1
		ADataList[0] = 1 * urg;
		ADataList[1] = 1;
		ADataList[2] = 500;
		ADataList[3] = 1000;
		ADataList[4] = 1000;
		ADataList[5] = 1000;//
		ADataList[6] = 1000;//

		ALCData[0] = motor.XAXIS;
		ALCData[1] = motor.DV;
		ALCData[2] = motor.AUTO;
		ALCData[3] = motor.CPDIR;
		ALCData[4] = motor.LINE;
		ALCData[5] = dir;
		ALCData[6] = p;
	}	
	
//new0809016
	private static void SetLCDL1(int p, int dir, int urg) {// 紧急度urg保留
		int startcv=ControlMotor.ReadCV(motor.XAXIS);	//加减速度没有变化，效果不会明显
		//startcv是理论速度值，要和绝对光码盘返回计算得出的速度进行比较，或者说做一个加速度的是测试	
	//	===============================================================================
		
		int max_drv=8000;//和SetM()有关
		double kp=0;//试验及经验
		double ki=0,kd=0;//试验及经验
		double diskp,diski,diskd;
		
		
		dif=iNextAngle;		
		if(dif>node[2])
			drivecv=max_drv;
		else{
			if(dif<=node[0]){	//分段PID参数设置
				kp=15;
				ki=1;
				kd=0.1;
			}
			if( (dif>node[0]) && (dif<=node[1]) ){			
				kp=10;
				ki=0.5;
				kd=0.1;
			}
			if( (dif>node[1]) &&(dif<=node[2]) ){			
				kp=20;
				ki=0;
				kd=0.1;
			}	
			//PID控制算法
			diskp = kp * (dif-dif_1);
			diski = (ki * dif);
			diskd = (kd * (dif_2-2 * dif_1 + dif));
			drivecv = (int)(drivecv_1+diskp+ diski+ diskd);//
			dif_2 = dif_1;
			dif_1 = dif;
			drivecv_1 = drivecv;
			
			if (drivecv>max_drv)	
				drivecv = max_drv;			
		}	
			
		//===============================================================================	
		if(urg<=1)urg=1;//new080901
		if(urg>=3)urg=3;
		// int urg = 1; // 默认为1
	//	ADataList[0] = 1 * urg;
		ADataList[0] = 1 ;//new080916
		ADataList[1] = 1;//初始速度待考虑
		ADataList[2] = drivecv;
		ADataList[3] = 500*urg;//加减速度基值需要测试，new080916
		ADataList[4] = 500*urg;			
		ADataList[5] = 1000*urg;//
		ADataList[6] = 1000*urg;

		ALCData[0] = motor.XAXIS;
		ALCData[1] = motor.DV;
		ALCData[2] = motor.AUTO;
		ALCData[3] = motor.CWCCW;
		ALCData[4] = motor.LINE;
		ALCData[5] = dir;
		ALCData[6] = p;			
	}
	
//	new080907，似乎没用
	private static void SetLCDL2(int p, int dir, int urg) {// 紧急度urg保留
		int startcv=ControlMotor.ReadCV(motor.XAXIS);	//加减速度没有变化，效果不会明显
		//startcv是理论速度值，要和绝对光码盘返回计算得出的速度进行比较，或者说做一个加速度的是测试
		
	//---------------------------------------------------------------------	
	/*	int drivecv_1;
		int max_drv=8000;//和SetM()有关
		int max_accdrv=100;//需要试验
		double kp=0;//试验及经验
		double ki=0,kd=0;//试验及经验
		double accdrvkp,accdrvki,accdrvkd;
		
		drivecv_1=startcv;//drivecv_1上一次的目标值,startcv这一次的理论值		
		dif=max_drv-startcv;		
		if(dif>node[2])
			accdrvcv=max_accdrv;
		else{
			if(dif<=node[0]){	//分段PID参数设置
				kp=15;
				ki=1;
				kd=0.1;
			}
			if( (dif>node[0]) && (dif<=node[1]) ){			
				kp=10;
				ki=0.5;
				kd=0.1;
			}
			if( (dif>node[1]) &&(dif<=node[2]) ){			
				kp=20;
				ki=0;
				kd=0.1;
			}	
			//PID控制算法
			accdrvkp = kp * (dif-dif_1);
			accdrvki = (int)(ki * dif);
			accdrvkd = (int)(kd * (dif_2-2 * dif_1 + dif));
			accdrvcv = (int)(accdrvcv_1+accdrvkp+ accdrvki+ accdrvkd);//
			dif_2 = dif_1;
			dif_1 = dif;
			accdrvcv_1 = accdrvcv;
			
			if (accdrvcv>8000)	
				accdrvcv = 8000;			
		}
	*/	
	//---------------------------------------------------------------------	
		if(urg<=1)urg=1;//new080901
		if(urg>=3)urg=3;
		// int urg = 1; // 默认为1
		ADataList[0] = 1 * urg;
		ADataList[1] = startcv;
		ADataList[2] = drivecv;
		ADataList[3] = 500;
		ADataList[4] = 500;			
		ADataList[5] = 1000;
		ADataList[6] = 1000;

		ALCData[0] = motor.XAXIS;
		ALCData[1] = motor.LV;
		ALCData[2] = motor.AUTO;
		ALCData[3] = motor.CWCCW;
		ALCData[4] = motor.LINE;
		ALCData[5] = dir;
		ALCData[6] = p;			
	}
	
	// 设置参考原点函数
	protected static void SetReferAngle(int refangle) {
		ReferAngle = refangle;
	}

	// 格雷码转化为十进制数 用于绝对光码盘
	/*
	 * public static int graytodecimal(int x) { 
	 * 		int y = x; 
	 * 		while((x>>=1)!=0) 
	 * 		y^=x; 
	 * 		return y; 
	 * }
	 * 
	 * public static int ReadGax() { 
	 * // DatG=0; 
	 * // aa=0; 
	 * // bb=0; int DatT=0; 
	 * // BMQclick=1; 
	 * // delay_us(1); 
	 * // ControlMotor.GetRR3Status(PA_rr3); 
	 * 
	 *	  for(i=0;i<25;i++) { 
	 * 		BMQclick=0; 
	 * 		delay_us(2); 
	 * 		BMQclick=1; 
	 * 		BMQclick=1;
	 * 		BDat[i]=BMQpos; 
	 *    }
	 *
	 * 	  for(int i=0;i<10;i++) { DatT=DatT<<1; DatT=DatT | PA_rr3[i]; } 
	 *    return graytodecimal(DatT); 
	 *  
	 *    loop=DatD/8192; 
	 *    angle =DatD % 8192;
	 *    if(DatTemp!=DatD) { 
	 * 		Show10(loop); 
	 * 		Show10(angle); 
	 * 		Send0(0x20);
	 * 		Send0(0x20); 
	 * 		Send0(0x20); 
	 * 		DatTemp=DatD; 
	 *    } 
	 * }
	 */

	private static int g2b(int lStart, int lEnd, int lValue) {
		int Start = lStart;
		int End = lEnd;
		int Counter = 0;
		int Temp = 0;
		boolean Type = true;
		while (Start < End) {
			Temp = Counter + ((End - Start + 1) >> 1);
			if (Type ^ (lValue < Temp)) {
				if (Type)
					Counter = Temp;
				Start = (Start + End + 1) >> 1;
				Type = false;
			} else {
				if (!Type)
					Counter = Temp;
				End = (Start + End - 1) >> 1;
				Type = true;