예제 #1
0
void main(void) {
  (void)printf("Start of E128 program\r\n");
  InitPorts();
  InitTimer();
  InitMotors();
  InitSide();
  InitBeacons();
  EnableInterrupts;

  StartMasterSM();

  while(1) {		   		     // Repeat State Machine Forever
    (void)RunMasterSM(CheckEvents());
  }
}
예제 #2
0
int main( void )
{
    int         i;
    int         led = 0;
    uint16_t    prevSwitches = 0;

    InitHardware();
    InitADC();
    InitMotors();

    eeprom_read_block( &gMemParam, &gEEParam, sizeof( gMemParam ));

    if ( gMemParam.thresh_hi == 0xFF )
    {
        gMemParam.thresh_hi = 0x80;
    }
    if ( gMemParam.thresh_lo == 0xFF )
    {
        gMemParam.thresh_lo = 0x10;
    }

#if CFG_LOG_USE_STDIO
    fdevopen( UART0_PutCharStdio, UART0_GetCharStdio );

    LogInit( stdout );
#endif

    // The first handle opened for read goes to stdin, and the first handle
    // opened for write goes to stdout. So u0 is stdin, stdout, and stderr

    Log( "*****\n" );
    Log( "***** Line Maze program\n" );
    Log( "*****\n" );

    LCD_Init( 2, 16 );
    LCD_Printf( " SRS Sample Bot " );
    LCD_MoveTo( 0, 1 );
    LCD_Printf( "Second line" );

    Log( "\n" );

    MENU_Init( gTopMenu );

    LED_OFF( GREEN );

    while( 1 )
    {
        uint16_t    switches;
        uint8_t     pinc;
        uint8_t     pind;
        int8_t      error;

        LED_TOGGLE( GREEN );

        led++;
        if ( led >= 6 )
        {
            led = 0;
        }

        error = GetLineError();

        if ( MENU_IsActive() )
        {
            MENU_Event( MENU_EVENT_TIMER );
        }
        else
        {
            if ( MENU_IsModified() )
            {
                eeprom_write_block( &gMemParam, &gEEParam, sizeof( gMemParam ));
                MENU_ClearModified();

                LCD_Clear();
                LCD_Printf( "EEPROM Updated\n" );
                Log( "EEPROM Updated\n" );
                ms_spin( 1000 );
            }

            //switches = EXP_TransferWord( ~( 1 << ( led + 2 )), EXP_OUT_LED_MASK );
            switches = EXP_TransferWord( 0, 0 );
            Log( "SW:%04x ", switches );

            if ( switches != prevSwitches )
            {
                LCD_Clear();
                prevSwitches = switches;
            }
            for ( i = 0; i < 8; i++ )
            {
                Log( "%02x ", gLineADC[ i ]);
            }

            //Log( "C: %02x\n", PINC );

            switch (( switches & 0xF0 ) >> 4 )
            {
            case 0:
            {
                LCD_MoveTo( 0, 0 );
                LCD_Printf( " SRS Sample Bot " );
                LCD_MoveTo( 0, 1 );
                LCD_Printf( "Second line" );
                break;
            }

            case 1:
            {
                LCD_MoveTo( 0, 0 );
                LCD_Printf( "Joy: %c%c%c%c%c",
                            (( switches & 0x4000 ) == 0 ) ? 'L' : ' ',
                            (( switches & 0x2000 ) == 0 ) ? 'R' : ' ',
                            (( switches & 0x0800 ) == 0 ) ? 'U' : ' ',
                            (( switches & 0x0400 ) == 0 ) ? 'D' : ' ',
                            (( switches & 0x1000 ) == 0 ) ? 'X' : ' ' );
                LCD_MoveTo( 0, 1 );
                LCD_Printf( "S1:%d S2:%d S3:%d",
                            ( switches & 0x0200 ) == 0,
                            ( switches & 0x0100 ) == 0,
                            ( PIND & ( 1 << 6 ))  == 0 );
                break;
            }

            case 2:
            {
                LCD_MoveTo( 0, 0 );
                LCD_Printf( "L %02x %02x %02x %02x %02x",
                            gLineADC[ 0 ],
                            gLineADC[ 1 ],
                            gLineADC[ 2 ],
                            gLineADC[ 3 ],
                            gLineADC[ 4 ] );

                LCD_MoveTo( 0, 1 );
                LCD_Printf( "E %02x %02x %3d B %02x", gADC[ 0 ], gADC[ 7 ], error, gADC[ 6 ] );
                break;
            }

            default:
            {
                LCD_MoveTo( 0, 0 );
                LCD_Printf( "Setting: %d", ( switches & 0xF0 ) >> 4 );
                break;
            }
            }

            pinc = PINC;
            pind = PIND;

            Log( " QL:%d%d QR:%d%d EC-L:%d EC-R:%d Err:%2d L:%5b H:%5b\n",
                 ( ENCODER_L_A_PIN & ENCODER_L_A_MASK ) != 0,
                 ( ENCODER_L_B_PIN & ENCODER_L_B_MASK ) != 0,
                 ( ENCODER_R_A_PIN & ENCODER_R_A_MASK ) != 0,
                 ( ENCODER_R_B_PIN & ENCODER_R_B_MASK ) != 0,
                 gEncoderCountL,
                 gEncoderCountR,
                 error, gLowMask, gHighMask );
        }

        // Tick rate is 100/sec so waiting for 50 waits for 1/2 sec

        for ( i = 0; i < 50; i++ )
        {
            WaitForTimer0Rollover();
            CheckSwitches();

#if 1
            if ( UART0_IsCharAvailable() )
            {
                char    ch = UART0_GetChar();

                if ( ch == ' ' )
                {
                    DebugKey();
                }
                else
                {
                    Log( "Read: '%c'\n", ch );
                }
            }
#endif
        }
    }

    return 0;
}
예제 #3
0
int main (void)
{
//	speed_t wrist;
//	speed_t finger;
	uint16_t fingerPosition;

	// initialize the hardware stuff we use
	InitTimer ();
	ADC_Init (ADC_PRESCALAR_AUTO);
	InitMotors ();
	InitTimerUART ();

	// set the LED port for output
	LED_DDR |= LED_MASK;

	// Flash the LED for 1/2 a second...
	ledOn ();
	ms_spin (500);
	ledOff ();

	Log ("*****\n");
	Log ("***** Bioloid Gripper Test\n");
	Log ("***** Copyright 2007 HUVrobotics\n");
	Log ("*****\n");

	SetMotorSpeed (SPEED_OFF, SPEED_OFF);

	fingerPosition = ADC_Read (7);
	Log ("Speed = 0... Position = %4d\n", fingerPosition);
	SetMotorSpeed (SPEED_OFF, 0);
	while (fingerPosition < MAX_FINGER)
	{
		fingerPosition = ADC_Read (7);
	}

	SetMotorSpeed (SPEED_OFF, SPEED_OFF);
	ms_spin (1000);

	fingerPosition = ADC_Read (7);
	Log ("Speed = 255... Position = %4d\n", fingerPosition);
	SetMotorSpeed (SPEED_OFF, 255);
	while (fingerPosition > MIN_FINGER)
	{
		fingerPosition = ADC_Read (7);
	}

	SetMotorSpeed (SPEED_OFF, SPEED_OFF);
	Log ("Done... Position = %4d\n", fingerPosition);

/*	ms_spin (1000);
	ledOn ();
	for (wrist = SPEED_OFF; wrist < 255; wrist++) {
		SetMotorSpeed (wrist, SPEED_OFF);
		ms_spin (25);}
	ms_spin (1000);
	ledOff ();
	SetMotorSpeed (SPEED_OFF, SPEED_OFF);
	ms_spin (1000);
	ledOn ();
	for (wrist = SPEED_OFF; wrist > 0; wrist--) {
		SetMotorSpeed (wrist, SPEED_OFF);
		ms_spin (25);}
	ms_spin (1000);
	ledOff ();
	SetMotorSpeed (SPEED_OFF, SPEED_OFF);
*/

	return 0;
}
예제 #4
0
int main(void)
{
    /* Initialize the SAM system */
    SystemInit();

	/* Initialize pins */
	Pin_Configuration();
	/* Initialize PWM generator */
	InitPWMController_MCLK(); 
	/* Initialize timers */
	Configure_Timers();
	
	InitMotors();
	
	/* Configre UART */
	configure_uart();
	
	sendString("###ON \n", 7);
	sendString("###Initializing\n", 16);
	
	
	
	/* Disable watchdog */
	WDT->WDT_MR |=  WDT_MR_WDDIS;

	//selfTest();
	
	// Set variables 
	initializeMotors = 0;
	flag12 = 0;
	
	// ----- TASK_1
	#if defined(TASK_1)
		while (1)
		{			
			if(getNewSpeed())
			{
				sendString("###New Speed\n", 14);
				newSpeed = 0;
				ControlledDrive(percentage_ST,percentage_DR);
				flag12 = 0;
			}
		}
		
	// ----- TASK_2
	#elif defined(TASK_2)
	
		while (1)
		{
			
			
		}
	
	// ----- TASK_3
	#elif defined(TASK_3)
		
		startStop_Camera = 0;
		
		while (1)
		{
			if(initializeMotors)
			{
				sendString("###Initializing\n", 16);
				InitMotors();
				initializeMotors = 0;
			}

			if(startStop_Camera)
			{ // Stop motors and ignore lidar values 
				WriteMotors(0,0);
			}
			else
			{ // Start and go based on lidar values
				
				if(getNewSpeed())
				{
					sendString("###New Speed\n", 14);
					newSpeed = 0;
					ControlledDrive(percentage_ST,percentage_DR);
					flag12 = 0;
				}
				
				//ForwardDrive();
			}
		}
	
	// ----- TASK_4
	#elif defined(TASK_4)
	
		while (1)
		{
			
		}	
	
	#endif

}
예제 #5
0
파일: maze.c 프로젝트: shameen/zomble
void maze(void)
{
	long i;
//long j;
	InitMotors();
	InitProx();
	LedClear();
	SetLed(4,1);
	SetStepsRight(1);
	SetStepsLeft(1);
	int left = 0;
	int right = 0;
//	int waitlonger = 0;
    int curious = 0;
	int distance;
	while(1){
		while (curious == 0) {
			int j;
			for(i=0;i<100000;i++) {asm("nop");}

			left = 300;
			right = 300;

			/*wait*/for(i=0;i<40000;i++) {asm("nop");}
			SetSpeedRight(right);
			SetSpeedLeft(left);

			if (GetProx(0) >= 600 || GetProx(7) >= 600){
				left= -300;
				right= 300;
				SetSpeedRight(right);
				SetSpeedLeft(left);
				for(i=0;i<20000;i++) {asm("nop");}
				curious = 1;
			}
			
			if( GetProx(2) >= 500 ){
				for(j=0; j<= 10000; j++){
					curious = 1;
				}
			}
		}

		while(curious==1){
			//too far from wall
			if(GetProx(2) < 400){
				//turn right, towards wall
				left = 300; 
				right = 100; 
				SetSpeedRight(right); 
				SetSpeedLeft(left); 
				/*wait*/for(i=0;i<40000;i++) {asm("nop");}
			}
			//wall in front and on the right
			while (GetProx(0) >= 500 || GetProx(7) >= 600 || GetProx(1) >= 350){
				//sharp turn left
				left = -300; 
				right = 300; 
				SetSpeedRight(right); 
				SetSpeedLeft(left); 
				/*wait*/for(i=0;i<40000;i++) {asm("nop");} 
			} 
			//too close to wall
			if (GetProx(2) > 600){ 
				//turn left
				left = 0; 
				right = 300; 
				SetSpeedRight(right); 
				SetSpeedLeft(left); 
				/*wait*/for(i=0;i<40000;i++) {asm("nop");} 
			} 	
			//enough for it to go straight forwards
			if ( GetProx(2) >= 400 && GetProx(0) < 500 && GetProx(7) < 600 && GetProx(1) < 350 && GetProx(6) <= 800 && GetProx(2) <= 600){ 
				//go forwards
				left = 300; 
				right = 300; 
				left = left * 0.9; 
				right = right * 0.9; 
				SetSpeedRight(right); 
				SetSpeedLeft(left); 
				/*wait*/for(i=0;i<40000;i++) {asm("nop");} 
			} 
			//
			if (GetProx(5) > 600 || GetProx(6) > 600 ){ 
				left = 300; 
				right = 0; 
				SetSpeedRight(right); 
				SetSpeedLeft(left); 
				/*wait*/for(i=0;i<40000;i++) {asm("nop");} 
			}
			//goal detection
			if (GetAmbientLight(0)<4000 || GetAmbientLight(7)<4000) {
				SetSpeedLeft(0);
				SetSpeedRight(0);
				int j;
				for (j=0;j<5;j++) {
					for (i=0;i<7;i++)
						SetLed(i,1);
					/*wait*/for(i=0;i<80000;i++) {asm("nop");}
					LedClear();
					/*wait*/for(i=0;i<80000;i++) {asm("nop");}
				}
				
				while (1);
			}
		}
	}		
}
예제 #6
0
int main(void){
	volatile uint32_t delay;
	int i;
	
	PLL_Init();
	Output_Init();
//	CAN0_Open();
	
	DisableInterrupts();
	
	SYSCTL_RCGCGPIO_R |= 0x20;
  while((SYSCTL_PRGPIO_R&0x0020) == 0){};// ready?
  Count = 0;                       // allow time to finish activating
//  GPIO_PORTF_DIR_R |= 0x04;        // make PF2 out (built-in LED)
//  GPIO_PORTF_AFSEL_R &= ~0x04;     // disable alt funct on PF2
//  GPIO_PORTF_DEN_R |= 0x04;        // enable digital I/O on PF2
//                                   // configure PF2 as GPIO
//  GPIO_PORTF_PCTL_R = (GPIO_PORTF_PCTL_R&0xFFFFF0FF)+0x00000000;
//  GPIO_PORTF_AMSEL_R = 0;          // disable analog functionality on PF
	
	GPIO_PORTF_LOCK_R = 0x4C4F434B;   // 2) unlock GPIO Port F
	GPIO_PORTF_CR_R = 0x1F;           // allow changes to PF4-0
	GPIO_PORTF_AMSEL_R = 0x00;        // 3) disable analog on
    
	GPIO_PORTF_DIR_R &= ~0x03;       // make PB6 in
  GPIO_PORTF_AFSEL_R |= 0x03;      // enable alt funct on PB6
  GPIO_PORTF_DEN_R |= 0x03;        // enable digital I/O on PB6
                                   // configure PB6 as T0CCP0
  GPIO_PORTF_PCTL_R = (GPIO_PORTF_PCTL_R&0xFFFFFF00)+0x00000077;
  GPIO_PORTF_AMSEL_R &= ~0xFF;     // disable analog functionality on PB6
		
	SYSCTL_RCGCGPIO_R  |= 0x01;
	delay = SYSCTL_RCGCGPIO_R;      // 2) allow time for clock to stabilize
  delay = SYSCTL_RCGCGPIO_R;
	GPIO_PORTA_DIR_R |= 0x01;  			// 3.11) make PA6 output
  GPIO_PORTA_AFSEL_R &= ~0x01; 		// 4.11) disable alternate function on PA6
  GPIO_PORTA_DEN_R |= 0x01;  			// 5.11) enable digital I/O on PA6
  GPIO_PORTA_AMSEL_R = 0; 				// 6.11) disable analog functionality on PA6	
	Switch_Init();
	
	Init_Timer4A();
	Timer4A_Wait(80000000); //wait 1 sec
	Init_Timer5A(80000);
	ADC0_InitTimer3ATriggerSeq3PD3(6000);
	ADC0_InitTimer3BTriggerSeq2PD2(5500);
	
	TimerCapture_Init(UserTask2);
	InitMotors();
	
	EnableInterrupts();
	
	
	while(1) {
		getSensorValues();
		printSensorValues(0);
		if(buttonL) {
			printSensorValues(6);
			ControlMotors(35000,35000);
			ST7735_SetCursor(0,8);ST7735_OutString("Buttonback");
			buttonL = 0;
			for(i = 0; i < 1440000; i++);
		}
		if(buttonR) {
			printSensorValues(6);
			ControlMotors(35000,35000);
			ST7735_SetCursor(0,1);ST7735_OutString("ButtonR");
			buttonR = 0;
			for(i = 0; i < 1440000; i++);
		}
		
		if (Ping1 < 18) {
			//ControlMotors(40000, 40000);
			ST7735_SetCursor(0,1);
			ST7735_OutString("Stopped");
			printSensorValues(3);
//			while(1) {
				ST7735_SetCursor(0,1);ST7735_OutString("Stopped");
				getSensorValues();
				printSensorValues(0);
//			}
			if(IR_L > IR_R) {
				ControlMotors(35000,35000);
				ST7735_SetCursor(0,8);ST7735_OutString("BL");
			}
			else {
				ControlMotors(35000,35000);
				ST7735_SetCursor(0,8);ST7735_OutString("BR");
			}
			for(i = 0; i < 1440000; i++);
		}
		else if (((Ping2 > 80) != (Ping3 > 80)) && (Ping1 < 40)) {
			if (Ping2>80) {
				//spot turn right
				ControlMotors(20000, 60000);
			}
			if (Ping3>80) {
				//spot turn left
				ControlMotors(60000, 20000); 
			}
		//	ControlMotors(40000,40000);
		}
		else if(IR_L < 1500 && IR_R < 1500) {
			ControlMotors(70000,65000);
			ST7735_SetCursor(0,8);ST7735_OutString("FO");
		}
		else if(IR_L < IR_R) {
			ControlMotors(70000,50000);
			ST7735_SetCursor(0,8);ST7735_OutString("LE");
		}
		else if(IR_R < IR_L) {
			ControlMotors(70000,72000);
			ST7735_SetCursor(0,8);ST7735_OutString("RI");
		}
	}
}
예제 #7
0
파일: SCOPE.C 프로젝트: MelBartels/scope
void main( int argc, char** argv)
{
	//struct Position Pos;
	int Ix;

	strcpy( ConfigFile, "config.dat");

	Parity = NoneParity;
	DataBits = 8;
	StopBits = 1;

	CalledByGuideFlag = No;
	KeepGoingFlag = No;
	ReadSlewFlag = No;
	StartScrollFlag = No;

	/* if '-k' and '-s' (after full init): slew to Ra, Dec in slew.dat and keep going, exiting when
	desired, writing slew_out.dat file;
	if '-k' (before full init): (no slew.dat), keep going until centered on init position, write
	slew_out.dat file and exit;
	if '-s': slew to Ra, Dec in slew.dat and exit, writing slew_out.dat file;
	if no '-k' and no '-s': write slew_out.dat file and exit;

	if -c, then use following string as configuration file name, ie scope.exe -c config.dat will
	result in config.dat being used

	if -x, then use following string as scroll file name, and execute scroll file upon program
	startup, ie scope.exe -x nan.scr	will cause nan.scr to be loaded and started */

	/* argv[0] is name of executing program */
	for( Ix = 1; Ix < argc; Ix++)
		if( argv[Ix][0] == '-')
			if( strcmpi( &argv[Ix][1], "GUIDE") == 0)
				CalledByGuideFlag = Yes;
			else if( argv[Ix][1] == 'k')
				KeepGoingFlag = Yes;
			else if( argv[Ix][1] == 's')
				ReadSlewFlag = Yes;
			else if( (argv[Ix][1] == 'c' || argv[Ix][1] == 'C') && Ix < argc-1)
				strcpy( ConfigFile, argv[Ix+1]);
			else if( (argv[Ix][1] == 'x' || argv[Ix][1] == 'X') && Ix < argc-1)
			{
				strcpy( ScrollFilename, argv[Ix+1]);
				StartScrollFlag = Yes;
			}

	InitCommonVars();
	ReadConfig();

	/*
		Pos.Ra = Pos.Dec = 0;
		applyCorrectionsFromDataFileCoordYearToEpochNow(&Pos);
		printf("\n%f %f %f %f %f %f %f %f", Pos.Precession.A*RadToArcsec, Pos.Precession.Z*RadToArcsec,
		Pos.Nutation.A*RadToArcsec, Pos.Nutation.Z*RadToArcsec, Pos.AnnualAberration.A*RadToArcsec,
		Pos.AnnualAberration.Z*RadToArcsec, Pos.Ra*RadToArcsec, Pos.Dec*RadToArcsec);
		ContMsgRoutine();
	*/
	/*
		HsRecFile = fopen( HsRecFilename, "w");
		if( HsRecFile == NULL)
			BadExit( strcat( "Could not create ", HsRecFilename));
		HsRecIx = 0;
	*/

	/*
		InitTimes( DisplayOpeningMsgs, Tz, DST, LongitudeDeg);
		InitConvert();
		TestConvert();
		getch();
		TestAltAltAzTrack();
	*/


	if( DisplayOpeningMsgs)
	{
		printf( "\nCopyright BBAstroDesigns Inc. 2009\n");
		printf( "\nLIMITED WARRANTY This software is provided ``as is'' and any express or");
		printf( "\nimplied warranties, including, but not limited to, the implied warranties");
		printf( "\nof merchantability and fitness for a particular purpose are disclaimed.");
		printf( "\nIn no event shall BBAstroDesigns be liable for any direct, indirect,");
		printf( "\nincidental, special, exemplary, nor consequential damages (including, but");
		printf( "\nnot limited to, procurement of substitute goods or services, loss of use,");
		printf( "\ndate, or profits, or business interruption) however caused and on any");
		printf( "\ntheory of liability, whether in contract, strict liability, or tort");
		printf( "\n(including negligence or otherwise) arising in any way out of the use of");
		printf( "\nthis software, even if advised of the possibility of such damage.\n");
		printf( "\nThis software licensed under the GNU GENERAL PUBLIC LICENSE. You may");
		printf( "\ndistribute this software per the GNU GPL. See the enclosed gpl.txt.\n\n");
		ContMsgRoutine();
	}

	if( DisplayOpeningMsgs)
		printf( "\ncalled by guide: %d, keep_going: %d, read slew.dat file %d",
		CalledByGuideFlag, KeepGoingFlag, ReadSlewFlag);

	/* if( strcmpi( TestString, "TestSerial") == 0)
	{
		InitSerial( EncoderComPort, EncoderBaudRate, Parity, DataBits, StopBits);
		TestSerial( EncoderComPort);
		CloseSerial( EncoderComPort);
	} */
	/* else if( strcmpi( TestString, "TestVideo") == 0)
	{
		InitVideo( DisplayOpeningMsgs);
		TestVideo();
	} */
	/* else if( strcmpi( TestString, "TestATimes") == 0)
	{
		InitTimes( DisplayOpeningMsgs, Tz, DST, LongitudeDeg);
		TestTimes();
	} */
	/* else if( strcmpi( TestString, "TestParallelPort") == 0)
	{
		InitPPort();
		TestPPort();
		ClosePPort();
	} */
	/* else if( strcmpi( TestString, "TestRefract") == 0)
	{
		InitRefract();
		TestRefract();
	} */
	/* else if( strcmpi( TestString, "TestMouse") == 0)
	{
		TestMouse();
	} */
	/* else if( strcmpi( TestString, "TestEncoders") == 0)
	{
		InitSerial( EncoderComPort, EncoderBaudRate, Parity, DataBits, StopBits);
		InitEncoders();
		TestEncoders();
		CloseSerial( EncoderComPort);
	} */
	/* else if( strcmpi( TestString, "TestHandpad") == 0)
	{
		InitPPort();
		InitializeHandpad();
		TestHandpad();
		ClosePPort();
	} */
	/* else if( strcmpi( TestString, "TestConversion") == 0)
	{
		InitTimes( DisplayOpeningMsgs, Tz, DST, LongitudeDeg);
		InitConvert();
		TestConvert();
	} */
	/* else if( strcmpi( TestString, "TestAltOffset") == 0)
	{
		InitTimes( DisplayOpeningMsgs, Tz, DST, LongitudeDeg);
		InitConvert();
		TestAltOffset();
	} */
	/* else if( strcmpi( TestString, "TestIACA") == 0)
	{
		InitTimes( DisplayOpeningMsgs, Tz, DST, LongitudeDeg);
		TestIACA();
		InitIACA();
	} */
	/* else if( strcmpi( TestString, "WritePWMValues") == 0)
	{
		InitTimes( DisplayOpeningMsgs, Tz, DST, LongitudeDeg);
		InitVideo( DisplayOpeningMsgs);
		InitPPort();
		InitMotors();
		WritePWMValues();
		CloseSteppers();
		ClosePPort();
	}
	else */
	{
		if( strcmpi( TestString, "NoTest") != 0
		&& strcmpi( TestString, "PreloadGuidexx.dat") != 0
		&& strcmpi( TestString, "Track") != 0)
		{
			if( DisplayOpeningMsgs)
				printf( "\nsetting unrecognized TestString to 'NoTest'");
			strcpy( TestString, "NoTest");
		}
		InitSerial( EncoderComPort, EncoderBaudRate, Parity, DataBits, StopBits);
		InitEncoders();
		InitMouseControl();
		InitTimes( DisplayOpeningMsgs, Tz, DST, LongitudeDeg);
		InitVideo( DisplayOpeningMsgs);
		InitPPort();
		InitializeHandpad();
		InitMotors();
		InitConvert();
		InitRefract();
		InitPEC();
		InitGuide();
		if( strcmpi( TestString, "PreloadGuidexx.dat") == 0)
		{
			LoadGuideAlts();
			LoadGuideAzs();
		}
		InitIACA();
		InitLX200Input();
		InitHPEvent();
		if( !CalledByGuideFlag ||
		(CalledByGuideFlag && (KeepGoingFlag || ReadSlewFlag)))
		{
			InitKBEvent();
			if( ReadSlewFlag)
				InputEquatSlewDat();
			if( StartScrollFlag)
				LoadScrollFileFromFile();
			if( strcmpi( TestString, "Track") == 0)
				Start2MotorTrackWithDefaultValues();
			while( !QuitFlag)
			{
				SequentialTaskController();
				/* GrandTourFlag used to flag next object: set in ProcessHPEventsModeSwitch() */
				if( GrandTourLoaded && GrandTourFlag)
					ProcessGrandTour();
				else
					if( ScrollLoaded && ScrollFlag)
						ProcessScroll();
					else
						if( HPPolarAlignLoaded && HPPolarAlignFlag)
							ProcessHPPolarAlign();
						else
						{
							if( UseMouseFlag && ProcessMouseEvent())
								;
							else
								if( KeyStroke)
									ProcessKBEvents();
								else
									ProcessHPEvents();
						}
			}
			CloseKBEvent();
			if( DisplayOpeningMsgs)
			{
				AskAndWriteConfig();
				WriteLogFile();
			}
		}
		CloseSteppers();
		ClosePPort();
		CloseEncoderResetLogFile();
		CloseSerial( EncoderComPort);
		CloseSerial( LX200ComPort);
		if( CalledByGuideFlag)
			WriteAltazSlewOutFile();
		CloseMouseControl();
	}

	/*
		for( Ix = 0; Ix < HsRecSize; Ix++)
			fprintf( HsRecFile, "%8ld   %8ld\n", HsRec[Ix].A, HsRec[Ix].Z);
		// first position is index 0
		fprintf( HsRecFile, " last entry in circular queue at position %d", HsRecIx);
		fclose( HsRecFile);
	*/
}
예제 #8
0
파일: aggressive.c 프로젝트: shameen/zomble
void aggressive(void)
{
	long i = 0;			//loop counter, mainly for waiting
	int left = 0;		//speed of left wheel
	int right = 0;		//speed of right wheel
	int waitlonger = 0;	//if set to 1, will wait a bit longer in between each loop
	
	//setup
	InitMotors();
	InitProx();
	LedClear();
	//start with body light on and moving forwards
	SetBodyLed(1);
	SetStepsRight(1);
	SetStepsLeft(1);
	SetSpeedRight(100);
	SetSpeedLeft(100);
	
	while (1) {
		//smooth
		left = left * 0.9;
		right = right * 0.9;
		left+=38;
		right+=38;

		/*light up led if something is near it in that direction*/
		/*and also adjusts wheel speed depending on which ones are lit up*/
		for (i=0;i<7;i++) {
			//~1000 is about to touch
			//any less than 500 and it starts having false positives
			if (GetProx(i)>=500 || GetAmbientLight(i)<4000) {	
				switch (i) {
					//left side sensors
					case 0:left+=150;right+=100;break;
					case 1:left+=100;right-=100;break;
					case 2:left+=300;right-=400;break;
					//back sensors
					case 3:left+=400;right-=300;break;
					//case 4:left-=300;right+=400;break;
					case 4:left+=400;right-=300;break;//testing left turn on both
					//right side sensors
					case 5:left-=400;right+=300;break;
					case 6:left-=100;right+=100;break;
					case 7:left+=150;right+=100;break;
				}
				SetLed(i,1);
			}
			else SetLed(i,0);
		}

		/*
		//back sensors
		if (GetProx(3)>800 || GetProx(4)>800) {
			//spin around in panic
			left = 700;
			right = -800;
			waitlonger = 1;
			
		*/
		
		//front sensors
		//todo: blindly charges straight forwards, is that what we want?
		//		if not, then adjust slightly based on sensors
		if (GetProx(0)>800 || GetProx(7)>800) {
			//light up
			for (i=0;i<7;i++) {
				SetLed(i,1);
			}
			SetFrontLed(1);
			//push for a while
			SetSpeedRight(1000);
			SetSpeedLeft(1000);
			/*wait*/for(i=0;i<800000;i++) {asm("nop");}
			
			LedClear();
			SetFrontLed(0);
			//reverse after pushing
			left = -900;
			right = -900;
			waitlonger = 1;
		}
		
		//max speed
		if (left>1000) {
			left = 1000;
		}
		if (right>1000) {
			right = 1000;
		}
		if (left<-1000) {
			left = -1000;
		}
		if (right<-1000) {
			right = -1000;
		}
		
		/*wait*/for(i=0;i<40000;i++) {asm("nop");}
		SetSpeedRight(right);
		SetSpeedLeft(left);
		
		if (waitlonger == 1) {
			/*wait*/for(i=0;i<100000;i++) {asm("nop");}
			waitlonger = 0;
		}
	}
}