steeringwheelclass.java

采用PCI1020芯片控制三个电机

			}
		}
		return Start;
	}

	private static int GtoB(int lValue)// 已验证 正确
	{
		int lV = lValue;
		int lMax = 1;
		while (lV > 0) {
			lV >>= 1;
			lMax <<= 1;
		}
		if (lMax == 0)
			return -1;
		return g2b(0, --lMax, lValue);
	}

	protected static int ReadAngle() {
		// int[]pa_rr3=new int[16];
		// PA_rr3=rr3;
		ControlMotor.GetRR3Status(PA_rr3);
		/*
		 * return PA_rr3[0] + PA_rr3[1] * 2 + PA_rr3[2] * 4 + PA_rr3[3] * 8 +
		 * PA_rr3[4] * 16 + PA_rr3[5] * 32 + PA_rr3[6] * 64 + PA_rr3[7] 128 +
		 * PA_rr3[8] * 256 + PA_rr3[9] * 512;
		 */
		//寄存器值RR3加权，D0，D1，D2，D3，D8，D9，D10，D11，D12，D13  new080831
		return PA_rr3[0] + PA_rr3[1] * 2 + PA_rr3[2] * 4 + PA_rr3[3] * 8
				+ PA_rr3[8] * 16 + PA_rr3[9] * 32 + PA_rr3[10] * 64
				+ PA_rr3[11] * 128 + PA_rr3[12] * 256 + PA_rr3[13] * 512;
		/*
		 * PA_rr3[0] + PA_rr3[1] * 2 + PA_rr3[2] * 4 + PA_rr3[4] * 8 + PA_rr3[5] *
		 * 16 + PA_rr3[8] * 32 + PA_rr3[9] * 64 + PA_rr3[10] 128 + PA_rr3[12] *
		 * 256 + PA_rr3[13] * 512;
		 */
	}
/*	public static int ReadAngle() {
		// int[]pa_rr3=new int[16];
		// pa_rr3=rr3;
		// ControlMotor.GetRR3Status(pa_rr3);
		// return
		// int iLocation;
		// int gray=getGray();
		// iLocation = GtoB(GetGray());
		return GtoB(GetGray());// Gray码是角度？不是
	}*/

	// 角度 方向 紧急度 为加减速齿轮 旋转三周后角度减圈数
	/**
	 * Rotate函数重新编写 假设方向盘如果先逆时针转动一圈 编码盘停在270度 下一个命令时是顺时针60度， 
	 * 则方向盘应该先顺时针回到原点 再转60度，可能编码盘上1度 实际电机转2度 在程序中设置 
	 * 总之 方向盘先原路返回再转 同一侧的例外 
	 */
	//=========================================================================
	public static void RotateSteeringWheel(double angle) {
		int iDirection;
		if (angle < 0) {
			iDirection = motor.PDIRECTION;
			angle = -angle;
		}// 顺时针
		else
			iDirection = motor.MDIRECTION;
		// 通过当前速度判断紧急度 读增量光码盘算出

		// Rotate(angle, iDirection, urgency);// 算出后计算
	}
	//=========================================================================
	@SuppressWarnings( { "static-access", "static-access" })
	public static boolean Rotate(double angle, int direction, int urgency) {
	//public boolean Rotate(int angle, int direction) {//angle需要转动的角度，相对值，new080902
		boolean bFinished = false;
		double iCurrentAngle;//现在的角度，绝对值，new080902
	//	double iNextAngle;//下一个角度，绝对值，new080902
		int iPPS;
		// int iReferAngle = 0;// 参考点默认为0度 设置成静态成员
		// iCurrentAngle = (int) (ReadAngle() * 0.3515);// 读绝对光码盘转换成角度
		iCurrentAngle = ReadAngle() * 0.3515;//360/1024=0.3515625,new080902
	//-------------------------------------------------------------------------------------------
	//考虑绝对光码盘的圈数度，要做相应修改，new080916
		if (direction == motor.PDIRECTION) { // 正向认为电机顺时针旋转 负向认为电机逆时针旋转
			// angle为绝对角度 相对于0点 假设小于180度正值
			if (iCurrentAngle >= 180 + ReferAngle)
				iNextAngle = angle + 360 - iCurrentAngle;
			else if ((iCurrentAngle < 180 + ReferAngle)
					&& (iCurrentAngle >= angle)) {
				iNextAngle = iCurrentAngle - angle;
				direction = motor.MDIRECTION;
			} else
				iNextAngle = angle - iCurrentAngle;// 考虑绝对光码盘输出的角度
		} else {
			if (iCurrentAngle < 180 + ReferAngle)
				iNextAngle = iCurrentAngle + angle;
			else if ((iCurrentAngle > 180 + ReferAngle)
					&& (iCurrentAngle < 360 - angle)) {
				iNextAngle = 360 - iCurrentAngle - angle;
				direction = motor.PDIRECTION;
			} else
				iNextAngle = angle - 360 + iCurrentAngle;
		}
	//------------------------------------------------------------------------------------------
		iPPS = (int) Math.round(iNextAngle * MULTIPLE * STEP10_OF_ANGLE);// 计算发出脉冲数
		SetLCDL(iPPS, direction, urgency);
	//  this.RunLVDV(motor.XAXIS, ADataList, ALCData);驱动A轴
		ControlMotor.RunLVDV(motor.XAXIS, ADataList, ALCData);// 静态方法调用
		
		ControlMotor.GetRR0Status(PA_rr0);
		while (PA_rr0[0] != 0);// X轴未停止等待
		bFinished = true;

		return bFinished;// 放在主程序中？？不用
	}

