stepmotor_new.c

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

	}

    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(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 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;
			default:
				break;
		}
	}

	// Update Servo Period with task in mind
	if(countertime == 0)
	{
		command_servo(1,3,g_servo1_period);
		command_servo(2,3,g_servo2_period);
	}

	//Increase Countertime!
	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 >= (2*checktime))
				{
					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 50:
				if(countertime >= (0.125*forwardtime))
				{
					MODE = 51;
					doflag = 0;
				}
				break;
			case 51:
				if(countertime >= (0.5*checktime))
				{
					MODE = 20;
					doflag = 0;
				}
				break;
			case 52:
				if(countertime >= (0.125*forwardtime))
				{
					MODE = 53;
					doflag = 0;
				}
				break;
			case 53:
				if(countertime >= (0.5*checktime))
				{
					MODE = 20;
					doflag = 0;
				}
				break;
			case 54:
				if(countertime >= (0.125*forwardtime))
				{
					MODE = 55;
					doflag = 0;
				}
				break;
			case 55:
				if(countertime >= (0.5*checktime))
				{
					MODE = 20;
					doflag = 0;
				}
				break;
			case 56:
				if(countertime >= (0.125*forwardtime))
				{
					MODE = 57;
					doflag = 0;
				}
				break;
			case 57:
				if(countertime >= (0.5*checktime))
				{
					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

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

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

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


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

	//Get Digital Input
	g_digitalia = (float) dig_in();

	switch(digitalia){
		case 200:	//Forward-Right sensor
			MODE = 50;
			break;
		case 200:	//Forward-Left sensor
			MODE = 52;
			break;
		case 200:	//Rear-Right sensor
			MODE = 54;
			break;
		case 200:	//Rear-Left sensor
			MODE = 56;
			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;	//pinhole light
		g_senddata[1] = g_photo1_avg;	//ambient light
		g_senddata[2] = g_optical0;
		g_senddata[3] = g_optical1;
		g_senddata[4] = 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] = 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] = g_servo1_period;
		g_senddata[17] = g_servo2_period;
		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???!?!?!?
}