// ============================================================================================ // 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 loop() { unsigned long jiffies = getJiffies(); if (jiffies % 50 == 0) { LED_PORT.OUTTGL = LED_USR_0_PIN_bm; // toggle red light every 50 ms (20 Hz) servo_motor_control(); } if (jiffies % 100 == 0) { swarm_communication(); swarm_calculation(); } }
// ============================================================================================ // 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_initialization1(); while (1){ if (usart.available()) input_char = usart.get(); // main loop if (input_char == 0x1b) xboot_reset(); 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; 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; }
// -------------------------------------------------------------------------------------------- void swarm_calculation2() { int i; float sum=0, vals, valc, th=0.1, alpha, angle; for(i=0;i<NUM_NEIGHBORS;i++){ if(mchip.open[i]) sum += mchip.neix[i]; } if(sensor) sum=1; if(sum != 0 && mchip.flag == false){ mchip.tim1 = 0; mchip.tim2 = 0; mchip.flag = true; } if(mchip.tim2<=1.0*PI) mchip.tim1 += 0.1; else mchip.tim1=0; mchip.tim2+=0.01; vals=sin(mchip.tim2); valc=cos(mchip.tim2); if(vals>0 && vals<th && valc<0 && mchip.flag) mchip.px=1.0; else mchip.px = 0.0; if(mchip.tim2>2.0*PI && vals>0 && valc<0) mchip.flag = false; if(mchip.tim2>=3.0*PI) mchip.tim2=3.0*PI; alpha=sin(mchip.tim2); if(alpha<0) alpha=0; angle = (10.0 * alpha) * sin(mchip.tim1); servo_motor_control(angle); }
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) { }; } }