stepmotor.c

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

	//	61 - If postive heading, turn CCW
	//	62 - If negative heading, turn CW
	//	63 - Turn towards origin
	//	64 - Move until LiSAR is near origin
	//	65 - STOP! Mission Complete
	//
	//----------------------------------------------------

	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
				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;
			case 60: //Target located.  energize solenoid
				countertime = 0;
				g_servo1_period = (float)0.0015;
				g_servo2_period = (float)0.0015;
				DoFlag = 1;
				break;
			case 61: //Turn CCW towards 0 degree heading
				countertime = 0;
				g_servo1_period = (float)0.0015 - delta;
				g_servo2_period = (float)0.0015 - delta;
				DoFlag = 1;
				break;
			case 62: //Turn CW towards 0 degree heading
				countertime = 0;
				g_servo1_period = (float)0.0015 + delta;
				g_servo2_period = (float)0.0015 + delta;
				DoFlag = 1;
				break;
			case 63: //Turn CCW towards origin
				countertime = 0;
				g_servo1_period = (float)0.0015 - delta;
				g_servo2_period = (float)0.0015 - delta;
				DoFlag = 1;
				break;
			case 64: //Go Forward towards origin
				countertime = 0;
				g_servo1_period = (float)0.0015 + delta;
				g_servo2_period = (float)0.0015 - delta;
				DoFlag = 1;
				break;
			case 65: //STOP! Mission complete
				countertime = 0;
				g_servo1_period = (float)0.0015;
				g_servo2_period = (float)0.0015;
				DoFlag = 1;
				break;
			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.7*checktime)) //LiSAR has left bias
				{
					MODE = 12;
					DoFlag = 0;
				}
				break;
			case 12:
				if(countertime >= (0.85*checktime))
				{
					MODE = 20;
					DoFlag = 0;
				}
				break;
			case 20:
				if(countertime >= forwardtime)
				{
					if(FoundFlag == 1)
					{
						MODE = 60;
						DoFlag = 0;
						phi = 3.14159 - atan(gx/gy);
					}
					if(FoundFlag == 0)
					{
						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;
				}
				break;
			case 52:
				if(countertime >= (0.25*forwardtime))
				{
					MODE = 53;
					DoFlag = 0;
				}
				break;
			case 53:
				if(countertime >= turntime)
				{
					MODE = 20;
					DoFlag = 0;
					TactileFlag = 0;
				}
				break;
			case 54:
				if(countertime >= (0.25*forwardtime))
				{
					MODE = 55;
					DoFlag = 0;
				}
				break;
			case 55:
				if(countertime >= turntime)
				{
					MODE = 20;
					DoFlag = 0;
					TactileFlag = 0;
				}
				break;
			case 56:
				if(countertime >= (0.25*forwardtime))
				{
					MODE = 57;
					DoFlag = 0;
				}
				break;
			case 57:
				if(countertime >= turntime)
				{
					MODE = 20;
					DoFlag = 0;
					TactileFlag = 0;
				}
				break;
			case 60://DRAMATIC PAUSE - DOES A KEANU
				if(countertime >= (100))
				{
					if(heading <= 3.14159)//>= 0)
					{
						MODE = 61;
						DoFlag = 0;
					}
					if(heading > 3.14159)//< 0)
					{
						MODE = 62;
						DoFlag = 0;
					}
				}
				break;
			case 61:
				if(heading <= 0.088)//0.088 rad is 5 degrees
				{
					MODE = 63;
					DoFlag = 0;
				}
				break;
			case 62:
				if(heading <= 0.088)//0.088 rad is 5 degrees
				{
					MODE = 63;
					DoFlag = 0;
				}
				break;
			case 63:
				if((heading > 0.26)&&(heading < (2*3.14159-phi+.000013)))// 0.26 rad is 15 degrees
				{
					MODE = 64;
					DoFlag = 0;
				}
				break;
			case 64:
				if(gy < 0)//&&(abs(gx) < 100))
				{
					MODE = 65;
					DoFlag = 0;
				}
				break;
			case 65:
				//DO NOTHING!
				//MISSION IS COMPLETE!
				break;
			default:
				break;
		}
	}
				
	//----------------------------------------------------
	// Check Digital_In - if anything special, change MODE
	//----------------------------------------------------

	//Get Digital Input
	g_digitalia = (int) dig_in();
	mod_digit = (g_digitalia % 64);
	
	//Command LiSAR if one or MULTIPLE Dig_In's are hit
	//channel 1  mod(255-2-64,64)  // - 61
	//channel 2  mod(255-4-64,64)  // - 59
	//channel 3  mod(255-8-64,64)  // - busted
	//channel 4  mod(255-16-64,64) // - 47
	//channel 5  mod(255-32-64,64) // - 31


	//if(FoundFlag == 0)
	//{
		//Command LiSAR if one or MULTIPLE Dig_In's are hit
		if((mod_digit <= 61) && (mod_digit > 59))
		{
			//61 Forward-Right sensor - DigIn 1
				MODE = 50;
				DoFlag = 0;
				TactileFlag = 1;
		}
		if((mod_digit <= 59) && (mod_digit > 47))
		{
			//59 Forward-Left sensor - DigIn 2!
				MODE = 52;
				DoFlag = 0;
				TactileFlag = 1;
		}
		if((mod_digit <= 47) && (mod_digit > 31))
		{
			//47 Rear-Right sensor - DigIn 4
				MODE = 54;
				DoFlag = 0;
				TactileFlag = 1;
		}
		if((mod_digit <= 31))
		{
			//31 Rear-Left sensor - DigIn 5
				MODE = 56;
				DoFlag = 0;
				TactileFlag = 1;
		}
	//}

	/*if(FoundFlag == 1)
	{
		//Command LiSAR if one or MULTIPLE Dig_In's are hit
		if((mod_digit <= 61) && (mod_digit > 59))
		{
			//61 Forward-Right sensor - DigIn 1
				MODE = 50;
				DoFlag = 0;
				TactileFlag = 1;
		}
		if((mod_digit <= 59) && (mod_digit > 47))
		{
			//59 Forward-Left sensor - DigIn 2!
				MODE = 52;
				DoFlag = 0;
				TactileFlag = 1;
		}
		if((mod_digit <= 47) && (mod_digit > 31))
		{
			//47 Rear-Right sensor - DigIn 4
				MODE = 54;
				DoFlag = 0;
				TactileFlag = 1;
		}
		if((mod_digit <= 31))
		{
			//31 Rear-Left sensor - DigIn 5
				MODE = 56;
				DoFlag = 0;
				TactileFlag = 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 certain agreement between open and closed photoresistor
	// Sets variable indicating target has been spotted
	if((TactileFlag == 0)&&(FoundFlag == 0))
	{
		if (g_photo0_butter[0] <= (0.01*percentlight*g_photo1_butter[0]))
		{
			DoFlag = 0;
			MODE = 40;
			if(CheckFlag == 0)
			{
				checktime = checktime - 100;
				CheckFlag = 1;
			}
		}
	}

	//--------------------------------------
	// Target has been been picked up by
	// solenoid
	//--------------------------------------

	if((g_digitalia <= 192)&&(FoundFlag == 0))
	{
		MODE = 60;
		FoundFlag = 1;
		DoFlag = 0;

		//Calculate angle between gx and gy and then to origin
		phi = 3.14159 - atan(gx/gy);
	}

	//--------------------------------------
	// Ensure Target is still secure
	//--------------------------------------

	if((g_digitalia >192)&&(FoundFlag == 1))
	{
		MODE = 10;
		FoundFlag = 0;
		DoFlag = 0;
	}

	//----------------------------------------------------
	// Send stuff to VB so we can take a look
	//----------------------------------------------------
	if (g_dontupdateflag == 0) {
		g_senddata[0] = g_photo0_array[0];				//pinhole light
		g_senddata[1] = g_photo1_array[0];				//ambient light
		g_senddata[2] = gx;
		g_senddata[3] = gy;
		g_senddata[4] = (float)g_digitalia;
		g_senddata[5] = delta;
		g_senddata[6] = (float)MODE;
		g_senddata[7] = 0.0F;
		g_senddata[8] = (float)heading*180/3.14159;		//heading in degrees
		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;
	}
	//Save past data - first parameter should be 0, not 1, so it doesn't cycle through
	if((g_time % 10) == 0) //save only every 10ms *10 = 100 ms
	{
		savedata(0,((float)0.010*g_time),g_photo0_array[0],g_photo1_array[0],g_photo0_butter[0],g_photo1_butter[0],gx,gy,0.0F);
	}

	// update time index
	g_time = g_time + 1;

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

    return 0;
}

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

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

		g_photo0_butter[j] = 0.0F;
		g_photo1_butter[j] = 0.0F;
	}

	//Initialize Path and Obstacle array
	for(j = 0; j < 1000; j++)
	{
		for(i = 0; i < 2; i++)
		{
			g_map_path[j][i] = 0;
			g_map_obst[j][i] = 0;
		}
	}
}