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