motor.c

labview开发的智能仪器试验

#include "toolbox.h"
#include <cvirte.h>		
#include <userint.h>
#include "motor.h"
#include "dask.h"

static int panelHandle;
int cardID=-1;

int power_on=0;
int is_run=0;
int is_start=0;
int is_dc=1;
int method=1;
int volt_out=0;
int left_flag=0;
int right_flag=0;
double motor_volt=0;
double read=0;

///pid
float ki=5.0;
float kp=1.0;
float kd=0.05;
float a0=0;
float a1=0;
float a2=0;
float e0=0;
float e1=0;
float e2=0;
float u0=0;
float u1=0;
int range=50;
float delt_u=0;
float tick=0.1;

//setp motor
int stepLeft[8]={0xff1f,0xff3f,0xff2f, 0xff6f,0xff4f,0xffcf,0xff8f, 0xff9f};
int stepRight[8]={0xff9f,0xff8f,0xffcf, 0xff4f,0xff6f,0xff2f,0xff3f, 0xff1f} ;
int mask=0;
int count=0;
int index=0;
int target=0; 
int dis_count=0;

int impulse=0;
int inport=0;

double setPosition=6;
double curPosition=0;
double distance=0;
double speed=0;

double motor_arry[2]={0};
double pos_arry[2]={0};

int left_led=0;
int right_led=0;
unsigned short int temp1=0;
unsigned short int temp2=0;

void pid_compute()
{
	a0=kp*(1+tick/ki+kd/tick);
	a1=-kp*(1+2*kd/tick);
	a2=kp*(kd/tick);
} 

void display()
{
	//curPosition=Random(2,14);		////TEST
	pos_arry[0]=setPosition;
	pos_arry[1]=curPosition;
	PlotStripChart(panelHandle,PANEL_STRIPCHART,pos_arry,2,0,0,VAL_DOUBLE);
	SetCtrlVal(panelHandle,PANEL_NUMERIC_4,impulse);
	impulse=0;
	
	if(is_dc)
	{
		AI_ReadChannel(cardID,1,AD_U_5_V,&temp2);
		temp2=(temp2>>4)&0x0fff;
		//temp2=(int)Random(300,900);  /////TEST
		speed=80*temp2*12.0/4096;
		SetCtrlVal(panelHandle,PANEL_NUMERICGAUGE,speed); 
		
		motor_arry[0]=speed;
		motor_arry[1]=motor_volt;
		PlotStripChart(panelHandle,PANEL_STRIPCHART_2,motor_arry,2,0,0,VAL_DOUBLE); 
	}
	if(!is_dc)
	{
		if(target==0)
			target=1;
		speed=62.5;
		motor_arry[0]=speed;
		motor_arry[1]=0;
		SetCtrlVal(panelHandle,PANEL_NUMERICGAUGE,speed);
		PlotStripChart(panelHandle,PANEL_STRIPCHART_2,motor_arry,2,0,0,VAL_DOUBLE);  
	}
	
}


int main (int argc, char *argv[])
{
	if (InitCVIRTE (0, argv, 0) == 0)
		return -1;	/* out of memory */
	if ((panelHandle = LoadPanel (0, "motor.uir", PANEL)) < 0)
		return -1;
	cardID=Register_Card(PCI_9112,0);     
	DisplayPanel (panelHandle);
	RunUserInterface ();
	DiscardPanel (panelHandle);
	return 0;
}


int CVICALLBACK quit (int panel, int control, int event,
		void *callbackData, int eventData1, int eventData2)
{
	switch (event)
	{
		case EVENT_COMMIT:
			AO_WriteChannel(cardID,0,2048);
			DO_WritePort(cardID,0,0x0001);
			if(cardID>0)
			{
			   Release_Card(cardID);
			}
			QuitUserInterface (0);
			break;
	}
	return 0;
}

int CVICALLBACK dc_motor (int panel, int control, int event,
		void *callbackData, int eventData1, int eventData2)
{
	switch (event)
	{
		case EVENT_COMMIT:
			GetCtrlVal(panelHandle,PANEL_RADIOBUTTON,&is_dc);
			//is_dc=1;
			if(is_dc==1)
				AO_WriteChannel(cardID,0,2048); 
			SetCtrlVal(panelHandle,PANEL_RADIOBUTTON,is_dc);
			SetCtrlVal(panelHandle,PANEL_LED_2,is_dc);
			
			SetCtrlVal(panelHandle,PANEL_RADIOBUTTON_2,!is_dc);
			SetCtrlVal(panelHandle,PANEL_LED_3,!is_dc);
			
			ClearStripChart(panelHandle,PANEL_STRIPCHART);
			ClearStripChart(panelHandle,PANEL_STRIPCHART_2);
			
			SetCtrlAttribute(panelHandle,PANEL_RADIOBUTTON_3,ATTR_DIMMED,0);
			SetCtrlAttribute(panelHandle,PANEL_RADIOBUTTON_4,ATTR_DIMMED,0); 
			SetCtrlAttribute(panelHandle,PANEL_RADIOBUTTON_5,ATTR_DIMMED,0); 
			if(2==method)
			{
				SetCtrlAttribute(panelHandle,PANEL_NUMERIC,ATTR_DIMMED,0);
			 	SetCtrlAttribute(panelHandle,PANEL_NUMERIC_2,ATTR_DIMMED,0);
				SetCtrlAttribute(panelHandle,PANEL_NUMERIC_3,ATTR_DIMMED,0);
			}
			break;
	}
	return 0;
}

