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