copy of stepmotor.c

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

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

	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
				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 right again
				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;
		}
	}

	// 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;
				}
				break;
			case 11:
				if(countertime >= (1.6*checktime)) //LiSAR has left bias
				{
					MODE = 12;
					doflag = 0;
				}
				break;
			case 12:
				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;
				}
				break;
			case 52:
				if(countertime >= (0.25*forwardtime))
				{
					MODE = 53;
					doflag = 0;
				}
				break;
			case 53:
				if(countertime >= turntime)
				{
					MODE = 20;
					doflag = 0;
				}
				break;
			case 54:
				if(countertime >= (0.25*forwardtime))
				{
						MODE = 55;
						doflag = 0;
				}
				break;
			case 55:
				if(countertime >= turntime)
				{
					MODE = 20;
					doflag = 0;
				}
				break;
			case 56:
				if(countertime >= (0.25*forwardtime))
				{
					MODE = 57;
					doflag = 0;
				}
				break;
			case 57:
				if(countertime >= turntime)
				{
					MODE = 20;
					doflag = 0;
				}
				break;
			default:
				break;
		}
	}
				
	//----------------------------------------------------
	// Check Resistors - if anything special, change MODE
	//----------------------------------------------------

	g_photo0_array[0] = volt_in(0); //Pin hole resistor
	g_photo1_array[0] = volt_in(1); //Ambient Light Resistor

	g_photo0_array[1] = 0.0; //Pin hole resistor
	g_photo1_array[1] = 0.0; //Ambient Light Resistor

	//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];
	}

	
	//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 = FILTER_ORDER; j > 0; j--) {
		g_photo0_butter[j] = g_photo0_butter[j-1];
		g_photo1_butter[j] = g_photo1_butter[j-1];
		//g_photo0_array[j] = g_photo0_array[j-1]; updated up there
		//g_photo0_array[j] = g_photo0_array[j-1];
	}
	

	
	//Now Compute an average - less shifty, will be easier to work with?
	g_photo0_avg[0] = 0;
	g_photo1_avg[0] = 0;
	for(j = 0; j < 100; j++)
	{
		g_photo0_avg[0] += (float)0.01*g_photo0_butter[j];
		g_photo1_avg[0] += (float)0.01*g_photo1_butter[j];
	}

	for(j = 7; j > 0; j--)
	{
		g_photo0_avg[j] = g_photo0_avg[j-1];
		g_photo1_avg[j] = g_photo1_avg[j-1];
	}
	
	
	// If photoresistors sense "equivalence" in the resistance, move forward toward light
	// Sets variable indicating target has been spotted
	if (g_photo0_avg[0] < 5*g_photo1_avg[0])
	{
		doflag = 0;
		MODE = 40;
		TargetFlag = 1;
	}
		

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

	//Get Digital Input
	g_digitalia = (int) dig_in();
	
	//if (TargetFlag == 0){
	switch(g_digitalia){
		case 252:	//Forward-Right sensor - DigIn 1
			MODE = 50;
			doflag = 0;
			break;
		case 238:	//Forward-Left sensor - DigIn 4!
			MODE = 52;
			doflag = 0;
			break;
		case 250:	//Rear-Right sensor - DigIn 2
			MODE = 54;
			doflag = 0;
			break;
		case 246:	//Rear-Left sensor - DigIn 3
			MODE = 56;
			doflag = 0;
			break;
		default:
			break;
	}
	//}

	/*if (TargetFlag ==1){
	switch(g_digitalia){
		case 252:	//Forward-Right sensor - DigIn 1
			MODE = 54;
			doflag = 0;
			break;
		case 238:	//Forward-Left sensor - DigIn 4!
			MODE = 54;
			doflag = 0;
			break;
		case 250:	//Rear-Right sensor - DigIn 2
			MODE = 54;
			doflag = 0;
			break;
		case 246:	//Rear-Left sensor - DigIn 3
			MODE = 54;
			doflag = 0;
			break;
		default:
			break;
	}
	}*/
	//----------------------------------------------------
	// 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();
	g_optical1 = (float) read_chip2();
	
	//----------------------------------------------------
	// Send stuff to VB so we can take a look
	//----------------------------------------------------
	if (g_dontupdateflag == 0) {
		g_senddata[0] = g_photo0_avg[0];	//pinhole light
		g_senddata[1] = g_photo1_avg[0];	//ambient light
		g_senddata[2] = g_optical0;
		g_senddata[3] = g_optical1;
		g_senddata[4] = (float)g_digitalia;
		g_senddata[5] = delta;
		g_senddata[6] = 0.0F;
		g_senddata[7] = 0.0F;
		g_senddata[8] = 0.0F;
		g_senddata[9] = 0.0F;
		g_senddata[10] = g_photo0_butter[0];
		g_senddata[11] = g_photo1_butter[0];
		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;
	}

	//Send back to zero - only for testing purposes
	dig_out(0);

    return 0;
}

void FunctionINIT()	//Only here to initialize variables
{
	int j = 0;

	//Initialize photoresistor variables
	for(j = 0; j < 100; j++)
	{
		g_photo0_array[j] = 0.0F;
		g_photo1_array[j] = 0.0F;
	}

	//Can initialize more here???!?!?!?
}