notworking - stepmotor.c

网上下载的一个rtx例子

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

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

	}			

	//----------------------------------------------------
	// End of Found Flag = 0
	//----------------------------------------------------	

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

	//----------------------------------------------------
	// Beginning of Found Flag = 1
	//----------------------------------------------------	

	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;
					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: //Stop
					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;
			}
		}
	
		if (countertime == 0)
		{
			y_total = -(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;
						DoFlag = 0;
						turncount = countertime * 0.01;
					}
					break;
				case 11:
					if(countertime >= leftcount) //LiSAR has turned to 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;
			}
		}
		
		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 Found Flag = 1
	//----------------------------------------------------	

	// 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;				//LiSAR starts in Y direction
		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],g_x,g_y,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];
	}

	/*//Initialize Position array
	for(j = 0; j < 3; j++)
	{
		for(i = 0; i < 2; i++)
		{
			g_map_pos[j][i] = 0;
		}
	}

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