stepmotor_godknows.c

RTX是当前非常流行的RTX实时操作系统

	//Read optical encoder
	hard_latch();	// This command latches all four encoder chip's count value
					// hard_latch MUST be called before any calls to read_chip functions
	g_optical0 = (float) read_chip1(); // read encoder chip # 1
	g_optical1 = (float) read_chip2();


	//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);

	//----------------------------------------------------
	// MODE:
	// 	10 - Checking  area: from forward positiion, looks right
	// 	11 - Checking  area: looks left
	// 	12 - Checking  area: returns to forward position
	//
	// 	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
	//	
	//
	//----------------------------------------------------
	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;
		}	//end of Switch statement
	}	//end of DoFlag = 0
	
	if (countertime == 0){
			y_total = -((float)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) 
				{
					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 * 0.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 * 0.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 * 0.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;
		}
	}	//end of DoFlag = 1

	//----------------------------------------------------
	// 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]))
		{
			DoFlag = 0;
			MODE = 40;
			percentlight = (float)0.90*percentlight;
		}
	}
	}		//end of FoundFlag = 0

	


	if (FoundFlag == 1){
	if(DoFlag == 0){
		switch (MODE){
			case 10: //turn right
				countertime = 0;
				g_servo1_period = (float)0.0015 + delta;
				g_servo2_period = (float)0.0015 + delta;
				DoFlag = 1;
				break;
			case 11: //turn left
				countertime = 0;
				g_servo1_period = (float)0.0015 - delta;
				g_servo2_period = (float)0.0015 - delta;
				DoFlag = 1;