stepmotor_godknows.c

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

				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 30: //Go Forward
				countertime = 0;
				g_servo1_period = (float)0.0015;
				g_servo2_period = (float)0.0015;
				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 >= rightcount)
				{
					MODE = 20;			// drive forward
					DoFlag = 0;
					turncount = countertime * 0.01;
				}
				break;
			case 11:
				if(countertime >= leftcount) //LiSAR has turned in appropriate direction
				{
					MODE = 20;				//drive forward
					DoFlag = 0;
					turncount = countertime * 0.01;
				}
				break;
			case 20:
					if(g_y <= 0)				// LiSAR has reached x = 0
					{
						MODE = 30;
						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;
		}
	}	//end of DoFlag = 1

	// This section calls the function to turn the LiSAR toward the origin and drive there
	if (g_x >0)		// If the LiSAR is to the right of center
		{
				MODE = 11;
				DoFlag = 0;
				leftcount = (((g_heading+(3.14159-atan(g_x/g_y)))/1.5708)*1.025)/0.01;	// Turn left until facing in -x direction
			
		}
		if (g_x < 0)	//If the LiSAR is to the left of center
		{	
			MODE = 10;
			DoFlag = 0;
			rightcount = (((3.14159 + atan(g_x/g_y) - g_heading)/1.5708)*1.125)/0.01;
		}

	}	//end of FoundFlag = 1


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

	//Command LiSAR if one or MULTIPLE Dig_In's are hit
	if((mod_digit <= 12561 && (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;
	}

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

	if(g_digitalia <= 128)
	{
		//Change to Mode 60
		FoundFlag = 1;
	}
	

	

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


	//---------------------------------------------------------------
	// Use the pos array to log position
	// Row 1 is current data, absolute X,Y
	// Row 2 is past data, of X as an angle, and Y as relative radius
	// Row 3 is the entire length it has gone
	//
	//	[ X_current	Y_current ]         ]
	//	[ r_old		theta_old ]
	//	[ Y_prev_total	0	  ]
	//
	// To convert X, which is in 'counts', use the following conversion:
	//	531 counts = 90 degrees
	//	so, multiply counts by 3.14159/(2*531)
	//
	//---------------------------------------------------------------
	// This section updates the current heading and the current x,y position of the LiSAR every time
	// the MODE switches

	if((DoFlag ==0) || ((MODE == 20) && (FoundFlag == 1)))		// If a mode is switched or if LiSAR on final leg of return
	{
		if (turncount != 0)
		{
		g_newheading = (turncount*90*3.14159)/(180*counts);  //g_newheading is the most recent angular change (rad)
		g_heading +=  g_newheading;		//g_heading is the global angular change (rad)
		}
		x_old = g_x;
		y_old = g_y;
		r_new = -(g_optical1)-y_total;

		g_x = (float)r_new*sin(g_heading)+x_old;	//compute the overall global x-position
		g_y = (float)r_new*cos(g_heading)+y_old;	//compute the overall global y-position
	}

	//----------------------------------------------------
	// 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] = g_x;
		g_senddata[3] = g_y;
		g_senddata[4] = (float)g_digitalia;
		g_senddata[5] = delta;
		g_senddata[6] = MODE;
		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;
	}
	//Save past data - first parameter should be 0, not 1, so it doesn't cycle through
	savedata(0,((float)0.010*g_time),g_photo0_array[0],g_photo1_array[0],g_photo0_butter[0],g_photo1_butter[0],0.0F,0.0F,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[100];
		g_photo1_butter[100];
	}
}