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