chuguomain080105.c

通过迷你开发板m9020-fnu201实现对伺服电机的控制。运用力矩的方式对伺服电机控制而且运用力反馈的方式对伺服电机精确控制

	        gpio_clr(GPIOA0);
			gpio_clr(GPIOA1);
			gpio_clr(GPIOA2);
			can.ctl = 0;	 
 
	   	}		
		if(can.ctl == 2)
		{
			ch1_outdata[0]=U0;
			ch2_outdata[0]=U0;
			ch3_outdata[0]=U0; 
			gpio_set(GPIOA0); //enable3	
			gpio_set(GPIOA1); //enable3	
			gpio_set(GPIOA2); //enable3
			can.ctl = 0;
		}
		if(can.ctl == 3)
		{
			zerofinished=0;	
				//1-----------------------------------------------------------------------------
			gpio_clr(GPIOA0);   //enable1	
		    gpio_set(GPIOA7);	//codezero1
		    gpio_clr(GPIOA7);	//codezero1
		    	
			ldata[0]=0x0920;//0x08B0;
			ExBusWR16(0x02,1,ldata);  	    //1#驱动器控制电压输出0。
			OSTimeDly(OS_TICKS_PER_SEC);
			
			ExBusRD16(0x08,1,ldata); //读1 code 
			tmp0=ldata[0] - 0x1804;
					
			ldata[0]=0x0680;	
			ExBusWR16(0x02,1,ldata);  	    //1#驱动器控制电压输出0。
		 	
		 	do                                       //等待上位机向控制器发送握手帧
			{
				ExBusRD16(0x08,1,ldata); //读1 code 
				fdb0=ldata[0];	
				if((fdb0-tmp0)>10) 
				{
					ldata[0]=0x0750;	
					ExBusWR16(0x02,1,ldata);
				}
				if((fdb0-tmp0)<-10) 
				{
					ldata[0]=0x0910;///900	
					ExBusWR16(0x02,1,ldata);
				}	
						
			}
			while(((fdb0-tmp0)>30) || ((fdb0-tmp0)<-10));

			ldata[0]=U0;//0x08B0;
			ExBusWR16(0x02,1,ldata);  	    //1#驱动器控制电压输出0。
			OSTimeDly(OS_TICKS_PER_SEC/20);
		 	do                                       //等待上位机向控制器发送握手帧
			{
				ExBusRD16(0x08,1,ldata); //读1 code 
				fdb0=ldata[0];	
				if((fdb0-tmp0)>10) 
				{
					ldata[0]=0x07E0;	
					ExBusWR16(0x02,1,ldata);
				}
				if((fdb0-tmp0)<-10) 
				{
					ldata[0]=0x08E0;	
					ExBusWR16(0x02,1,ldata);
				}	
						
			}
			while(((fdb0-tmp0)>10) || ((fdb0-tmp0)<-10));
				
			ldata[0]=U0;		
			ExBusWR16(0x02,1,ldata);  	    //1#驱动器控制电压输出0。
				
			gpio_set(GPIOA0);
			gpio_set(GPIOA7);
			gpio_clr(GPIOA7);
			gpio_clr(GPIOA0);   //enable1
			
			gpio_set(BUZZER);                            	//鸣叫BEEP
		    OSTimeDly(OS_TICKS_PER_SEC/10);             	//注意PWM输出引脚与蜂鸣器是同一引脚
			gpio_clr(BUZZER);
				
			OSTimeDly(OS_TICKS_PER_SEC/2);	
			
			//2-----------------------------------------------------------------------------
			gpio_clr(GPIOA1); //enable2	
			gpio_set(GPIOA9); //codezero1
			gpio_clr(GPIOA9); //codezero1	
		   	
			ldata[0]=0x0A70;			//970
			ExBusWR16(0x04,1,ldata);  	    //2#驱动器控制电压输出0。
			OSTimeDly(OS_TICKS_PER_SEC);
			
			ExBusRD16(0x0A,1,ldata); //读2 code 
			tmp0=ldata[0] - 0x1740;			//1740
				
			ldata[0]=0x05C0;	
			ExBusWR16(0x04,1,ldata);  	    //2#驱动器控制电压输出0。
		 	
		 	do                                       //等待上位机向控制器发送握手帧
			{
				ExBusRD16(0x0A,1,ldata); //读2 code 
				fdb0=ldata[0];	
				if((fdb0-tmp0)>100)    //100
				{
					ldata[0]=0x05C0;	//5c0
					ExBusWR16(0x04,1,ldata);
				}
				if((fdb0-tmp0)<-100)    //-100
				{
					ldata[0]=0x097;	//970
					ExBusWR16(0x04,1,ldata);
				}	
						
			}
			while(((fdb0-tmp0)>100) || ((fdb0-tmp0)<-100));   //100 -100
			
			ldata[0]=U0;
			ExBusWR16(0x04,1,ldata);  	    //2#驱动器控制电压输出0。
			OSTimeDly(OS_TICKS_PER_SEC/20);	
			do                                       //等待上位机向控制器发送握手帧
			{
				ExBusRD16(0x0A,1,ldata); //读2 code 
				fdb0=ldata[0];	
				if((fdb0-tmp0)>100) 
				{
					ldata[0]=0x06C0;	
					ExBusWR16(0x04,1,ldata);
				}
				if((fdb0-tmp0)<-100) 
				{
					ldata[0]=0x08F0;				//8f0
					ExBusWR16(0x04,1,ldata);
				}	
						
			}
			while(((fdb0-tmp0)>80)|| ((fdb0-tmp0)<-80));  //80  -80
			
			
			ldata[0]=U0;		
			ExBusWR16(0x04,1,ldata);  	    //2#驱动器控制电压输出0。
			
			gpio_set(GPIOA1);
			gpio_set(GPIOA9);
			gpio_clr(GPIOA9);
			gpio_clr(GPIOA1); //enable2
			
			gpio_set(BUZZER);                            	//鸣叫BEEP
		    OSTimeDly(OS_TICKS_PER_SEC/10);             	//注意PWM输出引脚与蜂鸣器是同一引脚
			gpio_clr(BUZZER); 

			OSTimeDly(OS_TICKS_PER_SEC/2);	
			
			//3-----------------------------------------------------------------------------
			
			gpio_clr(GPIOA2);  								//打开3通道使能enable3	
		    gpio_set(GPIOB1);								//codezero3
		    gpio_clr(GPIOB1);								//codezero3
		    	
			ldata[0]=0x0970;								//给通道3的max502写初值转动找0位。
			ExBusWR16(0x06,1,ldata);  	   					 //3#驱动器控制电压输出碰到正方向线位。
			OSTimeDly(OS_TICKS_PER_SEC);
			
			ExBusRD16(0x0C,1,ldata);     					//读3碰到线位时的编码器数据 code 
			tmp0=ldata[0] - 0x2609;							//假设碰到线位的编码器读数离寻0位置的编码器相差0X2910
				
			ldata[0]=0x0680;	
			ExBusWR16(0x06,1,ldata);  	    				//3#驱动器控制电压输出反方向。
		 	
		 	do                                       		//等待上位机向控制器发送握手帧
			{
				ExBusRD16(0x0C,1,ldata); 					//读3 反方向编码器碰到线位的编码器读数code 
				fdb0=ldata[0];	
				if((fdb0-tmp0)>80) 						//如果反方向的编码器读数比0位时编码器大800就反向转继续找0
				{
					ldata[0]=0x0781;	///680
					ExBusWR16(0x06,1,ldata);
				}
				if((fdb0-tmp0)<-80) 
				{
					ldata[0]=0x087A;	  //08B0
					ExBusWR16(0x06,1,ldata);
				}	
						
			}
			while(((fdb0-tmp0)>80) || ((fdb0-tmp0)<-80));
			
			ldata[0]=U0;	
			ExBusWR16(0x06,1,ldata);
			OSTimeDly(OS_TICKS_PER_SEC/20);
			do                                       		//等待上位机向控制器发送握手帧
			{
				ExBusRD16(0x0C,1,ldata); 					//读3 反方向编码器碰到线位的编码器读数code 
				fdb0=ldata[0];	
				if((fdb0-tmp0)>30) 						//如果反方向的编码器读数比0位时编码器大800就反向转继续找0
				{
					ldata[0]=0x0780;	
					ExBusWR16(0x06,1,ldata);
				}
				if((fdb0-tmp0)<-30) 
				{
					ldata[0]=0x0890;	
					ExBusWR16(0x06,1,ldata);
				}	
						
			}
			while(((fdb0-tmp0)>30) || ((fdb0-tmp0)<-30));
			
			ldata[0]=U0;		
			ExBusWR16(0x06,1,ldata); 	    				//3#驱动器控制电压输出0。
			
			gpio_set(GPIOA2);	
			gpio_set(GPIOB1);
			gpio_clr(GPIOB1);
			gpio_clr(GPIOA2);  		
			
			gpio_set(BUZZER);                            	//鸣叫BEEP
		    OSTimeDly(OS_TICKS_PER_SEC/10);             	//注意PWM输出引脚与蜂鸣器是同一引脚
			gpio_clr(BUZZER); 
			
			while(0)
			{
				ExBusRD16(0x08,1,ldata); //读1 code 
				fdb0=ldata[0];
				ExBusRD16(0x0A,1,ldata); //读2 code 
			    fdb0=ldata[0];	
			    ExBusRD16(0x0C,1,ldata); //读3 code					//读3 反方向编码器碰到线位的编码器读数code 
			    fdb0=ldata[0];			
			}	
			gpio_set(GPIOA7);
			gpio_clr(GPIOA7);
			gpio_set(GPIOA9);
			gpio_clr(GPIOA9);
			gpio_set(GPIOB1);
			gpio_clr(GPIOB1);		
			zerofinished=1;	
			can.ctl = 0;			
		}
		if(can.ctl == 4 && zerofinished == 1)
		{
		    ch1_outdata[0] = can.ch1_cmd;       
	        if(ch1_outdata[0]>=MAXVOUT)
	       	{
	        	ch1_outdata[0]=MAXVOUT;
	        }
	        if(ch1_outdata[0]<=MINVOUT)
	        {
	     		ch1_outdata[0]=MINVOUT;
	       	}      	
	    	//ExBusWR16(0x02,1,ch1_outdata);
	    	ExBusRD16(0x08,1,ldata); //读编码器数据
	    	can.ch1_fdb=ldata[0];
	    	
	//--------------------------------------------------------------------------------
	        ch2_outdata[0] =can.ch2_cmd;
	        if(ch2_outdata[0]>=MAXVOUT)
	       	{
	        	ch2_outdata[0]=MAXVOUT;
	        }
	        if(ch2_outdata[0]<=MINVOUT)
	        {
	     		ch2_outdata[0]=MINVOUT;
	       	}
	        //ExBusWR16(0x04,1,ch2_outdata);           
	    	ExBusRD16(0x0A,1,ldata); //读编码器数据
	    	can.ch2_fdb=ldata[0];
	    	
	//-----------------------------------------------------------------------------
	        ch3_outdata[0] =can.ch3_cmd;
	        if(ch3_outdata[0]>=MAXVOUT)
	       	{
	        	ch3_outdata[0]=MAXVOUT;
	        }
	        if(ch3_outdata[0]<=MINVOUT)
	        {
	     		ch3_outdata[0]=MINVOUT;
	       	}
	       // ExBusWR16(0x06,1,ch3_outdata);          
	    	ExBusRD16(0x0C,1,ldata);           //读编码器数据
	    	can.ch3_fdb=ldata[0];
			
		 	ExBusWR16(0x02,1,ch1_outdata); //1#驱动器控制电压输出。
	    	ExBusWR16(0x04,1,ch2_outdata); //2#驱动器控制电压输出。        
        	ExBusWR16(0x06,1,ch3_outdata); //3#驱动器控制电压输出。	        
	   
	   		can.ctl = 0;
	    }
	  	 	if(can.ctl == 5)
			{
				gpio_set(BUZZER);                            	//鸣叫BEEP
			    OSTimeDly(OS_TICKS_PER_SEC/10);             	
	   		    gpio_clr(BUZZER); 				
				gpio_set(BUZZER);                            	//鸣叫BEEP
			    OSTimeDly(OS_TICKS_PER_SEC/10);             	
	   		    gpio_clr(BUZZER); 
				gpio_set(BUZZER);                            	//鸣叫BEEP
			    OSTimeDly(OS_TICKS_PER_SEC/10);             	
	   		    gpio_clr(BUZZER); 
				gpio_set(BUZZER);                            	//鸣叫BEEP
			    OSTimeDly(OS_TICKS_PER_SEC/10);             	
	   		    gpio_clr(BUZZER); 	   		    	   		    				
				gpio_set(BUZZER);                            	//鸣叫BEEP
			    OSTimeDly(OS_TICKS_PER_SEC/10);             	
	   		    gpio_clr(BUZZER); 				
				can.ctl = 0;		
			}
			if(can.ctl == 6)
			{
				gpio_clr(BUZZER);                            	//鸣叫BEEP
				can.ctl = 0;		
			}		
	}

    while (1)
    {
         OSTimeDly(OS_TICKS_PER_SEC);
    }   
}

/*******************************************************************************************
** Function name: TASK3
** Descriptions : 
** Input        : 无
** Output       : 无 
*******************************************************************************************/
void TASK3(void *pdata)  
{
	pdata = pdata;
	
    while (1)
    {
        OSTimeDly(OS_TICKS_PER_SEC);
    }   
}

/*******************************************************************************************
** Function name: TASK4
** Descriptions : 
** Input        : 无
** Output       : 无 
*******************************************************************************************/
void TASK4(void *pdata)  
{

	 
	pdata = pdata;
	

    while (1)
    {
        OSTimeDly(OS_TICKS_PER_SEC);
    } 
      
}

/*******************************************************************************************
** Function name: TASK5
** Descriptions : 系统正常运行指示灯，如果系统正常运行，LDE会正常闪烁。
** Input        : 无
** Output       : 无 
*******************************************************************************************/
void TASK5(void *pdata)  
{
	pdata = pdata;
    while (1)
    {
        OSTimeDly(OS_TICKS_PER_SEC);
    }  
}
/*******************************************************************************************
**                            End Of File
*******************************************************************************************/