int main(void) { WordVal src_addr_init = {SRC_ADDR}; WordVal src_pan_id_init = {SRC_PAN_ID}; WordVal dst_addr_init = {DST_ADDR}; SetupClock(); SwitchClocks(); SetupPorts(); batSetup(); swatchSetup(); radioInit(src_addr_init, src_pan_id_init, RXPQ_MAX_SIZE, TXPQ_MAX_SIZE); radioSetChannel(MY_CHAN); //Set to my channel macSetDestAddr(dst_addr_init); dfmemSetup(); unsigned char memsize; memsize = dfmemGetChipSize(); xlSetup(); gyroSetup(); mcSetup(); cmdSetup(); //senSetup(); adcSetup(); pidSetup(); steeringSetup(); //radioReadTrxId(id); LED_RED = 1; LED_BLUE = 0; LED_YELLOW = 0; //while(1); if(phyGetState() == 0x16) { LED_GREEN = 1; } //print("Ready"); //readDFMemBySample(5); while(1) { cmdHandleRadioRxBuffer(); //Simple idle ; reduces idle current to 70 mA // TODO (abuchan, apullin, fgb) : Idle() causes unexpected behavior //if(radioIsRxQueueEmpty()){ // Idle(); //} } }
task main() { pidSetup(); // initialise arrays: // initialiseWeights(); // ... // every few seconds stop and measure distance with sonar (z) // update all the particles, weights, etc: // // updateWeightArray(z); // normalise(); // updateCumulativeWeightArray(); // resample(); // MOVE!!! }
task main() { pidSetup(); StartTask(checkObstacles); wait1Msec(500); while(true){ wait1Msec(400); if(handleObstacleLeft){ StopTask(obstacleHandler); StartTask(obstacleHandler); } else if(handleObstacleRight){ StopTask(obstacleHandler); StartTask(obstacleHandler); } else if(!passingByObstacle){ moveToTarget(); } } }
int main() { // Processor Initialization SetupClock(); SwitchClocks(); SetupPorts(); sclockSetup(); LED_1 = 1; LED_2 = 1; LED_3 = 1; // Message Passing fun_queue = carrayCreate(FUN_Q_LEN); cmdSetup(); // Radio setup radioInit(RADIO_RXPQ_MAX_SIZE, RADIO_TXPQ_MAX_SIZE); radioSetChannel(RADIO_CHANNEL); radioSetSrcAddr(RADIO_SRC_ADDR); radioSetSrcPanID(RADIO_PAN_ID); uart_tx_packet = NULL; uart_tx_flag = 0; //uartInit(&cmdPushFunc); tactileInit(); // Need delay for encoders to be ready delay_ms(100); amsEncoderSetup(); mpuSetup(); tiHSetup(); dfmemSetup(); telemSetup(); adcSetup(); pidSetup(); LED_1 = 0; LED_3 = 1; while(1){ // Send outgoing radio packets radioProcess(); /* // Send outgoing uart packets if(uart_tx_flag) { uartSendPacket(uart_tx_packet); uart_tx_flag = 0; }*/ checkTactileBuffer(); // move received packets to function queue while (!radioRxQueueEmpty()) { // Check for unprocessed packet rx_packet = radioDequeueRxPacket(); if(rx_packet != NULL) { cmdPushFunc(rx_packet); } } // process commands from function queue while(!carrayIsEmpty(fun_queue)) { rx_packet = carrayPopHead(fun_queue); unsigned int rx_src_addr = rx_packet->src_addr.val; if(rx_packet != NULL) { rx_payload = macGetPayload(rx_packet); if(rx_payload != NULL) { rx_function = (test_function)(rx_payload->test); if(rx_function != NULL) { LED_2 = ~LED_2; (rx_function)(payGetType(rx_payload), payGetStatus(rx_payload), payGetDataLength(rx_payload), payGetData(rx_payload), rx_src_addr); } } ppoolReturnFullPacket(rx_packet); } } } return 0; }