//new080906
	public static boolean Rotate1(double angle, int direction, int urgency) {
		//public boolean Rotate(int angle, int direction) {//angle需要转动的角度，相对值，new080902
		angle0=angle;//new080907
		boolean bFinished = false;
		double iCurrentAngle;//现在的角度，绝对值，new080902
		double iNextAngle;//下一个角度，绝对值，new080902
		int iPPS;
		// int iReferAngle = 0;// 参考点默认为0度 设置成静态成员
		// iCurrentAngle = (int) (ReadAngle() * 0.3515);// 读绝对光码盘转换成角度
		iCurrentAngle = ReadAngle() * 0.3515;//360/1024=0.3515625,new080902
	//-------------------------------------------------------------------------------------------
	//考虑绝对光码盘的圈数度，要做相应修改，new080916
		if (direction == motor.PDIRECTION) { // 正向认为电机顺时针旋转 负向认为电机逆时针旋转
			// angle为绝对角度 相对于0点 假设小于180度正值
			if (iCurrentAngle >= 180 + ReferAngle)
				iNextAngle = angle + 360 - iCurrentAngle;
			else if ((iCurrentAngle < 180 + ReferAngle)
					&& (iCurrentAngle >= angle)) {
				iNextAngle = iCurrentAngle - angle;
				direction = motor.MDIRECTION;
			} 
			else
				iNextAngle = angle - iCurrentAngle;// 考虑绝对光码盘输出的角度
		} 
		else {
			if (iCurrentAngle < 180 + ReferAngle)
				iNextAngle = iCurrentAngle + angle;
			else if ((iCurrentAngle > 180 + ReferAngle)
					&& (iCurrentAngle < 360 - angle)) {
				iNextAngle = 360 - iCurrentAngle - angle;
				direction = motor.PDIRECTION;
		    } 
			else
				iNextAngle = angle - 360 + iCurrentAngle;
		}
	//-------------------------------------------------------------------------------
		//判断紧急度，new080916
	//==============================================================================	
		if(iNextAngle>node[2]&speed<node_v[0])
			urgency=3;
		else if(iNextAngle<node[2]&iNextAngle>node[1])
			urgency=2;
		else
			urgency=1;
		
	//==============================================================================
	//无用
	//===============================================================================
	/*
		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 = drivecv_1+diskp+ diski+ diskd;//
			dif_2 = dif_1;
			dif_1 = dif;
			drivecv_1 = drivecv;
			
			if (drivecv>max_drv)	
				drivecv = max_drv;			
		}
		*/
	//===============================================================================
		iPPS = (int) Math.round(iNextAngle * MULTIPLE * STEP10_OF_ANGLE);// 计算发出脉冲数
		SetLCDL1(iPPS, direction, urgency);
		//  this.RunLVDV(motor.XAXIS, ADataList, ALCData);驱动A轴
		ControlMotor.RunLVDV(motor.XAXIS, ADataList, ALCData);// 静态方法调用
			
	/*	ControlMotor.GetRR0Status(PA_rr0);
		while (PA_rr0[0] != 0);// X轴未停止等待
		bFinished = true;
	*/

		return bFinished;// 放在主程序中？？不用
		}

