#define F_CPU 16000000UL //Set processor speed 16 Mhz
#include <avr/io.h>
#include <avr/interrupt.h>
#include <util/delay.h>
#include <dynamixel.h>
 
int i;
 
int main(void)
{
	DDRC = 0xFF; //Set all PORT C as output
	PORTC = 0xFE; //Turn on D1 LED
 
	TCCR0B |= (1<<CS02)|(1<<CS00);
	TIMSK0 |= (1<<TOIE0);
 
	TCNT0 = 0;
	sei(); // enable global interrupts
 
	dxl_initialize(0,1); //0= number connected com devices, 1= Baud rate 
	serial_initialize(57600);
 
 
	while(1);
}
 
 
 
 
ISR(TIMER0_OVF_vect)
{
	static int j=0, i=0;
	j++;
	if (j>=64) {
		j=0;
		//PORTC = ~(1<<i);
		i++;
		if (i > 4) i=0;
		int goal_pos = 160*i;
		int mot_num = 18;
		int address = 30;
		//int pos = dxl_read_word(18,36);
		dxl_write_word(254,address,goal_pos);
	}
}

I2RlZmluZSBGX0NQVSAxNjAwMDAwMFVMIC8vU2V0IHByb2Nlc3NvciBzcGVlZCAxNiBNaHoKI2luY2x1ZGUgPGF2ci9pby5oPgojaW5jbHVkZSA8YXZyL2ludGVycnVwdC5oPgojaW5jbHVkZSA8dXRpbC9kZWxheS5oPgojaW5jbHVkZSA8ZHluYW1peGVsLmg+CgppbnQgaTsKCmludCBtYWluKHZvaWQpCnsKCUREUkMgPSAweEZGOyAvL1NldCBhbGwgUE9SVCBDIGFzIG91dHB1dAoJUE9SVEMgPSAweEZFOyAvL1R1cm4gb24gRDEgTEVECgkKCVRDQ1IwQiB8PSAoMTw8Q1MwMil8KDE8PENTMDApOwoJVElNU0swIHw9ICgxPDxUT0lFMCk7CgkKCVRDTlQwID0gMDsKCXNlaSgpOyAvLyBlbmFibGUgZ2xvYmFsIGludGVycnVwdHMKCQoJZHhsX2luaXRpYWxpemUoMCwxKTsgLy8wPSBudW1iZXIgY29ubmVjdGVkIGNvbSBkZXZpY2VzLCAxPSBCYXVkIHJhdGUgCglzZXJpYWxfaW5pdGlhbGl6ZSg1NzYwMCk7CgoJCgl3aGlsZSgxKTsKfQoKCgoKSVNSKFRJTUVSMF9PVkZfdmVjdCkKewoJc3RhdGljIGludCBqPTAsIGk9MDsKCWorKzsKCWlmIChqPj02NCkgewoJCWo9MDsKCQkvL1BPUlRDID0gfigxPDxpKTsKCQlpKys7CgkJaWYgKGkgPiA0KSBpPTA7CgkJaW50IGdvYWxfcG9zID0gMTYwKmk7CgkJaW50IG1vdF9udW0gPSAxODsKCQlpbnQgYWRkcmVzcyA9IDMwOwoJCS8vaW50IHBvcyA9IGR4bF9yZWFkX3dvcmQoMTgsMzYpOwoJCWR4bF93cml0ZV93b3JkKDI1NCxhZGRyZXNzLGdvYWxfcG9zKTsKCX0KfQ==