// ============================================================================================ // MAIN FUNCTION // ============================================================================================ int main(void) { char input_char; int end_time; _delay_ms(50); init(); xgrid.rx_pkt = &rx_pkt; LED_PORT.OUT = LED_USR_0_PIN_bm; fprintf_P(&usart_stream, PSTR("avr-xgrid build %ld\r\n"), (unsigned long) &__BUILD_NUMBER); // ##### Initialization of swarm dynamics ##### switch(program_num){ case 1: swarm_initialization1(); break; case 2: swarm_initialization2(); break; case 3: swarm_initialization3(); break; } // ############################################ while (1){ end_time = jiffies + 100; if (usart.available()) input_char = usart.get(); // main loop if (input_char == 0x1b) xboot_reset(); else if(input_char != 0x00){ fprintf_P(&usart_stream, PSTR("CPU: %c\r\n"), input_char); external_command(input_char); input_char = 0x00; // clear the most recent computer input } communication(); // send position data to neighbors // ##### Processing for swarm dynamics ##### switch(program_num){ // select program case 1: swarm_calculation1(); break; // Ken's swarm dynamics case 2: swarm_calculation2(); break; // two rythms case 3: swarm_calculation3(); break; // Van der Pol Oscillator default: no_movement(); } // ######################################### servo_motor_control(); // servo control if(jiffies>end_time){ LED_PORT.OUT = LED_USR_1_PIN_bm; // turn on yellow LED } while(jiffies<end_time); if(jiffies>60000) jiffies = 0; //reset jiffies in every 60 sec } return 0; }
void serial_communication_task(void) { // Wait for timeout and reset device if the command was issued if (reset) { static uint8_t reset_timeout = RESET_TIMEOUT * F_TASK_SLOW; --reset_timeout; if (reset_timeout == 0) { xboot_reset(); } return; } // If nothing was received, there is nothing to do if (!usb_bytes_received()) { return; } // Process update data / command characters if (update_in_progress) { process_update_data(); } else { process_command_char(); } }
// ============================================================================================ // MAIN FUNCTION // ============================================================================================ int main(void) { float angle; char input_char; _delay_ms(50); init(); xgrid.rx_pkt = &rx_pkt; LED_PORT.OUT = LED_USR_0_PIN_bm; swarm_initialization2(); while (1){ if (usart.available()) input_char = usart.get(); // main loop if (input_char == 0x1b) xboot_reset(); if (input_char == 's') sensor = true; if (input_char == ' ') sensor = false; if(communication_on){ communication(); communication_on = false; } if(calculation_on){ /* mchip.hd_memo = mchip.hd; swarm_calculation1(); mchip.hd_diff = (mchip.hd - mchip.hd_memo) / 5.0; //divide into 5 step */ servo_tick = 0; swarm_calculation2(); calculation_on = false; } if(servo_motor_on){ //angle = mchip.hd_memo + (float)servo_tick * mchip.hd_diff; //servo_motor_control(angle); //servo_motor_on = false; } } return 0; }
int main(void) { char old_btn = 0; char btn; uint32_t j = 0; _delay_ms(50); init(); xgrid.rx_pkt = &rx_pkt; LED_PORT.OUT = LED_USR_0_PIN_bm; fprintf_P(&usart_stream, PSTR("avr-xgrid build %ld\n"), (unsigned long) &__BUILD_NUMBER); while (1) { // main loop if (usart.available() && usart.get() == 0x1b) xboot_reset(); // check for button press btn = !(BTN_PORT.IN & BTN_PIN_bm); if (btn && (btn != old_btn)) { char str[] = "test"; Xgrid::Packet pkt; pkt.type = 0; pkt.flags = 0; pkt.radius = 1; pkt.data = (uint8_t *)str; pkt.data_len = 4; xgrid.send_packet(&pkt); LED_PORT.OUTTGL = LED_USR_1_PIN_bm; } old_btn = btn; j = jiffies + 10; while (j > jiffies) { }; } }
int main(void) { //char old_btn = 0; //char btn; uint32_t j = 0; char input_char, first_char; _delay_ms(50); init(); xgrid.rx_pkt = &rx_pkt; LED_PORT.OUT = LED_USR_0_PIN_bm; fprintf_P(&usart_stream, PSTR("avr-xgrid build %ld\r\n"), (unsigned long) &__BUILD_NUMBER); /* char str[] = "boot up"; Xgrid::Packet pkt; pkt.type = 0; pkt.flags = 0; pkt.radius = 1; pkt.data = (uint8_t *)str; pkt.data_len = 4; xgrid.send_packet(&pkt); */ // KEN'S CODE swarm_initialization(); int16_t servo_position_deg = 0; // servo position in degrees // END KEN'S CODE if (usart.available()) { first_char = usart.get(); } while (1) { j = jiffies + 10; // from here, you have 10 ms to reach the bottom of this loop if (usart.available()) { input_char = usart.get(); } // main loop if (input_char == 0x1b) xboot_reset(); else if(input_char != 0x00) { fprintf_P(&usart_stream, PSTR("CPU: %c**\r\n"), input_char); if (input_char == 'a') { char str[] = "A"; Xgrid::Packet pkt; // Packet is defined on line 72 of xgrid.h pkt.type = 0; pkt.flags = 0; pkt.radius = 1; pkt.data = (uint8_t *)str; pkt.data_len = 4; xgrid.send_packet(&pkt); } else if(input_char == 'y') { LED_PORT.OUTTGL = LED_USR_1_PIN_bm; } input_char = 0x00; // clear the most recent computer input } // KEN'S CODE: //swarm_communication(); //swarm_calculation(); servo_motor_control(); // END KEN'S CODE if(jiffies > j) // if TRUE, then we took too long to get here, halt program { cli(); //disable_interrupts LED_PORT.OUTSET = LED_USR_0_PIN_bm; // turn red light on and keep it on LED_PORT.OUTSET = LED_USR_1_PIN_bm; // turn yellow light on and keep it on while(1==1) {}; } while (j > jiffies) { }; } }
int CommsHandler::process(uint8_t *responseBuffer, uint16_t responseBufferLen) { if (commsPtr == NULL) { errno = errorCommsNotSet; return false; } bool validResponse = false; errno = errorNoError; commsPtr->poll(); switch (state) { case stateWaitingForMessages: if (!stack.isEmpty()) { messageLen = stack.read(messageBuffer, messageBufferLen); if (messageLen) { bytesSent = 0; state = statePowerUp; if (commsBlockSize) { // Append null characters to fill a complete block // const uint8_t xrfBlockSize = 12; uint8_t remainder = messageLen % commsBlockSize; if (remainder) { uint16_t newLen = messageLen + (commsBlockSize - remainder); if (newLen <= messageBufferLen) { memset(messageBuffer + messageLen, 0, commsBlockSize - remainder); messageLen = newLen; } } } } } break; case statePowerUp: if (commsPtr->powerOn()) state = sendingData; break; case sendingData: // Send each byte individually. At present // HardwareSerial.write() waits until the buffer has room but // in future it might return immediately with a return value of // zero. if (bytesSent == 0 && verbosity) AWPacket::printPacket(messageBuffer, messageLen, console); if (bytesSent == 0) commsPtr->messageStart(); { size_t bytesToSend = commsPtr->messageWriteSize(); if (bytesToSend == 0) bytesToSend = messageLen; bytesSent += commsPtr->write(messageBuffer + bytesSent, bytesToSend); } if (bytesSent == messageLen) { commsPtr->messageEnd(); responseLen = 0; responseTimeout.start(responseTimeout_ms, AsyncDelay::MILLIS); responsePacketLen = 65535; // Use maximum value until we know state = stateWaitingForResponse; } break; case stateWaitingForResponse: if (responseTimeout.isExpired()) { Serial.println("Message timeout"); errno = errorResponseTimeout; ++ messagesWithoutAck; uint8_t maxMessagesNoAck = eeprom_read_byte((uint8_t*)EEPROM_MAX_MESSAGES_NO_ACK); if (maxMessagesNoAck && messagesWithoutAck >= maxMessagesNoAck) { Serial.println("Reboot due to timeout"); // DEBUG delay(1000); xboot_reset(); //wdt_enable(WDTO_1S); //while (1) // ; } // Put message back into queue if retries not exceeded AWPacket failedPacket(messageBuffer, messageLen); if (failedPacket.getRetries() < maxRetries) { failedPacket.incrementRetries(); // Update the signature for new retries failedPacket.setKey(key, keyLen); failedPacket.putSignature(messageBuffer, messageBufferLen); messageLen = 0; stack.write(messageBuffer, AWPacket::getPacketLength(messageBuffer)); } else { Serial.print("Too many retries: "); AWPacket::printPacket(messageBuffer, messageBufferLen, Serial); } state = stateTimedOut; break; } commsPtr->checkForResponse(); if (int avail = commsPtr->available()) { if (responseLen >= responseBufferLen) { errno = errorBufferTooSmall; messageLen = 0; state = stateWaitingForMessages; break; } for (int i = avail; i; --i) { uint8_t b = commsPtr->read(); responseBuffer[responseLen++] = b; } if (validateResponse(responseBuffer, responseLen)) { // The response in the buffer is valid, but is it a response // to our last message? uint32_t messageSeconds, responseSeconds; uint16_t messageFraction, responseFraction; AWPacket::getTimestamp(messageBuffer, messageSeconds, messageFraction); AWPacket::getTimestamp(responseBuffer, responseSeconds, responseFraction); if ((AWPacket::getSiteId(responseBuffer) == AWPacket::getSiteId(messageBuffer)) && (messageSeconds == responseSeconds) && (messageFraction == responseFraction) && (AWPacket::getSequenceId(responseBuffer) == AWPacket::getSequenceId(messageBuffer)) && (AWPacket::getRetries(responseBuffer) == AWPacket::getRetries(messageBuffer))) { // Valid response found validResponse = true; messageLen = 0; state = stateWaitingForMessages; } else { // Need another response Serial.println("######################"); Serial.println("Packet valid but incorrect response"); AWPacket::printPacket(responseBuffer, responseLen, Serial); Serial.println("######################"); responseLen = 0; responsePacketLen = 65535; // Use maximum value until we know } } } break; case stateTimedOut: // Stuck here until a new message is added break; } return validResponse; }
int main(void) { //char old_btn = 0; //char btn; uint32_t j = 0; char input_char, first_char; _delay_ms(50); init(); xgrid.rx_pkt = &rx_pkt; LED_PORT.OUT = LED_USR_0_PIN_bm; fprintf_P(&usart_stream, PSTR("avr-xgrid build %ld\r\n"), (unsigned long) &__BUILD_NUMBER); /* char str[] = "boot up"; Xgrid::Packet pkt; pkt.type = 0; pkt.flags = 0; pkt.radius = 1; pkt.data = (uint8_t *)str; pkt.data_len = 4; xgrid.send_packet(&pkt); */ if (usart.available()) { first_char = usart.get(); } while (1) { if (usart.available()) { input_char = usart.get(); } // main loop if (input_char == 0x1b) xboot_reset(); else if(input_char == 0x00) { //fprintf_P(&usart_stream, PSTR("received: first char (%i)**\r\n"),first_char); } else { fprintf_P(&usart_stream, PSTR("CPU: %c**\r\n"), input_char); if (input_char == 'a') { char str[] = "A"; Xgrid::Packet pkt; // Packet is defined onl line 72 of xgrid.h pkt.type = 0; pkt.flags = 0; pkt.radius = 1; pkt.data = (uint8_t *)str; pkt.data_len = 4; xgrid.send_packet(&pkt); } else if(input_char == 'y') { LED_PORT.OUTTGL = LED_USR_1_PIN_bm; } else if (input_char == '1') { char str[] = "1"; Xgrid::Packet pkt; // Packet is defined on line 72 of xgrid.h pkt.type = 0; pkt.flags = 0; pkt.radius = 1; pkt.data = (uint8_t *)str; pkt.data_len = 1; xgrid.send_packet(&pkt); } else if (input_char == '2') { char str[] = "22"; Xgrid::Packet pkt; // Packet is defined on line 72 of xgrid.h pkt.type = 0; pkt.flags = 0; pkt.radius = 1; pkt.data = (uint8_t *)str; pkt.data_len = 2; xgrid.send_packet(&pkt); } else if (input_char == '3') { char str[] = "333"; Xgrid::Packet pkt; // Packet is defined on line 72 of xgrid.h pkt.type = 0; pkt.flags = 0; pkt.radius = 1; pkt.data = (uint8_t *)str; pkt.data_len = 3; xgrid.send_packet(&pkt); } else if (input_char == '4') { char str[] = "4444"; Xgrid::Packet pkt; // Packet is defined on line 72 of xgrid.h pkt.type = 0; pkt.flags = 0; pkt.radius = 1; pkt.data = (uint8_t *)str; pkt.data_len = 4; xgrid.send_packet(&pkt); } else if (input_char == '5') { char str[] = "55555"; Xgrid::Packet pkt; // Packet is defined on line 72 of xgrid.h pkt.type = 0; pkt.flags = 0; pkt.radius = 1; pkt.data = (uint8_t *)str; pkt.data_len = 5; xgrid.send_packet(&pkt); } else if (input_char == '6') { char str[] = "666666"; Xgrid::Packet pkt; // Packet is defined on line 72 of xgrid.h pkt.type = 0; pkt.flags = 0; pkt.radius = 1; pkt.data = (uint8_t *)str; pkt.data_len = 6; xgrid.send_packet(&pkt); } else if (input_char == '7') { char str[] = "7777777"; Xgrid::Packet pkt; // Packet is defined on line 72 of xgrid.h pkt.type = 0; pkt.flags = 0; pkt.radius = 1; pkt.data = (uint8_t *)str; pkt.data_len = 7; xgrid.send_packet(&pkt); } else if (input_char == '8') { char str[] = "88888888"; Xgrid::Packet pkt; // Packet is defined on line 72 of xgrid.h pkt.type = 0; pkt.flags = 0; pkt.radius = 1; pkt.data = (uint8_t *)str; pkt.data_len = 8; xgrid.send_packet(&pkt); } else if (input_char == '9') { char str[] = "999999999"; Xgrid::Packet pkt; // Packet is defined on line 72 of xgrid.h pkt.type = 0; pkt.flags = 0; pkt.radius = 1; pkt.data = (uint8_t *)str; pkt.data_len = 9; xgrid.send_packet(&pkt); } else if (input_char == '0') { char str[] = "1010101010"; Xgrid::Packet pkt; // Packet is defined on line 72 of xgrid.h pkt.type = 0; pkt.flags = 0; pkt.radius = 1; pkt.data = (uint8_t *)str; pkt.data_len = 10; xgrid.send_packet(&pkt); } else if (input_char == '-') { char str[] = "11111111111"; Xgrid::Packet pkt; // Packet is defined on line 72 of xgrid.h pkt.type = 0; pkt.flags = 0; pkt.radius = 1; pkt.data = (uint8_t *)str; pkt.data_len = 11; xgrid.send_packet(&pkt); } else if (input_char == '=') { char str[] = "121212121212"; Xgrid::Packet pkt; // Packet is defined on line 72 of xgrid.h pkt.type = 0; pkt.flags = 0; pkt.radius = 1; pkt.data = (uint8_t *)str; pkt.data_len = 12; xgrid.send_packet(&pkt); } else if (input_char == '[') { char str[] = "1313131313131"; Xgrid::Packet pkt; // Packet is defined on line 72 of xgrid.h pkt.type = 0; pkt.flags = 0; pkt.radius = 1; pkt.data = (uint8_t *)str; pkt.data_len = 13; xgrid.send_packet(&pkt); } else if (input_char == ']') { char str[] = "14141414141414"; Xgrid::Packet pkt; // Packet is defined on line 72 of xgrid.h pkt.type = 0; pkt.flags = 0; pkt.radius = 1; pkt.data = (uint8_t *)str; pkt.data_len = 14; xgrid.send_packet(&pkt); } input_char = 0x00; } j = jiffies + 10; while (j > jiffies) { }; } }
// ============================================================================================ // KEY INPUT PROCESS // ============================================================================================ void key_input() { char input_char = 0; if (usart.available()) input_char = usart.get(); if(input_char == 0x1b) xboot_reset(); //reboot the board if(input_char == 'Z') //Reboot whole system { send_message(MESSAGE_COMMAND, ALL_DIRECTION, ALL, "Z"); temp_time = jiffies + 3000; while(jiffies < temp_time) { LED_PORT.OUTTGL = LED_USR_1_PIN_bm; _delay_ms(100); } xboot_reset(); } // print version number if(input_char == 'v') fprintf_P(&usart_stream, PSTR("build number = %ld\r\n"), (unsigned long) &__BUILD_NUMBER); if(input_char == 'd') { if(display) display = false; else display = true; } if(input_char == 'r') //set sec_counter as 0 { send_message(MESSAGE_COMMAND, ALL_DIRECTION, ALL, "r"); sec_counter = 0; } // ============================================================================================ /// updateRate commands // ============================================================================================ // Hold Current Servo Position if(input_char == 'n'){ fprintf_P(&usart_stream, PSTR("setting updateRate to NONE\n")); updateRate = NONE; send_message(MESSAGE_COMMAND, ALL_DIRECTION, ALL, "n"); } if(input_char == 's'){ fprintf_P(&usart_stream, PSTR("setting updateRate to 10ms\n")); updateRate = SMOOTH; send_message(MESSAGE_COMMAND, ALL_DIRECTION, ALL, "s"); } if(input_char == 'h'){ fprintf_P(&usart_stream, PSTR("setting updateRate to 200ms\n")); updateRate = TWO_HUNDRED; send_message(MESSAGE_COMMAND, ALL_DIRECTION, ALL, "h"); } // ============================================================================================ /// currentMode commands // ============================================================================================ if(input_char == 'p'){ fprintf_P(&usart_stream, PSTR("'p' - etting currentMode to PERIODIC\n")); currentMode = PERIODIC; send_message(MESSAGE_COMMAND, ALL_DIRECTION, ALL, "p"); } if(input_char == 'V'){ fprintf_P(&usart_stream, PSTR("'V' - setting currentMode to AVERAGE\n")); currentMode = AVERAGE; send_message(MESSAGE_COMMAND, ALL_DIRECTION, ALL, "V"); } if(input_char == 'w'){ fprintf_P(&usart_stream, PSTR("'w' - setting currentMode to SWEEP\n")); currentMode = SWEEP; send_message(MESSAGE_COMMAND, ALL_DIRECTION, ALL, "w"); } if(input_char == 'c'){ cycleOn = !cycleOn; fprintf_P(&usart_stream, PSTR("'c' - cycle all modes\n")); send_message(MESSAGE_COMMAND, ALL_DIRECTION, ALL, "c"); } // ============================================================================================ /// toggle debug print // ============================================================================================ if(input_char == 'P'){ fprintf_P(&usart_stream, PSTR("'P' - toggling debug printf on/off \n")); debugPrint = !debugPrint; } if(input_char == 'c'){ printKeyCommands(); } }
// ============================================================================================ // KEY INPUT PROCESS // ============================================================================================ void key_input() { char input_char = 0; if (usart.available()) input_char = usart.get(); if(input_char == 0x1b) xboot_reset(); //reboot the board if(input_char == 'Z') //Reboot whole system { send_message(MESSAGE_COMMAND, ALL_DIRECTION, ALL, "Z"); temp_time = jiffies + 3000; while(jiffies < temp_time) { LED_PORT.OUTTGL = LED_USR_1_PIN_bm; _delay_ms(100); } xboot_reset(); } if(input_char == 'D') //Debug Mode { speedup_on = false; communication_on = false; send_message(MESSAGE_COMMAND, ALL_DIRECTION, ALL, "D"); } if(input_char == 'R') //Run Mode { speedup_on = true; communication_on = true; send_message(MESSAGE_COMMAND, ALL_DIRECTION, ALL, "R"); } if(input_char == 'v') fprintf_P(&usart_stream, PSTR("build number = %ld\r\n"), (unsigned long) &__BUILD_NUMBER); if(input_char == 'd') { if(display) display = false; else display = true; } if(input_char == 'I') //initialization { send_message(MESSAGE_COMMAND, ALL_DIRECTION, ALL, "I"); init_variables(); sec_counter = 0; } if(input_char == 'i') //reset variables { send_message(MESSAGE_COMMAND, ALL_DIRECTION, ALL, "i"); init_variables(); } if(input_char == 'r') //set sec_counter as 0 { send_message(MESSAGE_COMMAND, ALL_DIRECTION, ALL, "r"); sec_counter = 0; } if(input_char == '1') { send_message(MESSAGE_COMMAND, ALL_DIRECTION, ALL, "1"); init_variables(); sec_counter = STGtime1; } if(input_char == '2') { send_message(MESSAGE_COMMAND, ALL_DIRECTION, ALL, "2"); init_variables(); sec_counter = STGtime2; } if(input_char == '3') { send_message(MESSAGE_COMMAND, ALL_DIRECTION, ALL, "3"); init_variables(); sec_counter = STGtime3; } if(input_char == '4') //initialization { send_message(MESSAGE_COMMAND, ALL_DIRECTION, ALL, "4"); init_variables(); sec_counter = STGtime4; } if(input_char == '5') //initialization { send_message(MESSAGE_COMMAND, ALL_DIRECTION, ALL, "5"); init_variables(); sec_counter = STGtime5; } if(input_char == '6') //initialization { send_message(MESSAGE_COMMAND, ALL_DIRECTION, ALL, "6"); init_variables(); sec_counter = STGtime6; } if(input_char == '7') //initialization { send_message(MESSAGE_COMMAND, ALL_DIRECTION, ALL, "7"); init_variables(); sec_counter = STGtime7; } if(input_char == '8') //initialization { send_message(MESSAGE_COMMAND, ALL_DIRECTION, ALL, "8"); init_variables(); sec_counter = STGtime8; } if(input_char == '9') //initialization { send_message(MESSAGE_COMMAND, ALL_DIRECTION, ALL, "9"); init_variables(); sec_counter = STGtime9; } if(input_char == '0') //initialization { send_message(MESSAGE_COMMAND, ALL_DIRECTION, ALL, "0"); init_variables(); sec_counter = LASTtime; } }