//new080907
	public static boolean Rotate2(double angle, int direction, int urgency) {
		//public boolean Rotate(int angle, int direction) {//angle需要转动的角度，相对值，new080902
		angle0=angle;//new080907
		boolean bFinished = false;
		double iCurrentAngle;//现在的角度，绝对值，new080902
		double iNextAngle;//下一个角度，绝对值，new080902
		int iPPS;
		// int iReferAngle = 0;// 参考点默认为0度 设置成静态成员
		// iCurrentAngle = (int) (ReadAngle() * 0.3515);// 读绝对光码盘转换成角度
		iCurrentAngle = ReadAngle() * 0.3515;//360/1024=0.3515625,new080902
	//--------------------------------------------------------------------------------
		if (direction == motor.PDIRECTION) { // 正向认为电机顺时针旋转 负向认为电机逆时针旋转
			// angle为绝对角度 相对于0点 假设小于180度正值
			if (iCurrentAngle >= 180 + ReferAngle)
				iNextAngle = angle + 360 - iCurrentAngle;
			else if ((iCurrentAngle < 180 + ReferAngle)
					&& (iCurrentAngle >= angle)) {
				iNextAngle = iCurrentAngle - angle;
				direction = motor.MDIRECTION;
			} 
			else
				iNextAngle = angle - iCurrentAngle;// 考虑绝对光码盘输出的角度
		} 
		else {
			if (iCurrentAngle < 180 + ReferAngle)
				iNextAngle = iCurrentAngle + angle;
			else if ((iCurrentAngle > 180 + ReferAngle)
					&& (iCurrentAngle < 360 - angle)) {
				iNextAngle = 360 - iCurrentAngle - angle;
				direction = motor.PDIRECTION;
		    } 
			else
				iNextAngle = angle - 360 + iCurrentAngle;
		}
	//----------------------------------------------------------------------------------
		iPPS = (int) Math.round(iNextAngle * MULTIPLE * STEP10_OF_ANGLE);// 计算发出脉冲数
		SetLCDL2(iPPS, direction, urgency);
		//  this.RunLVDV(motor.XAXIS, ADataList, ALCData);驱动A轴
		ControlMotor.RunLVDV(motor.XAXIS, ADataList, ALCData);// 静态方法调用
	
	/*	
		ControlMotor.GetRR0Status(PA_rr0);
		while (PA_rr0[0] != 0);// X轴未停止等待
		bFinished = true;
	*/

		return bFinished;// 放在主程序中？？不用
		}
	
	// 返回参考点 ，初始化时使用
	public static boolean ReturnRefAngle(int r) {// 考虑方向盘左右旋转 程序相应要改
		boolean bFinished = false; //
		int iCurrentAngle = ReadAngle();// 当前角度 光码盘读数int
	    ReferAngle=r;
		// iCurrentAngle = (int) Math.rint(ReadAngle() * 0.3515);
		int iPPS;// 发给电机脉冲数
		int iDirection;
		if (ReferAngle <= 512) {
			if ((iCurrentAngle>ReferAngle)&&(iCurrentAngle<ReferAngle + 512)){
//?????		if(512<iCurrentAngle<ReferAngle+512) PDIRECTION;new080902
				iDirection = motor.MDIRECTION;
				iPPS = (int) Math.round((iCurrentAngle - ReferAngle) * MULTIPLE
						* STEP10_OF_ANGLE);
			} else {
				iDirection = motor.PDIRECTION;
				if ((iCurrentAngle <= 1023)
						&& (iCurrentAngle > ReferAngle + 512))
					iPPS = (int) Math.round((1024 - iCurrentAngle + ReferAngle)
							* MULTIPLE * STEP10_OF_ANGLE);
				else
					iPPS = (int) Math.round(ReferAngle - iCurrentAngle);
			}// ReferAngle在0-512之内
		} else {
			if ((iCurrentAngle < ReferAngle)&&(iCurrentAngle>ReferAngle - 512)){
//?????     if(ReferAngle-512<iCurrentAngle<512) MDIRECTION;new080902
				iDirection = motor.PDIRECTION;
				iPPS = (int) Math.round((ReferAngle - iCurrentAngle) * MULTIPLE
						* STEP10_OF_ANGLE);
			} else {
				iDirection = motor.MDIRECTION;
				if (iCurrentAngle > ReferAngle)
					iPPS = (int) Math.round((iCurrentAngle - ReferAngle)
							* MULTIPLE * STEP10_OF_ANGLE);
				else
					iPPS = (int) Math.round((iCurrentAngle + 1024 - ReferAngle)
							* MULTIPLE * STEP10_OF_ANGLE);
			}
		}// ReferAngle>512
		
		SetLCDL(iPPS, iDirection, 1);
		ControlMotor.RunLVDV(motor.XAXIS, ADataList, ALCData);
		
		ControlMotor.GetRR0Status(PA_rr0);
		while (PA_rr0[0] != 0);// 等待X轴停止
		
		bFinished = true;
		return bFinished;
	}
}