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