int mainControlInit(void)
{
ROTATION_DIAMETER = sqrt(pow(WHEELS_DISTANCE, 2) + pow(WHEELS_SPACING, 2));
motorsInit();
encodersInit();
mulimeterInit();
telemetersInit();
speedControlInit();
positionControlInit();
wallFollowControlInit();
lineFollowControlInit();
transfertFunctionInit();
wallSensorInit();
adxrs620Init();
speed_params.initial_speed = 0;
control_params.wall_follow_state = 0;
control_params.position_state = 0;
control_params.speed_state = 0;
control_params.line_follow_state = 0;
return MAIN_CONTROL_E_SUCCESS;
}
int mainControlInit(void)
{
pid_loop.start_state = FALSE;
ROTATION_DIAMETER = sqrt(pow(WHEELS_DISTANCE, 2) + pow(WHEELS_SPACING, 2));
motorsInit();
encodersInit();
telemetersInit();
adxrs620Init();
speedControlInit();
positionControlInit();
wallFollowControlInit();
lineFollowControlInit();
transfertFunctionInit();
positionControlSetPositionType(ENCODERS);
mainControlSetFollowType(NO_FOLLOW);
mainControlSetMoveType(STRAIGHT);
control_params.wall_follow_state = 0;
control_params.line_follow_state = 0;
return MAIN_CONTROL_E_SUCCESS;
}
int main()
{
int wait;
// Set up port for start switch
lcdInit();
// Set up motors and encoders
motorInit();
encodersInit();
servoInit();
// Set up IRs
gp2d12Init(PIN_HEAD_IR);
digitalSetDirection(PIN_IR,AVRRA_INPUT);
digitalSetData(PIN_IR,AVRRA_LOW);
// Set up OP599A - analog already initialized
digitalSetDirection(PIN_PHOTO,AVRRA_INPUT);
digitalSetData(PIN_PHOTO,AVRRA_LOW);
// Set up fan & test
digitalSetData(PIN_FAN, AVRRA_LOW);
digitalSetDirection(PIN_FAN, AVRRA_OUTPUT);
digitalSetData(PIN_FAN, AVRRA_HIGH);
delayms(25);
digitalSetData(PIN_FAN, AVRRA_LOW);
lcdPrint("Wait... ");
wait = digitalGetData(PIN_START);
while(wait) {
// standard delay ..
delayms(10);
wait = digitalGetData(PIN_START);
}
// Start our map
mapInit();
lcdClear();
PrintHeading(lheading,nStart);
sei();
// Run Behaviors
while(1) {
// update odometer
while(rCount > COUNTS_CM(1) ) {
// odometer is in CM
odometer = odometer - 1;
rCount = rCount - COUNTS_CM(1);
}
// now run behaviors
if(arbitrate()>0) {
// let motors wind down
delayms(500);
clearCounters;
plan();
clearCounters;
}
}
}
int main()
{
//SYSTEMConfigPerformance(SYS_FREQ);
// Configure the device for maximum performance but do not change the PBDIV
// Given the options, this function will change the flash wait states, RAM
// wait state and enable prefetch cache but will not change the PBDIV.
// The PBDIV value is already set via the pragma FPBDIV option above..
SYSTEMConfig(SYS_FREQ, SYS_CFG_WAIT_STATES | SYS_CFG_PCACHE);
PORTFbits.RF1 = 0;
//Function that initializes all of the required I/O
initIO();
//Initialize all of the LED pins
ledinit();
adcConfigureAutoScan();
timer1Init();
encodersInit();
motorinit();
PWMinit();
motorRstop();
motorLstop();
leftspeed = 310;
rightspeed = 300;
while(PORTDbits.RD3 == 0)
{}
for(i = 0; i < 5000000; i++)
{
}
setpwmR(rightspeed);
setpwmL(leftspeed);
motorRfwd();
motorLfwd();
//turnright();
//turnleft();
while (1)
{
if(readpins(0) > 300) //left
{
setpwmR(100);
for(i=0;i<800;i++);
/*
if(leftspeed < 315)
leftspeed++;
if(rightspeed > 275)
rightspeed--;
*/
setpwmR(rightspeed);
setpwmL(leftspeed);
motorRfwd();
motorLfwd();
}
//for(i = 0; i < 500; i++){};
//if (channel13 < 600)
if(readpins(2) > 600) //right
{
setpwmL(100);
for(i=0;i<800;i++);
/*
if(leftspeed > 265)
leftspeed--;
if(rightspeed < 325)
rightspeed++;
*/
setpwmR(rightspeed);
setpwmL(leftspeed);
motorRfwd();
motorLfwd();
}
//for(i = 0; i < 500; i++){};
//if (channel13 < 900)
if(readpins(1) > 900) //middle
{
setpwmR(0);
setpwmL(0);
motorRstop();
motorLstop();
turnright();
}
//for(i = 0; i < 1500; i++){};
}
return (EXIT_SUCCESS);
}
