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