//###################################################################################
void Chegada_Farol ()
{
int detectLine = 0;
if( readLineSensors(0) > 5)
{
while(readLineSensors(0) > 10 && detectLine <= 5)
{
detectLine ++;
}
if(detectLine >= 5)
{
estado = 2;
while(readLineSensors(0) > 5)
{
Rodar_Sobre_Si();
}
}
}
}
//############################################################################################################################################################3
void Chegada_Farol ()
{
if (readLineSensors(0) == 1 )//||readLineSensors(1) == 1 || readLineSensors(2) == 1 || readLineSensors(3) == 1)
{
wait(2); //para se ver melhor depois
Stop_robot();
estado = 0;
printf("Cheguei ao farol CARALHO!!!!!!!!!!!!!!!!!\n\n");
}
}
int main(void){
initPIC32();
closedLoopControl( false );
setVel2(0, 0); //Stop Engines
while(1){
printf("Press start to continue\n");
while(!startButton());
do{
waitTick40ms();
int ground = readLineSensors(0);
if(NEAR_LEFT_BLACK(ground))
setVel2(0, 50);
else if(NEAR_RIGHT_BLACK(ground))
setVel2(50, 0);
else if(FAR_LEFT_BLACK(ground))
setVel2(-50, 50);
else if (FAR_RIGHT_BLACK(ground))
setVel2(50, -50);
else if(GROUND_CENTER_BLACK(ground))
setVel2(55, 60);
else
setVel2(50, -50);
}while(!stopButton());
}
return 0;
}
void readGroundSensor(){
sensor_sensorReadings.groundSensor = readLineSensors(0);
sensor_sensorReadings.atBeaconArea = (sensor_sensorReadings.groundSensor > 0);
}
/*****************************************************
* Timer 5 is used to provide the main system clock
* It generates an interrupt every millisecond
*
* This ISR is where most of the actual work gets done.
* with the sensors running and the straights profiler
* active, this interrupt takes about 220us of which
* 120us is processing the sensors
*****************************************************/
void _ISR SYSTIM_INTERRUPT(void)
{
int pidOut;
unsigned char rxBytes;
/* reset the interrupt flag */
SYSTIM_IF = 0;
//LED_ON;
tickCount++;
millisecondCount--;
commCount++;
if(!GDO0)
{
rxBytes = CC2500_receive_packet();
if(rxBytes>PACKET_LEN)
{
CC2500_idle_mode();
CC2500_clear_rx_fifo();
CC2500_clear_tx_fifo();
CC2500_receive_mode();
commEnabled = FALSE;
}
else
commEnabled = TRUE;
if(newPacket)
{
deassamble_packet();
getFellowCoveredSqrs();
my.obzClear = OBZ_clear();
if(inOBZ(fellow.location))
{
LED_ON;
}
else
{
LED_OFF;
}
dataUpdated = TRUE;
newPacket = FALSE;
noCommCount = 0;
}
else
noCommCount++;
}
if((commCount>=6)&&commEnabled)
{
assamble_packet();
if(!GDO0)
{
CC2500_transmit_packet();
commCount = 0;
}
}
else if(commCount==4)
{
CC2500_idle_mode();
__delay_us(1);
CC2500_clear_rx_fifo();
__delay_us(1);
CC2500_clear_tx_fifo();
__delay_us(1);
CC2500_receive_mode();
}
if(!(tickCount&1))
readCubeSensors();
if(tickCount>300000L)
{
motorsOff();
sensorsOff();
stopSystemTimer();
LED_ON;
}
doButtons();
readLineSensors();
readCounters();
doProfiler();
pidOut = doPID( &left_PID_param);
if( pidOut < -MOTORS_MAX_DC )
pidOut = -MOTORS_MAX_DC;
else if( pidOut > MOTORS_MAX_DC )
pidOut = MOTORS_MAX_DC;
motorsLeftSetDutyCycle(pidOut);
pidOut = doPID( &right_PID_param);
if( pidOut < -MOTORS_MAX_DC )
pidOut = -MOTORS_MAX_DC;
else if( pidOut > MOTORS_MAX_DC )
pidOut = MOTORS_MAX_DC;
motorsRightSetDutyCycle(pidOut);
//LED_OFF;
}
