sm_pc.c

基于mcx314的运动控制

							else if(RegNo==30)
								{	
								
								// read RR0
									RegNr = 0;
									Reg_RR = Read_FPGA(RegNr);	
									SerialPrintf( "$%1d%02d#%d*",0,RegNr,Reg_RR);

								// read Param(X)
								for(RegNr=0;RegNr<4;RegNr++)
									{
									Write_FPGA(0,256+16+RegNr);
									Reg_RR = Read_FPGA(7);	
									Reg_RR <<= 16;	
									Reg_RR += Read_FPGA(6);	
									SerialPrintf( "$%1d%02d#%d*",0,RegNr+20,Reg_RR);
									}
								}
							else if(RegNo==31)
								{	
								
								// read RR0
									RegNr = 0;
									Reg_RR = Read_FPGA(RegNr);	
									SerialPrintf( "$%1d%02d#%d*",0,RegNr,Reg_RR);

								// read Param(Y)
								for(RegNr=0;RegNr<4;RegNr++)
									{
									Write_FPGA(0,512+16+RegNr);
									Reg_RR = Read_FPGA(7);	
									Reg_RR <<= 16;	
									Reg_RR += Read_FPGA(6);	
									SerialPrintf( "$%1d%02d#%d*",1,RegNr+20,Reg_RR);
									}
								}
							else if(RegNo==32)
								{	
								
								// read RR0
									RegNr = 0;
									Reg_RR = Read_FPGA(RegNr);	
									SerialPrintf( "$%1d%02d#%d*",0,RegNr,Reg_RR);

								// read Param(Z)
								for(RegNr=0;RegNr<4;RegNr++)
									{
									Write_FPGA(0,1024+16+RegNr);
									Reg_RR = Read_FPGA(7);	
									Reg_RR <<= 16;	
									Reg_RR += Read_FPGA(6);	
									SerialPrintf( "$%1d%02d#%d*",2,RegNr+20,Reg_RR);
									}
								}
							else if(RegNo==33)
								{	
								
								// read RR0
									RegNr = 0;
									Reg_RR = Read_FPGA(RegNr);	
									SerialPrintf( "$%1d%02d#%d*",0,RegNr,Reg_RR);

								// read Param(U)
								for(RegNr=0;RegNr<4;RegNr++)
									{
									Write_FPGA(0,2048+16+RegNr);
									Reg_RR = Read_FPGA(7);	
									Reg_RR <<= 16;	
									Reg_RR += Read_FPGA(6);	
									SerialPrintf( "$%1d%02d#%d*",3,RegNr+20,Reg_RR);
									}
								}
							else if(RegNo==34)		// For IPL
								{	
								
								// read RR0
									RegNr = 0;
									Reg_RR = Read_FPGA(RegNr);	
									SerialPrintf( "$%1d%02d#%d*",0,RegNr,Reg_RR);

								// read RR1
									RegNr = 1;
									Reg_RR = Read_FPGA(RegNr);	
									SerialPrintf( "$%1d%02d#%d*",0,RegNr,Reg_RR);
								
								// read LP param
									RegNr = 0;

									// LP(X)
									Write_FPGA(0,256+16+RegNr);
									Reg_RR = Read_FPGA(7);	
									Reg_RR <<= 16;	
									Reg_RR += Read_FPGA(6);	
									SerialPrintf( "$%1d%02d#%d*",0,RegNr+20,Reg_RR);

									// LP(Y)
									Write_FPGA(0,512+16+RegNr);
									Reg_RR = Read_FPGA(7);	
									Reg_RR <<= 16;	
									Reg_RR += Read_FPGA(6);	
									SerialPrintf( "$%1d%02d#%d*",1,RegNr+20,Reg_RR);

									// LP(Z)
									Write_FPGA(0,1024+16+RegNr);
									Reg_RR = Read_FPGA(7);	
									Reg_RR <<= 16;	
									Reg_RR += Read_FPGA(6);	
									SerialPrintf( "$%1d%02d#%d*",2,RegNr+20,Reg_RR);

									// LP(U)
									Write_FPGA(0,2048+16+RegNr);
									Reg_RR = Read_FPGA(7);	
									Reg_RR <<= 16;	
									Reg_RR += Read_FPGA(6);	
									SerialPrintf( "$%1d%02d#%d*",3,RegNr+20,Reg_RR);
								}
								
							else if(RegNo==35)		// For All
								{	
								// read RR0~5(X)
								Write_FPGA(0,0x010f);
								for(RegNr=0;RegNr<6;RegNr++)
									{
									//Reg_RR = Read_FPGA(RegNr);	
									Reg_RR = Read_FPGA(RegNr);	
									SerialPrintf( "$%1d%02d#%d*",0,RegNr,Reg_RR);
									}
								// read WR3~5(X)
								for(RegNr=13;RegNr<16;RegNr++)
									{
									Reg_RR = Read_FPGA(RegNr);	
									SerialPrintf( "$%1d%02d#%d*",0,RegNr,Reg_RR);
									}
								
								// read RR1~3(Y)
								Write_FPGA(0,0x020f);
								for(RegNr=1;RegNr<4;RegNr++)
									{
									Reg_RR = Read_FPGA(RegNr);	
									SerialPrintf( "$%1d%02d#%d*",1,RegNr,Reg_RR);
									}
								// read WR3~3(Y)
								for(RegNr=13;RegNr<14;RegNr++)
									{
									Reg_RR = Read_FPGA(RegNr);	
									SerialPrintf( "$%1d%02d#%d*",1,RegNr,Reg_RR);
									}
								
								// read RR1~3(Z)
								Write_FPGA(0,0x040f);
								for(RegNr=1;RegNr<4;RegNr++)
									{
									Reg_RR = Read_FPGA(RegNr);	
									SerialPrintf( "$%1d%02d#%d*",2,RegNr,Reg_RR);
									}
								// read WR3~3(Z)
								for(RegNr=13;RegNr<14;RegNr++)
									{
									Reg_RR = Read_FPGA(RegNr);	
									SerialPrintf( "$%1d%02d#%d*",2,RegNr,Reg_RR);
									}
								
								// read RR1~3(U)
								Write_FPGA(0,0x080f);
								for(RegNr=1;RegNr<4;RegNr++)
									{
									Reg_RR = Read_FPGA(RegNr);	
									SerialPrintf( "$%1d%02d#%d*",3,RegNr,Reg_RR);
									}
								// read WR3~3(U)
								for(RegNr=13;RegNr<14;RegNr++)
									{
									Reg_RR = Read_FPGA(RegNr);	
									SerialPrintf( "$%1d%02d#%d*",3,RegNr,Reg_RR);
									}

								// read Param(X)
								for(RegNr=0;RegNr<4;RegNr++)
									{
									Write_FPGA(0,256+16+RegNr);
									Reg_RR = Read_FPGA(7);	
									Reg_RR <<= 16;	
									Reg_RR += Read_FPGA(6);	
									SerialPrintf( "$%1d%02d#%d*",0,RegNr+20,Reg_RR);
									}

								// read Param(Y)
								for(RegNr=0;RegNr<4;RegNr++)
									{
									Write_FPGA(0,512+16+RegNr);
									Reg_RR = Read_FPGA(7);	
									Reg_RR <<= 16;	
									Reg_RR += Read_FPGA(6);	
									SerialPrintf( "$%1d%02d#%d*",1,RegNr+20,Reg_RR);
									}

								// read Param(Z)
								for(RegNr=0;RegNr<4;RegNr++)
									{
									Write_FPGA(0,1024+16+RegNr);
									Reg_RR = Read_FPGA(7);	
									Reg_RR <<= 16;	
									Reg_RR += Read_FPGA(6);	
									SerialPrintf( "$%1d%02d#%d*",2,RegNr+20,Reg_RR);
									}

								// read Param(U)
								for(RegNr=0;RegNr<4;RegNr++)
									{
									Write_FPGA(0,2048+16+RegNr);
									Reg_RR = Read_FPGA(7);	
									Reg_RR <<= 16;	
									Reg_RR += Read_FPGA(6);	
									SerialPrintf( "$%1d%02d#%d*",3,RegNr+20,Reg_RR);
									}
								}	
							else		// For each WR/RR register 
								{
								Reg_RR = Read_FPGA(RegNo);	
								if(RegNo>0 && RegNo<4)			// RR1~3
								    AxisNr = AxisNo;
								else if(RegNo>10 && RegNo<14)	// WR1~3
								    AxisNr = AxisNo;
								else
									AxisNr = 0;     	
								SerialPrintf( "$%1d%02d#%d*",AxisNr,RegNo,Reg_RR);
								}
							}
						
						else if( set )
							{
							if(RegNo == 4 && Data == 1)
								for(nTime = 0; nTime < 960; nTime++)
									SerialPrintf( "$%d#%d*",nTime,nTime*nTime % 65536 );
							}
								
						start = 0;
						}
						break;
				default:
						if(start)
						{
						*pStr++= c;
						*pStr= '\0';
    					//SerialPrintf( "others=%s\n",Str ) ;
						}
						break;
			}			
		}
			
     SerialPrintf( " Monitor Ending... \n");
	 return;
}