notworking - stepmotor.c

网上下载的一个rtx例子

	//Send out voltage?  ...nah
	//volt_out(0,uopen);  //uopen is the sent out voltage

	//Send data to VB just so we can see what's up
	if (g_dontupdateflag == 0) {
		g_senddata[0] = g_photo0;	//pinhole light
		g_senddata[1] = g_photo1;	//ambient light
		g_senddata[2] = g_optical0;
		g_senddata[3] = g_optical1;
		g_senddata[4] = (float)g_digitalia;
		g_senddata[5] = 0.0F;
		g_senddata[6] = 0.0F;
		g_senddata[7] = 0.0F;
		g_senddata[8] = 0.0F;
		g_senddata[9] = 0.0F;
		g_senddata[10] = 0.0F;
		g_senddata[11] = 0.0F;
		g_senddata[12] = 0.0F;
		g_senddata[13] = 0.0F;
		g_senddata[14] = 0.0F;
		g_senddata[15] = 0.0F;
		g_senddata[16] = 0.0F;
		g_senddata[17] = 0.0F;
		g_senddata[18] = 0.0F;
		g_senddata[19] = 0.0F;
	}

    return 0;
}



int RTFCNDCL TmrAuto(PVOID unused) { // AUTONOMOUS!

	int j = 0;			//counter for various things

	//Send Digout 4, for testing purposes only
	dig_out(4);

	//----------------------------------------------------
	// Use Optical Encoder to Map area
	//----------------------------------------------------

	//Read optical encoder
	hard_latch();	// hard_latch MUST be called before any calls to read_chip functions
					
	g_optical0 = (float) read_chip1();	//X-Direction...
	g_optical1 = (float) read_chip2();	//Y-Direction.. forward is negative Y

	//----------------------------------------
	//			-Y(forward)
	//			 _
	//			/ \
	//	+X	   |   |	-X (turning right) ->
	//			\_/
	//
	//			+Y
	//----------------------------------------


	//----------------------------------------------------
	// MODE:
	// 	10 - Checking  area: from forward positiion, looks right
	// 	11 - Checking  area: looks left until centered
	// 	12 - Checking  area: continues left
	//  13 - Checking  area: looks right until centered
	//
	// 	20 - Goes forward
	// 
	//	30 - Photoresistor senses a 'maximum' in light
	//
	//	40 - Photoresistor senses close agreement to ambient light
	//
	//	50 - Tactile sensorr has gone off: F - right, backup
	//	51 - F-right, turn left
	//	52 - F-left, backup
	//	53 - f-left, turn right
	//	54 - R-right, go forward
	//	55 - R-right, turn right
	//	56 - R-left, go forward
	//	57 - R-left, turn left
	//
	//	60 - Target found, turn on solenoid
	//	
	//
	//----------------------------------------------------

	//----------------------------------------------------
	// Beginning of Found Flag = 0
	//----------------------------------------------------	

	
	if (FoundFlag == 0)
	{
		if(DoFlag == 0)
		{
			switch (MODE)
			{
				case 10: //Check around - 1st step, look right
					countertime = 0;
					g_servo1_period = (float)0.0015 + delta;
					g_servo2_period = (float)0.0015 + delta;
					DoFlag = 1;
					break;
				case 11: //Check around - 2nd step, look left, back to center
					countertime = 0;
					g_servo1_period = (float)0.0015 - delta;
					g_servo2_period = (float)0.0015 - delta;
					DoFlag = 1;
					break;
				case 12: //Check around - 3rd step, look left
					countertime = 0;
					g_servo1_period = (float)0.0015 - delta;
					g_servo2_period = (float)0.0015 - delta;
					DoFlag = 1;
					break;
				case 13: //Check around - 4th step, look right, back to center
					countertime = 0;
					g_servo1_period = (float)0.0015 + delta;
					g_servo2_period = (float)0.0015 + delta;
					DoFlag = 1;
					break;
				case 20: //Go Forward
					countertime = 0;
					g_servo1_period = (float)0.0015 + delta;
					g_servo2_period = (float)0.0015 - delta;
					DoFlag = 1;
					break;
				case 40: //Target located--move forward
					g_servo1_period = (float)0.0015 + delta;
					g_servo2_period = (float)0.0015 - delta;
					DoFlag = 1;
					/*if (g_photo1_avg[0] < .001){
						MODE = 60;
					}*/
					break;
				case 50: //F-Right tactile sensor touched, back up
					countertime = 0;
					g_servo1_period = (float)0.0015 - delta;
					g_servo2_period = (float)0.0015 + delta;
					DoFlag = 1;
					break;
				case 51: //F-Right cont., turn left a little 
					countertime = 0;
					g_servo1_period = (float)0.0015 - delta;
					g_servo2_period = (float)0.0015 - delta;
					DoFlag = 1;
					break;
				case 52: //F-Left tactile sensor touched, back up
					countertime = 0;
					g_servo1_period = (float)0.0015 - delta;
					g_servo2_period = (float)0.0015 + delta;
					DoFlag = 1;
					break;
				case 53: //F-Left cont., turn right a little
					countertime = 0;
					g_servo1_period = (float)0.0015 + delta;
					g_servo2_period = (float)0.0015 + delta;
					DoFlag = 1;
					break;
				case 54: //R-Right tactile sensor touched, go forward a little
					countertime = 0;
					g_servo1_period = (float)0.0015 + delta;
					g_servo2_period = (float)0.0015 - delta;
					DoFlag = 1;
					break;
				case 55: //R-Right cont., turn right a little 
					countertime = 0;
					g_servo1_period = (float)0.0015 + delta;
					g_servo2_period = (float)0.0015 + delta;
					DoFlag = 1;
					break;
				case 56: //R-Left tactile sensor touched, go forward a little
					countertime = 0;
					g_servo1_period = (float)0.0015 + delta;
					g_servo2_period = (float)0.0015 - delta;
					DoFlag = 1;
					break;
				case 57: //R-Left cont., turn left a little
					countertime = 0;
					g_servo1_period = (float)0.0015 - delta;
					g_servo2_period = (float)0.0015 - delta;
					DoFlag = 1;
					break;
				case 60: //Target located.  energize solenoid
					g_servo1_period = (float)0.0015;
					g_servo2_period = (float)0.0015;
				default:
					break;
			}
		}
	
		if (countertime == 0)
		{
			y_total = -(g_optical1);
			turncount = 0;
		}

		// Update Servo Period with task in mind
		command_servo(1,3,g_servo1_period);
		command_servo(2,3,g_servo2_period);

		//Increase Countertime! - 10ms has passed
		countertime++;


		//Check which mode it is in and see if it is done with its task, then switch modes
		if(DoFlag == 1)
		{
			switch(MODE)
			{
				case 10:
					if(countertime >= checktime)
					{
						MODE = 11;
						DoFlag = 0;
						counts = 1.125;			//time (in sec) for LiSAR to turn 90 degrees right
					}
					break;
				case 11:
					if(countertime >= checktime) //LiSAR has left bias
					{
						MODE = 12;
						DoFlag = 0;
					}
					break;
				case 12:
					if(countertime >= checktime)
					{
						MODE = 13;
						DoFlag = 0;
						counts = -1.025;		//time (in sec) for LiSAR to turn 90 degrees left
					}							//counts is negative to indicate that the angle turned is neg
					break;
				case 13:
					if(countertime >= checktime)
					{
						MODE = 20;	
						DoFlag = 0;
					}
					break;	
				case 20:
					if(countertime >= forwardtime)
					{
						MODE = 10;
						DoFlag = 0;
					}
					break;
				case 40:
					if(countertime >= targettime)
					{
						MODE = 10;
						DoFlag = 0;
					}
					break;
				case 50:
					if(countertime >= (0.25*forwardtime))
					{
						MODE = 51;
						DoFlag = 0;
					}
					break;
				case 51:
					if(countertime >= turntime)
					{
						MODE = 20;
						DoFlag = 0;
						TactileFlag = 0;
						counts = -1.025;				//time (in sec) for LiSAR to turn 90 degrees left
						turncount = countertime*.01;	//calculates the time (in sec) LiSAR has been turning
					}									//counts is negative to indicate that the angle turned is neg
					break;
				case 52:
					if(countertime >= (0.25*forwardtime))
					{
						MODE = 53;
						DoFlag = 0;
					}
					break;
				case 53:
					if(countertime >= turntime)
					{
						MODE = 20;
						DoFlag = 0;
						TactileFlag = 0;
						counts = 1.125;					//time (in sec) for LiSAR to turn 90 degrees right
						turncount = countertime*.01;	//calculates the time (in sec) LiSAR has been turning
					}
					break;
				case 54:
					if(countertime >= (0.25*forwardtime))
					{
						MODE = 55;
						DoFlag = 0;
					}
					break;
				case 55:
					if(countertime >= turntime)
					{
						MODE = 20;
						DoFlag = 0;
						TactileFlag = 0;
						counts = 1.125;					//time (in sec) for LiSAR to turn 90 degrees right
						turncount = countertime*.01;	//calculates the time (in sec) LiSAR has been turning
					}
					break;
				case 56:
					if(countertime >= (0.25*forwardtime))
					{
						MODE = 57;
						DoFlag = 0;
					}
					break;
				case 57:
					if(countertime >= turntime)
					{
						MODE = 20;
						DoFlag = 0;
						TactileFlag = 0;
						counts = -1.025;				//time (in sec) for LiSAR to turn 90 degrees left
						turncount = countertime*.01;	//calculates the time (in sec) LiSAR has been turning
					}									//counts is negative to indicate that the angle turned is neg
					break;
				default:
					break;
			}
		}

		//----------------------------------------------------
		// Check Resistors - if anything special, change MODE
		//----------------------------------------------------

		//First Collect Data from analog in, shift everything up, put new value at front
		for(j = 99; j > 0; j--)
		{
			g_photo0_array[j] = g_photo0_array[j-1];
			g_photo1_array[j] = g_photo1_array[j-1];
		}

		//Put new value at front
		g_photo0_array[0] = volt_in(0); //Pin hole resistor
		g_photo1_array[0] = volt_in(1); //Ambient Light Resistor

		//What the heck, lets put a butterworth filter on it
		g_photo0_butter[0] = 0.0;
		g_photo1_butter[0] = 0.0;

		for (j = 0; j < FILTER_ORDER; j++) {
			g_photo0_butter[0] += g_b[j]*g_photo0_array[j] - g_a[j+1]*g_photo0_butter[j+1];
			g_photo1_butter[0] += g_b[j]*g_photo1_array[j] - g_a[j+1]*g_photo1_butter[j+1];
		}

		g_photo0_butter[0] += g_b[j]*g_photo0_array[j];
		g_photo1_butter[0] += g_b[j]*g_photo1_array[j];

		//Update past data
		for (j = 99; j > 0; j--) {
			g_photo0_butter[j] = g_photo0_butter[j-1];
			g_photo1_butter[j] = g_photo1_butter[j-1];
		}
		
		// Search for a minimum in streaming photoresistor voltage
		// Also search for certain agreement between open and closed photoresistor
		// Sets variable indicating target has been spotted
		if(TactileFlag == 0)
		{
			if (g_photo0_array[0] <= (0.01*percentlight*g_photo1_array[0]))
			{	
				turncount = countertime*.01;
				DoFlag = 0;
				MODE = 40;
				percentlight = (float)1.0*percentlight;
			}
		}

		//--------------------------------------