int CVICALLBACK step_motor (int panel, int control, int event,
		void *callbackData, int eventData1, int eventData2)
{
	switch (event)
	{
		case EVENT_COMMIT:
			is_dc=0;
			AO_WriteChannel(cardID,0,2048);    
			SetCtrlVal(panelHandle,PANEL_RADIOBUTTON,is_dc);
			SetCtrlVal(panelHandle,PANEL_LED_2,is_dc);
			
			SetCtrlVal(panelHandle,PANEL_RADIOBUTTON_2,!is_dc);
			SetCtrlVal(panelHandle,PANEL_LED_3,!is_dc);
			
			ClearStripChart(panelHandle,PANEL_STRIPCHART);
			ClearStripChart(panelHandle,PANEL_STRIPCHART_2);
			
			SetCtrlAttribute(panelHandle,PANEL_NUMERIC,ATTR_DIMMED,1);
			SetCtrlAttribute(panelHandle,PANEL_NUMERIC_2,ATTR_DIMMED,1);
			SetCtrlAttribute(panelHandle,PANEL_NUMERIC_3,ATTR_DIMMED,1);
			SetCtrlAttribute(panelHandle,PANEL_RADIOBUTTON_3,ATTR_DIMMED,1);
			SetCtrlAttribute(panelHandle,PANEL_RADIOBUTTON_4,ATTR_DIMMED,1); 
			SetCtrlAttribute(panelHandle,PANEL_RADIOBUTTON_5,ATTR_DIMMED,1); 
			
			break;
	}
	return 0;
}

int CVICALLBACK power (int panel, int control, int event,
		void *callbackData, int eventData1, int eventData2)
{
	switch (event)
	{
		case EVENT_COMMIT:
			GetCtrlVal(panelHandle,PANEL_NUMERIC,&kp);
			GetCtrlVal(panelHandle,PANEL_NUMERIC_2,&ki);
			GetCtrlVal(panelHandle,PANEL_NUMERIC_3,&kd);
			
			GetCtrlVal(panelHandle,PANEL_TOGGLEBUTTON,&power_on);
			SetCtrlVal(panelHandle,PANEL_LED,power_on);
			/*if(power_on && cardID<0)
			{
				cardID=Register_Card(PCI_9112,0);
				if(cardID>0)
				{
					is_run=1;
				}
				else
				{
					MessagePopup("error","register card fail");	
					is_run=0;
				}
			}
			else
			{
				is_run=0;
			}  */
			if(cardID>=0)
			{
				Release_Card(cardID);
				cardID=Register_Card(PCI_9112,0);
			    is_run=1;
				AO_WriteChannel(cardID,0,2048);
				DO_WritePort(cardID,0,0x0001);
				
			}
			else
			{
				MessagePopup("warning","Register card fail");
				is_run=0;
			}
			break;
	}
	return 0;
}

int CVICALLBACK left_shift (int panel, int control, int event,
		void *callbackData, int eventData1, int eventData2)
{
	switch (event)
	{
		case EVENT_COMMIT:

			break;
		case EVENT_LEFT_CLICK:
		/*	if(is_start && !is_dc )    //step motor  
			{
				//DI_ReadPort(cardID,0,&mask); 
				DO_WritePort(cardID,0,stepLeft[index] );
				index++;
				index%=8;	
			}
			if(is_start && is_dc )    //dc motor
			{
				 AO_WriteChannel(cardID,1,2048-1000);	
			}  */
			if(is_start)
			{
				AO_WriteChannel(cardID,0,2048);
				left_flag=1;
				right_flag=0;
			}
			
			break;
	}
	return 0;
}

int CVICALLBACK right_shift (int panel, int control, int event,
		void *callbackData, int eventData1, int eventData2)
{
	switch (event)
	{
		case EVENT_COMMIT:

			break;
		case EVENT_LEFT_CLICK:
	/*	if(is_start && !is_dc && is_run)    //step motor  
		{
			DI_ReadPort(cardID,0,&mask); 
			DO_WritePort(cardID,0,stepRight[index]&(mask|0x00f0) );
			index++;
			index%=8;	
		}
		if(is_start && is_dc && is_run)    //dc motor
		{
			 AO_WriteChannel(cardID,1,2048+1000);	
		} */
		if(is_start)
			{
				AO_WriteChannel(cardID,0,2048);
				left_flag=0;
				right_flag=1;
			}
		break;
	}
	return 0;
}

int CVICALLBACK stop (int panel, int control, int event,
		void *callbackData, int eventData1, int eventData2)
{
	switch (event)
	{
		case EVENT_COMMIT:
			if(is_run==1)
			{
				is_start=0;
				left_flag=0;
				right_flag=0;
				DO_WritePort(cardID,0,0x002);   //急停
				DO_WritePort(cardID,0,0x001);
			}
			break;
	}
	return 0;
}

int CVICALLBACK setPositon (int panel, int control, int event,
		void *callbackData, int eventData1, int eventData2)
{
	switch (event)
	{
		case EVENT_COMMIT:

			break;
		case EVENT_VAL_CHANGED:
			GetCtrlVal(panelHandle,PANEL_NUMERICSLIDE_2,&setPosition);
			break;
	}
	return 0;
}
					/////////////////timer/////////////
int CVICALLBACK timer1 (int panel, int control, int event,
		void *callbackData, int eventData1, int eventData2)
{
	switch (event)
	{