// main int main(void) { wdt_enable(WDTO_1S); hardwareInit(); usbInit(); sei(); unsigned int adcValue,replymask,replyshift,replybyte; while(1) { uchar i = 0; wdt_reset(); usbPoll(); PORTD ^= (1 << 6); //jump to bootloader if jumper is HIGH if(bit_is_clear(PIND, 5)) { startBootloader(); } for(i = 0; i < ADC_CHANNELS; i++) { //adcValue = adc_read(ADC_PRESCALER_32, ADC_VREF_AVCC, i); adcValue = i; usb_reply[i] = adcValue >> 2; replybyte = 16 + (i / 4); replyshift = ((i % 4) * 2); replymask = (3 << replyshift); usb_reply[replybyte] = (usb_reply[replybyte] & ~replymask) | (replymask & (adcValue << replyshift)); _delay_us(ADCDELAY); } usb_reply[20] = PINC; } return 0; }
void computer_control_task() { hardwareInit(); engineSetSpeed(300); while (1) { if ((savedTime + 100) < get_ms()) { if (flag) { engineSetSpeed(300); flag = 0; } else { engineSetSpeed(200); flag = 1; } savedTime = get_ms(); } } }
int main(void) { wdt_enable(WDTO_1S); // watchdog status is preserved on reset hardwareInit(); usbInit(); usbDeviceDisconnect(); // fake USB disconnect for > 250 ms for( uint8_t i=255; i>0; i-- ) { wdt_reset(); _delay_ms(1); } usbDeviceConnect(); sei(); for(;;) { // main event loop wdt_reset(); usbPoll(); } return 0; }
int main(void) { wdt_enable(WDTO_1S); odDebugInit(); hardwareInit(); usbInit(); intr3Status = 0; sendEmptyFrame = 0; sei(); for(;;){ /* main event loop */ wdt_reset(); usbPoll(); uartPoll(); #if USB_CFG_HAVE_INTRIN_ENDPOINT3 /* We need to report rx and tx carrier after open attempt */ if(intr3Status != 0 && usbInterruptIsReady3()){ static uchar serialStateNotification[10] = {0xa1, 0x20, 0, 0, 0, 0, 2, 0, 3, 0}; if(intr3Status == 2){ usbSetInterrupt3(serialStateNotification, 8); }else{ usbSetInterrupt3(serialStateNotification+8, 2); } intr3Status--; } #endif } return 0; }
int main(void) { timeCount = 0; hardwareInit(); uart0_init(BAUD_SETTING); /* Initialise timer to divide by 1025, giving 32ms time tick */ timer0Init(0,5); rfm12_init(); wdtInit(); sei(); indx = 0; for(;;) { wdt_reset(); uint8_t sendMessage = false; uint16_t character = uart0_getc(); /* Wait for a serial incoming message to be built. */ if (character != UART_NO_DATA) { inBuf[indx++] = (uint8_t)(character & 0xFF); /* Signal to transmit if a CR was received, or string too long. */ sendMessage = ((indx > MAX_MESSAGE) || (character == 0x0D)); } /* Send a transmission if message is ready to send, or something was received and time waited is too long. */ if (sendMessage || (timeCount++ > TIMEOUT)) { /* Wait for the transmit buffer to be freed and message loaded. */ if ((indx > 0) && (rfm12_tx(indx, 0, inBuf) != RFM12_TX_OCCUPIED)) indx = 0; timeCount = 0; } rfm12_tick(); /* If an RF incoming message has been received, take it from the buffer one character at a time and transmit via the serial port. */ if (rfm12_rx_status() == STATUS_COMPLETE) { uint8_t *bufferContents = rfm12_rx_buffer(); uint8_t messageLength = rfm12_rx_len(); uint8_t i; for (i=0;i<messageLength;i++) { uart0_putc(bufferContents[i]); } /* Clear the "in use" status of the receive buffer to be available for rfm12lib. */ rfm12_rx_clear(); } } }
void hwInit() { taskRunning = 0; *(portNVIC_SYSPRI2) |= portNVIC_PENDSV_PRI; hardwareInit(); lwip_init(); return; }
int main(){ //setPrintLevelInfoPrint(); setPrintLevelWarningPrint(); //setPrintLevelNoPrint(); hardwareInit(); RunEveryData loop = {0.0,2000.0}; while(1){ if (_RF5==1){ setPrintLevelErrorPrint(); p_int_E(0);print_E(" Reset Button Pressed from loop"); SetColor(1,1,1); U1CON = 0x0000; DelayMs(100); Reset(); } if(RunEvery(&loop)>0){ // clearPrint(); // printCartesianData(); } if( printCalibrations == false && GetPIDCalibrateionState(linkToHWIndex(0))==CALIBRARTION_DONE&& GetPIDCalibrateionState(linkToHWIndex(1))==CALIBRARTION_DONE&& GetPIDCalibrateionState(linkToHWIndex(2))==CALIBRARTION_DONE ){ printCalibrations = true; int index=0; for(index=0;index<3;index++){ int group = linkToHWIndex(index); println_E("For Axis ");p_int_E(group); print_E(" upper: ");p_int_E(getPidGroupDataTable(group)->config.upperHistoresis); print_E(" lower: ");p_int_E(getPidGroupDataTable(group)->config.lowerHistoresis); print_E(" stop: ");p_int_E(getPidGroupDataTable(group)->config.stop); } startHomingLinks(); //Print_Level l= getPrintLevel(); //setPrintLevelInfoPrint(); printCartesianData(); int i; for(i=0;i<numPidMotors;i++){ printPIDvals(i); } //setPrintLevel(l); } bowlerSystem(); } }
//***************************************************************************** // Function: Initialise // Arguments: None // Action: Program Initialisation // Returns: None //***************************************************************************** void initialise( void ) { hardwareInit(); timerInit(); // eventHandlerInit(); // motorControlInit(); // batteryMonitorInit(); // soundsInit(); usageCounterInit(); interruptsInit(); configInit(); // asm("SLEEP"); }
int main(void) { hardwareInit(); initLEDs(); setOutputBit(&kOrangeLED, 1); sei(); USB_Init(); while (1) { HID_Device_USBTask(&mouseHIDInterface); USB_USBTask(); } }
int main(void) { uchar key, lastKey = 0, keyDidChange = 0; uchar idleCounter = 0; hardwareInit(); sei(); for (;;) /* main event loop */ { wdt_reset(); usbPoll(); key = keyPressed(); if (lastKey != key) { lastKey = key; keyDidChange = 1; } if (TIFR & (1<<TOV0)) /* ~63 ms timer */ { TIFR = 1<<TOV0; /* clear overflow */ if (idleRate != 0) { if (idleCounter >= OVERFLOWS_4MS) { idleCounter -= OVERFLOWS_4MS; } else { /* USB HID poll timer reached * send current state regardless of real key change */ idleCounter = idleRate; keyDidChange = 1; } } } if (keyDidChange && usbInterruptIsReady()) { keyDidChange = 0; /* use last key and not current key status in order to avoid lost changes in key status. */ buildReport(lastKey); usbSetInterrupt(reportBuffer, sizeof(reportBuffer)); } } return 0; }
int main (void) { Wall wall; Heli heli; Landscape landscape; hardwareInit(); initLandscape(&landscape, 70); heli_init(&heli); wall_init(&wall, 10, getYRoof(&landscape, 159), getYBottom(&landscape, 159) ); while(1) { //DelayMs(100); heli_clear(&heli); if(DIORead(USW0)) { DPRINT("up\n"); heli_update(&heli, 0.1, 100); // - means up } else { DPRINT("down\n"); heli_update(&heli, -0.1, 100); // + means down } landscapeFlow(&landscape, 70); wall_clear(&wall); wall_update(&wall, 100); if(heli_check_collision(&heli, wall.x, wall.y1, wall.x, wall.y2)) { heli_init(&heli); wall_init(&wall, 10, getYRoof(&landscape, 159), getYBottom(&landscape, 159) ); } if(touch(&landscape, heli.x, heli.y+heliRadius)) { heli_init(&heli); wall_init(&wall, 10, getYRoof(&landscape, 159), getYBottom(&landscape, 159) ); } if(touch(&landscape, heli.x, heli.y-heliRadius)) { heli_init(&heli); wall_init(&wall, 10, getYRoof(&landscape, 159), getYBottom(&landscape, 159) ); } wall_draw(&wall); heli_draw(&heli); } }
int main(void) { wdt_enable(WDTO_2S); /* Enable watchdog timer 2s */ hardwareInit(); /* Initialize hardware (I/O) */ usbInit(); /* Initialize USB stack processing */ Setup_init(); speed_init(); PWM_init(); KeyScan_init(); switch (Setup_key12LED){ case Setup_key12LED_Always: case Setup_key12LED_OftenOn: PWM_setOutputLevel(0,PWM_TotalLevel); PWM_setOutputLevel(1,PWM_TotalLevel); break; case Setup_key12LED_Never: case Setup_key12LED_OftenOff: PWM_setOutputLevel(0,0); PWM_setOutputLevel(1,0); } sei(); /* Enable global interrupts */ WorkMode_set(WorkMode_Unused); for(;;){ /* Main loop */ wdt_reset(); /* Reset the watchdog */ usbPoll(); if(KeyScan_keyChanged && usbInterruptIsReady()){ KeyScan_keyChanged = 0; buildReport(); usbSetInterrupt(reportBuffer, sizeof(reportBuffer)); }else{ if(TIFR0&(1<<TOV0)){ TIFR0 |= 1<<TOV0; PWM_Generator(); } } } return 0; }
int main(void) { unsigned long tmr1; int retVal; hardwareInit(); systemInit(); pnetNodeNumber = readNodeSw(); if (pnetNodeNumber == 0x0F) { sysStat |= ST_DEBUG_MODE; rs485TransmitEna(); putStr("\n\r DB CCS Child\r\n Ver "); // DEB, not for PROD putStr(verStr); putStr(", MLA\n\r"); menuDisplay(); putPrompt(); } rs485TransmitDisa(); while (1) { pnetNodeNumber = readNodeSw(); if (pnetNodeNumber == 0xF) sysStat |= ST_DEBUG_MODE; else sysStat &= ~ST_DEBUG_MODE; serialParse(); adcRHumid(ADC_SILENT); serialParse(); adcTemper(ADC_SILENT); adcTemperSecondary(ADC_SILENT); retVal = adcFloodSensor(ADC_SILENT); if (retVal == 1) sysStat |= ST_LIQUID_DETECTED; else if (retVal == 0) sysStat &= ~ST_LIQUID_DETECTED; if (sysStat & ST_DEBUG_MODE) LED_GREEN = 1; if (tmr1++ > 5000) { tmr1 = 0; childStateMach(CANCEL); // This is a crude timeout. Not sure how to improve it. We will rs485TransmitDisa(); // occasionally lose valid messages. But we need to break out of junky ones. } } // EO while (1) }
int main (void) { DPRINT("Starting...\n"); hardwareInit(); DPRINT("Hardware Initialized!\n"); int i; for(i=0; i<10; i++) { MIODispWriteText("testg", i, i); } //DelayMs(3000); Game* game = game_init(); DPRINT("Game Initialized!\n"); while(1) { game_run(game); } }
int main(void) { uchar key, lastKey = 0, keyDidChange = 0; uchar idleCounter = 0; wdt_enable(WDTO_2S); hardwareInit(); usbInit(); sei(); for(;;) { wdt_reset(); usbPoll(); key = keyPressed(); if(lastKey != key) { lastKey = key; keyDidChange = 1; } // 22 ms timer if(TIFR & (1<<TOV0)) { TIFR = 1<<TOV0; if(idleRate != 0) { if(idleCounter > 4) { // 22 ms in units of 4 ms idleCounter -= 5; } else { idleCounter = idleRate; keyDidChange = 1; } } } if(keyDidChange && usbInterruptIsReady()) { keyDidChange = 0; // use last key and not current key status in order to avoid lost // changes in key status. buildReport(lastKey); usbSetInterrupt(reportBuffer, sizeof(reportBuffer)); } } return 0; }
int main(void) { wdt_disable(); /* Stop watchdog timer */ hardwareInit(); /* Initialize the processor specific hardware */ uartInit(); /* Main loop */ for(;;) { /* Wait for data to appear */ uint16_t inputChar = getch(); uint8_t messageError = high(inputChar); if (messageError != NO_DATA) { sendch(low(inputChar)); } } }
void UserInit(void){ //setPrintStream(&USBPutArray); setPrintLevelInfoPrint(); println_I("\n\nStarting PIC initialization"); //DelayMs(1000); hardwareInit(); println_I("Hardware Init done"); ReleaseAVRReset(); CheckRev(); LoadEEstore(); LoadDefaultValues(); CartesianControllerInit(); InitPID(); UpdateAVRLED(); lockServos(); setPrintLevelInfoPrint(); BOOL brown = getEEBrownOutDetect(); setCoProcBrownOutMode(brown); setBrownOutDetect(brown); println_I("###Starting PIC In Debug Mode###\n");// All printfDEBUG functions do not need to be removed from code if debug is disabled DelayMs(1000); //setPrintLevelErrorPrint(); println_E("Error level printing"); println_W("Warning level printing"); println_I("Info level printing"); }
int main(void) { uchar updateNeeded = 0; uchar idleCounter = 0; wdt_enable(WDTO_2S); /* Enable watchdog timer 2s */ hardwareInit(); /* Initialize hardware (I/O) */ //odDebugInit(); usbInit(); /* Initialize USB stack processing */ sei(); /* Enable global interrupts */ for(;;){ /* Main loop */ wdt_reset(); /* Reset the watchdog */ usbPoll(); /* Poll the USB stack */ updateNeeded|=scankeys(); /* Scan the keyboard for changes */ /* Check timer if we need periodic reports */ if(TIFR0 & (1<<TOV0)){ TIFR0 = 1<<TOV0; /* Reset flag */ if(idleRate != 0){ /* Do we need periodic reports? */ if(idleCounter > 4){ /* Yes, but not yet */ idleCounter -= 5; /* 22 ms in units of 4 ms */ }else{ /* Yes, it is time now */ updateNeeded = 1; idleCounter = idleRate; } } } /* If an update is needed, send the report */ if(updateNeeded && usbInterruptIsReady()){ updateNeeded = 0; usbSetInterrupt(reportBuffer, sizeof(reportBuffer)); } } return 0; }
void UserInit(void) { //setPrintStream(&USBPutArray); clearPrint(); setPrintLevelInfoPrint(); println_I("Start PIC"); //DelayMs(1000); hardwareInit(); //println_I("Hardware Init done"); InitializeDyIODataTableManager(); CheckRev(); LoadEEstore(); UpdateAVRLED(); lockServos(); setPrintLevelInfoPrint(); boolean brown = getEEBrownOutDetect() ? true:false; setBrownOutDetect(brown); //Data table needs to be synced before the PID can init properly SyncDataTable(); InitPID(); //println_I("###Starting PIC In Debug Mode###\n"); // All printfDEBUG functions do not need to be removed from code if debug is disabled //DelayMs(1000); setPrintLevelWarningPrint(); //println_E("Error level printing"); //println_W("Warning level printing"); //println_I("Info level printing"); }
/* * Function Name: main() * Description: Cycle through RBG LED every time switch SW2 is pressed. * In this experiment we have to unlock PORTF Pin0 on which SW2 was connected. */ void main(void) { hardwareInit(); GPIOPinWrite(GPIO_PORTF_BASE, GPIO_PIN_1|GPIO_PIN_2|GPIO_PIN_3, 0); // all off rgbCycle(); GPIOPinWrite(GPIO_PORTF_BASE, GPIO_PIN_1|GPIO_PIN_2|GPIO_PIN_3, 0); // all off while(1) // forever loop { if (GPIOPinRead(GPIO_PORTF_BASE,GPIO_PIN_0) == 0) { SysCtlDelay(670); // switch debouncing rgbCycle(); while(GPIOPinRead(GPIO_PORTF_BASE,GPIO_PIN_0) == 0); GPIOPinWrite(GPIO_PORTF_BASE, GPIO_PIN_1|GPIO_PIN_2|GPIO_PIN_3, 0); // all off // SysCtlDelay(670); switchStatus++; } } }
int main(){ hardwareInit(); // initialize hardware _delay_ms(50); sei(); // enable interrupts for(;;){// infinite program loop #if defined(IRSENSORS) irDiff = readIRsmoothed(IR_LEFT, ir_LH_accum, &ir_start_cnt_LH) - readIRsmoothed(IR_RIGHT, ir_RH_accum, &ir_start_cnt_RH); // calculate new accelerometer data controlsys(irDiff, &MotorDutyCycle, &integrator, &lasterror); // process control system #elif defined(ACCELEROMETER) accelY = accelerometerSmoothed(accumulator, &accel_accum); // calculate new accelerometer data controlsys(accelY, &MotorDutyCycle, &integrator, &lasterror); // process control system #endif OCR0A = MotorDutyCycle; //_delay_ms(1); /* Toggle the test pin */ if ((PORTD & _BV(PD7)) == _BV(PD7)) PORTD &= ~_BV(PD7); else PORTD |= _BV(PD7); } return 0; }
int main(void) { char just_detected = 1; Gamepad *pad = NULL; hardwareInit(); gcn64protocol_hwinit(); #ifdef WAIT_FOR_PAD do { pad = tryDetectController(); } while (pad == NULL); curGamepad = pad; #else char i = 60; do { pad = tryDetectController(); if (pad) { curGamepad = pad; break; } _delay_ms(16); } while (--i); #endif reconnect: cli(); #ifdef SNES_MODE if (curGamepad && curGamepad->reportDescriptor) { rt_usbHidReportDescriptor = curGamepad->reportDescriptor; rt_usbHidReportDescriptorSize = curGamepad->reportDescriptorSize; } else { rt_usbHidReportDescriptor = (void*)gcn64_usbHidReportDescriptor; rt_usbHidReportDescriptorSize = getUsbHidReportDescriptor_size(); } #else rt_usbHidReportDescriptor = (void*)gcn64_usbHidReportDescriptor; rt_usbHidReportDescriptorSize = getUsbHidReportDescriptor_size(); #endif if (curGamepad && curGamepad->deviceDescriptor) { rt_usbDeviceDescriptor = curGamepad->deviceDescriptor; rt_usbDeviceDescriptorSize = curGamepad->deviceDescriptorSize; } else { rt_usbDeviceDescriptor = (void*)usbDescrDevice; rt_usbDeviceDescriptorSize = getUsbDescrDevice_size(); } // patch the config descriptor with the HID report descriptor size my_usbDescriptorConfiguration[25] = rt_usbHidReportDescriptorSize; my_usbDescriptorConfiguration[26] = rt_usbHidReportDescriptorSize >> 8; // Do hardwareInit again. It causes a USB reset. wdt_enable(WDTO_2S); usbInit(); usbReset(); sei(); while (1) { usbPoll(); wdt_reset(); if ((curGamepad == NULL )) { //either no controller detected or we changed mode... lets try to redetect controller ! pad = tryDetectController(); if (pad) { curGamepad = pad; just_detected = 1; /*if (pad->reportDescriptor != rt_usbHidReportDescriptor) { goto reconnect; }*/ } else { curGamepad = NULL; } } if (curGamepad != NULL) { controller_present_doTasks(just_detected); just_detected = 0; } } return 0; }
int main(void) { int adcOld[8] = { -1, -1, -1, -1, -1, -1, -1, -1 }; uchar key, lastKey = 0; uchar keyDidChange = 0; uchar midiMsg[8]; uchar channel = 0; int value; uchar iii; wdt_enable(WDTO_1S); hardwareInit(); odDebugInit(); usbInit(); sendEmptyFrame = 0; sei(); // only ADC channel 6 and channel 7 are used, start with channel 6 channel = 6; for (;;) { /* main event loop */ wdt_reset(); usbPoll(); key = keyPressed(); if (lastKey != key) keyDidChange = 1; if (usbInterruptIsReady()) { if (keyDidChange) { PORTC ^= 0x40; // DEBUG LED /* use last key and not current key status in order to avoid lost changes in key status. */ // up to two midi events in one midi msg. // For description of USB MIDI msg see: // http://www.usb.org/developers/devclass_docs/midi10.pdf // 4. USB MIDI Event Packets iii = 0; if (lastKey) { /* release */ midiMsg[iii++] = 0x08; midiMsg[iii++] = 0x80; midiMsg[iii++] = lastKey; midiMsg[iii++] = 0x00; } if (key) { /* press */ midiMsg[iii++] = 0x09; midiMsg[iii++] = 0x90; midiMsg[iii++] = key; midiMsg[iii++] = 0x7f; } if (8 == iii) sendEmptyFrame = 1; else sendEmptyFrame = 0; usbSetInterrupt(midiMsg, iii); keyDidChange = 0; lastKey = key; } else { // check analog input if no key event value = adc(channel); // 0..1023 // hysteresis if (adcOld[channel] - value > 7 || adcOld[channel] - value < -7) { // analog value has changed PORTC ^= 0x80; // DEBUG LED adcOld[channel] = value; // MIDI CC msg midiMsg[0] = 0x0b; midiMsg[1] = 0xb0; midiMsg[2] = channel + 70; // cc 70..77 midiMsg[3] = value >> 3; sendEmptyFrame = 0; usbSetInterrupt(midiMsg, 4); } channel++; channel &= 0x07; // TEST start with channel 6 if (0 == channel) channel = 6; } } // usbInterruptIsReady() } // main event loop
int main() { // TODO: watchdog wdt_disable(); // no watchdog, just because I'm lazy //position = 0; TCCR1B = _BV(CS12) | _BV(CS11); // timer is initialized, used to keep track of idle period hardwareInit(); usbInit(); // start v-usb usbDeviceDisconnect(); // enforce USB re-enumeration, do this while interrupts are disabled! _delay_ms(250); usbDeviceConnect(); sei(); // enable interrupts uint8_t to_send = 1; // boolean, true for first time while (1) { usbPoll(); /* * this area is where you should set the movement * and button values of the reports using the input * method of your choice * */ updateReport(); // determine whether or not the report should be sent if ((TCNT1 > ((4 * (F_CPU / 1024000)) * idle_rate) || TCNT1 > 0x7FFF) && idle_rate != 0) {// using idle rate to_send = 1; } else {// or if data has changed if (memcmp(&gamepad_report, &gamepad_report_old, sizeof(gamepad_report_t)) != 0) { to_send = 1; } } usbPoll(); if (to_send != 0) { // send the data if needed usbSendHidReport((uchar*)&gamepad_report, sizeof(gamepad_report_t)); TCNT1 = 0; // reset timer } usbPoll(); memcpy(&gamepad_report_old, &gamepad_report, sizeof(gamepad_report_t)); to_send = 0; // reset flag } return 0; }
int main(void) { unsigned char temp_sqr,temp_dir,temp_diff,temp; unsigned int i; unsigned char channel=150; hardwareInit(); //FOLD_ARMS; /*__delay_ms(50); do{ if(key_UP) { channel+=10; LED_ON; __delay_ms(20); LED_OFF; } if(key_DN) { channel-=10; LED_ON; __delay_ms(20); LED_OFF; } __delay_ms(300); readButtons(); } while(!key_GO); LED_ON; __delay_ms(200); LED_OFF; CC2500_init(); CC2500_write_register(CHANNR,channel); CC2500_idle_mode(); CC2500_receive_mode(); do{ if(key_UP) { speed++; if(speed>4) speed = 4; LED_ON; __delay_ms(20); LED_OFF; } if(key_DN) { speed--; if(speed<0) speed = 0; LED_ON; __delay_ms(20); LED_OFF; } __delay_ms(300); readButtons(); }while(!key_GO);*/ LED_ON; __delay_ms(150); LED_OFF; __delay_ms(150); LED_ON; __delay_ms(150); LED_OFF; #ifdef MACHINE_A statusInitA(); #endif #ifdef MACHINE_B statusInitB(); #endif assamble_packet(); CC2500_idle_mode(); CC2500_clear_rx_fifo(); CC2500_clear_tx_fifo(); CC2500_receive_mode(); __delay_ms(100); i = CC2500_read_status_register( RXBYTES); i&=0xFF; sprintf(outBuf,"\r\n%u\r\n",i); putsUART(outBuf); if(i) { CC2500_idle_mode(); CC2500_clear_rx_fifo(); CC2500_clear_tx_fifo(); CC2500_receive_mode(); while(GDO0==0); while(GDO0==1); SYSTIM_TMR = 9581; __delay_ms(3); commCount = 0; startSystemTimer(); noCommCount=0; commEnabled = TRUE; if(waitDataUpdate()) my.obzEntrCnt = fellow.obzEntrCnt; /*LED_ON; __delay_ms(2000);*/ } else { startSystemTimer(); noCommCount=0; commEnabled = TRUE; } #ifdef MACHINE_A while(!key_GO){ if(noCommCount>100) { commEnabled = FALSE; waitToTick(); CC2500_init(); commEnabled = TRUE; __delay_ms(40); //noCommCount = 0; } } #endif if(displayEnabled) { switch(resetSource()) { case POWER_ON_RESET: sprintf(outBuf,"\r\nBattry Voltage : %u",batteryVoltage); putsUART(outBuf); break; case EXTERNAL_RESET: sprintf(outBuf,"\r\nUser Reset..."); putsUART(outBuf); break; default: sprintf(outBuf,"\r\nUnknown Reset"); putsUART(outBuf); break; } } /*while(TRUE) { printSensors(); __delay_ms(500); }*/ gridInit(); #ifdef MACHINE_B while(TRUE) { if(noCommCount>100) { commEnabled = FALSE; waitToTick(); CC2500_init(); commEnabled = TRUE; __delay_ms(40); //noCommCount = 0; } if(dataUpdated) { printStatus(&fellow); /*rx_buff_temp[0] = rx_buff0; rx_buff_temp[1] = rx_buff1; rx_buff_temp[2] = rx_buff2; rx_buff_temp[3] = rx_buff3; rx_buff_temp[4] = rx_buff4; rx_buff_temp[5] = rx_buff5; sprintf(outBuf,"\r\n\r\n%u,%u,%u,%u,%u,%u", rx_buff_temp[0], rx_buff_temp[1], rx_buff_temp[2], rx_buff_temp[3], rx_buff_temp[4], rx_buff_temp[5]); putsUART(outBuf);*/ dataUpdated = FALSE; } } //while(!key_GO); while(fellow.heading==10&&!key_GO); //while(fellow.heading==10); #endif while(TRUE) { if(noCommCount>100) { commEnabled = FALSE; waitToTick(); CC2500_init(); commEnabled = TRUE; __delay_ms(40); //noCommCount = 0; } switch(mcState) { case MC_START: { cubeSensorsOn(); waitToTick(); waitToTick(); cubeSensorsOff(); temp_sqr = nextSquare(my.location,my.heading&0x07); grid[temp_sqr] |= VISITED; if(frontCube) { myCube[++cubeInd] = temp_sqr; mcState=MC_DELIVER; } else mcState=MC_SCAN; break; } case MC_SCAN: { temp_sqr = scanMap[my.smInd]; if(my.location==temp_sqr) { my.smInd++; break; } my.goalSquare = temp_sqr; mcNextState = MC_SCAN; mcState = MC_GET_M_THR; /*temp_dir = nextDirection(my.location,temp_sqr); temp_diff = temp_dir - (my.heading&0x07); if(prState==PR_STR_ACC||prState==PR_STR_DEC) { if(temp_diff) break; waitDataUpdate(); if(commonSquare(temp_sqr)) break; my.heading &= 0x07; finalPos += COUNTS_PER_CELL*256; if(prState==PR_STR_DEC) prState = PR_STR_ACC; while(relativeDistance>distance[ORTHO_SENS_DIST]*2); } else if(prState==PR_FINISHED) { if(temp_diff) { commandList[cmlInd++] = getTurnCmd(temp_diff); my.heading = temp_dir | NODIR; } waitDataUpdate(); if(commonSquare(temp_sqr)) { commandList[cmlInd++] = CMD_STOP; getNextMove(); break; } my.heading &= 0x07; commandList[cmlInd++] = CMD_STRAIGHT | CMD_FORWARD; commandList[cmlInd++] = CMD_STOP; getNextMove(); } else break; while(prState!=PR_STR_ACC); while(relativeDistance<distance[ORTHO_SENS_DIST]*2); my.location = nextSquare(my.location,my.heading); grid[my.location] |= ONROUTE; my.heading |= NODIR; if(scanNeighbours()) { mcState = MC_DELIVER; }*/ my.smInd++; if(my.smInd>=17) { my.smInd = 0; #ifdef MACHINE_A my.goalSquare = 20; #endif #ifdef MACHINE_B my.goalSquare = 60; #endif mcNextState = MC_SCAN; mcState = MC_GET_M_THR; } break; } case MC_GET_M_THR: { waitDataUpdate(); updateGrid(); floodGrid(my.goalSquare); /*if(map[my.location] == 0) { mcState = mcNextState; break; }*/ temp_sqr = nextNeighbour(my.location); temp_dir = nextDirection(my.location,temp_sqr); temp_diff = temp_dir - (my.heading&0x07); waitDataUpdate(); if(commonSquare(temp_sqr)) { break; } if((!inOBZ(my.location)) && inOBZ(temp_sqr)) { if(!my.obzClear || !my.delivering) break; } if(inOBZ(my.location) && (!inOBZ(temp_sqr))) { my.obzEntrCnt+=2; } if(isDipoSquare(temp_sqr)&& my.delivering) { mcState = MC_DELIVER_FINISH; break; } if(prState==PR_STR_ACC||prState==PR_STR_DEC) { if(temp_diff) break; my.heading &= 0x07; finalPos += COUNTS_PER_CELL*256; if(prState==PR_STR_DEC) prState = PR_STR_ACC; while(relativeDistance>distance[ORTHO_SENS_DIST]*2); } else if(prState==PR_FINISHED) { if(temp_diff) { commandList[cmlInd++] = getTurnCmd(temp_diff); commandList[cmlInd++] = CMD_STOP; getNextMove(); my.heading = temp_dir|NODIR; break; } else { if(!(grid[temp_sqr]&VISITED)); { cubeSensorsOn(); waitToTick(); waitToTick(); cubeSensorsOff(); if(!commonSquare(temp_sqr)) { grid[temp_sqr] |= VISITED; if(frontCube) { updateCube(temp_sqr); break; } } } my.heading &= 0x07; commandList[cmlInd++] = CMD_STRAIGHT | CMD_FORWARD; commandList[cmlInd++] = CMD_STOP; getNextMove(); } } else break; while(prState!=PR_STR_ACC); while(relativeDistance<distance[ORTHO_SENS_DIST]*2); my.location = nextSquare(my.location,my.heading); grid[my.location] |= ONROUTE; my.heading |= NODIR; if(scanNeighbours()&&(my.delivering==FALSE)) { mcState = MC_DELIVER; break; } if(map[my.location] == 0) mcState = mcNextState; break; } case MC_DELIVER: { temp_sqr = unDipoCube(); if(temp_sqr) { waitDataUpdate(); if(inOBZ(temp_sqr)&&!my.obzClear) break; temp_dir = nextDirection(my.location,temp_sqr); if(temp_dir==NODIR) { temp_sqr = nearestNeighbourOfCube(); if(temp_sqr) { my.goalSquare = temp_sqr; mcNextState = MC_DELIVER; mcState = MC_GET_M_THR; } else { do{ my.smInd++; }while(grid[scanMap[my.smInd]]&ONROUTE); my.goalSquare = scanMap[my.smInd]; mcNextState = MC_SCAN; mcState = MC_GET_M_THR; } break; } else { grabCube(temp_dir); if(my.delivering) { temp = temp_sqr; my.goalSquare = my.location; temp_sqr = getDipoSquare(); if(temp_sqr) { for(i=0; i<4; i++) { if((myCube[i]<81)&&myCube[i]==temp) temp = i; } myCube[/*cubeIndForMcDeliver*/temp] = temp_sqr; my.goalSquare = temp_sqr; mcNextState = MC_DELIVER; mcState = MC_GET_M_THR; break; } } } break; } do{ my.smInd++; }while(grid[scanMap[my.smInd]]&ONROUTE); my.goalSquare = scanMap[my.smInd]; mcNextState = MC_SCAN; mcState = MC_GET_M_THR; break; } case MC_DELIVER_FINISH: { deliverCube(); if(unDipoCube()) { mcState = MC_DELIVER; break; } do{ my.smInd++; }while(grid[scanMap[my.smInd]]&ONROUTE); my.goalSquare = scanMap[my.smInd]; mcNextState = MC_SCAN; mcState = MC_GET_M_THR; } } } while(TRUE); return 0; }