/*****************************************************

This program was produced by the

CodeWizardAVR V2.04.4a Advanced
Automatic Program Generator
Copyright 1998-2009 Pavel Haiduc, HP InfoTech s.r.l.
http://www.hpinfotech.com
Project :
Version :
Date
: 1/9/2014
Author : NeVaDa
Company : mas karyo
Comments:
Chip type
: ATmega128
Program type
: Application
AVR Core Clock frequency: 16.000000 MHz
Memory model
: Small
External RAM size
: 0
Data Stack size
: 1024
*****************************************************/
#include
#include
#include
#include

<mega128.h>
<math.h>
<stdio.h>
<delay.h>

// Alphanumeric LCD Module functions

#asm
.equ __lcd_port=0x1B ;PORTA
#endasm
#include <lcd.h>
#ifndef RXB8
#define RXB8 1
#endif
#ifndef TXB8
#define TXB8 0
#endif
#ifndef UPE
#define UPE 2
#endif
#ifndef DOR
#define DOR 3
#endif
#ifndef FE
#define FE 4
#endif
#ifndef UDRE
#define UDRE 5
#endif
#ifndef RXC
#define RXC 7

#endif
#define
#define
#define
#define
#define

FRAMING_ERROR (1<<FE)
PARITY_ERROR (1<<UPE)
DATA_OVERRUN (1<<DOR)
DATA_REGISTER_EMPTY (1<<UDRE)
RX_COMPLETE (1<<RXC)

// USART1 Receiver buffer

#define RX_BUFFER_SIZE1 8
char rx_buffer1[RX_BUFFER_SIZE1];
#if RX_BUFFER_SIZE1<256
unsigned char rx_wr_index1,rx_rd_index1,rx_counter1;
#else
unsigned int rx_wr_index1,rx_rd_index1,rx_counter1;
#endif
// This flag is set on USART1 Receiver buffer overflow
bit rx_buffer_overflow1;
unsigned char ready = 0, get = 0,it,Dot, gerak = 0,d = 0;
int jmlTitik = 0, titik = 0;
int DataX[3],DataY[3];
int X[10], Y[10];
// USART1 Receiver interrupt service routine
interrupt [USART1_RXC] void usart1_rx_isr(void)
{
char status,data;
status=UCSR1A;
data=UDR1;
if(data == 'A'){
ready = 1;get = 1;it=0,Dot
}else if(data == 'B'){
ready = 1;get = 1;it=0,Dot
}else if(data == 'C'){
ready = 1;get = 1;it=0,Dot
}else if(data == 'D'){
ready = 1;get = 1;it=0,Dot
}else if(data == 'E'){
ready = 1;get = 1;it=0,Dot
}else if(data == 'F'){
ready = 1;get = 1;it=0,Dot
}else if(data == 'G'){
ready = 1;get = 1;it=0,Dot
}else if(data == 'O'){
ready = 1;get = 1;it=0,Dot
}else if(data == 'P'){
ready = 1;get = 1;it=0,Dot
}else if(data == 'Q'){
ready = 1;get = 1;it=0,Dot
}else if(data == 'R'){
ready = 1;get = 1;it=0,Dot
}else if(data == 'S'){
ready = 1;get = 1;it=0,Dot
}else if(data == 'T'){
ready = 1;get = 1;it=0,Dot
}else if(data == 'U'){
ready = 1;get = 1;it=0,Dot
}else if(data == 'V'){

= 1;
= 2;
= 3; d = 1;
= 1;
= 2;
= 3;
= 4; d = 2;
= 1;
= 2;
= 3;
= 4;
= 5; d = 3;
= 1;
= 2;

ready = 1;get = 1;it=0,Dot = 3;

}else if(data == 'W'){
ready = 1;get = 1;it=0,Dot = 4;
}else if(data == 'X'){
ready = 1;get = 1;it=0,Dot = 5;
}else if(data == 'Y'){
ready = 1;get = 1;it=0,Dot = 6; d = 4;
}else if(data == 'H'){
ready = 1;get = 1;it=0,Dot = 1;
}else if(data == 'I'){
ready = 1;get = 1;it=0,Dot = 2;
}else if(data == 'J'){
ready = 1;get = 1;it=0,Dot = 3;
}else if(data == 'K'){
ready = 1;get = 1;it=0,Dot = 4;
}else if(data == 'L'){
ready = 1;get = 1;it=0,Dot = 5;
}else if(data == 'M'){
ready = 1;get = 1;it=0,Dot = 6;
}else if(data == 'N'){
ready = 1;get = 1;it=0,Dot = 7; d = 5;
}else if(ready == 1 && gerak == 0){
if(data == ','){
get = 2;it=0;
}else if(data == '!'){
ready = 0;
X[Dot] = (DataX[0])*100 + (DataX[1])*10 + (DataX[2]) - 150;
Y[Dot] = (DataY[0])*100 + (DataY[1])*10 + (DataY[2]);
if(d == 1){
gerak = 1; jmlTitik = 3;
}else if(d == 2){
gerak = 1; jmlTitik = 4;
}else if(d == 3){
gerak = 1; jmlTitik = 5;
}else if(d == 4){
gerak = 1; jmlTitik = 6;
}else if(d == 5){
gerak = 1; jmlTitik = 7;
}
}else if(get == 1){
DataX[it] = data-48;
it ++;
}else if(get == 2){
DataY[it] = data-48;
it ++;
}
}
if ((status & (FRAMING_ERROR | PARITY_ERROR | DATA_OVERRUN))==0)
{
rx_buffer1[rx_wr_index1]=data;
if (++rx_wr_index1 == RX_BUFFER_SIZE1) rx_wr_index1=0;
if (++rx_counter1 == RX_BUFFER_SIZE1)
{
rx_counter1=0;
rx_buffer_overflow1=1;
};
};
}

// Get a character from the USART1 Receiver buffer

#pragma used+
char getchar1(void)
{
char data;
while (rx_counter1==0);
data=rx_buffer1[rx_rd_index1];
if (++rx_rd_index1 == RX_BUFFER_SIZE1) rx_rd_index1=0;
#asm("cli")
--rx_counter1;
#asm("sei")
return data;
}
#pragma used// Write a character to the USART1 Transmitter
#pragma used+
void putchar1(char c)
{
while ((UCSR1A & DATA_REGISTER_EMPTY)==0);
UDR1=c;
}
#pragma used// Standard Input/Output functions
#include <stdio.h>
// length definition for each part
#define coxa_l 95
#define femur_l 93
#define tibia_l 115
#define
#define
#define
#define
#define

L1 95
L2 93
L3 115
Rad 57.3248407643
ResServo 3.413

float femur_ang,tibia_ang,se,a1,a2,a3;
char buffer[16];
int a,flag,flag2=0;
void leg_kinematic(float x,float y)
{
se = sqrt(((x-coxa_l)*(x-coxa_l)) + (y*y));
a1 = atan2((x-coxa_l),y);
a2 = acos((se*se + femur_l*femur_l - tibia_l*tibia_l ) / (2 * femur_l *
se));
femur_ang = 90-(a1 + a2)*180/3.14;
tibia_ang = 180-(acos(((femur_l*femur_l)+(tibia_l*tibia_l)-(se*se))/(2*f
emur_l*tibia_l))*180/3.14);
}
// Declare your global variables here
void SetPosition(unsigned char ID,unsigned int GoalPosition)
{
unsigned int x,y;
putchar(0xFF); //header 1
putchar(0xFF); //header 2

y = ID; x =y;
putchar(y); //ID
y = 5; x +=y;
putchar(y); //Panjang Paket (length)
y = 0x03; x +=y;
putchar(y); //INS Write
y = 0x1e; x +=y;
putchar(y); //Address
y = GoalPosition & 255; x +=y;
putchar(y); //Data Low
y = GoalPosition >> 8; x +=y;
putchar(y); //Data high
putchar(~x);
}
float xp, yp;
float Res, sudut1, sudut2, sudut3;
void Convert2Angle(){
int data1, data2, data3;
data1 = 512 + (int)(sudut1 * ResServo);
data2 = 512 + (int)(sudut2 * ResServo);
data3 = 512 + (int)(sudut3 * ResServo);
SetPosition(1, data1);
SetPosition(2, data2);
SetPosition(3, data3);
SetPosition(4, 530);
}
void InverseKinematic(int x, int y, float angle){
xp = (float)x + L3 * sin(angle/Rad);
yp = (float)y - L3 * cos(angle/Rad);
Res = sqrt(xp * xp + yp * yp);
a1 = atan2(xp, yp)*Rad;
a2 = acos((Res*Res - L2 * L2 - L1 * L1)/(2*L1*L2))*Rad;
a3 = asin((L2/Res)*sin(a2/Rad))*Rad;
sudut1 = (a1 + a3);
sudut2 = -a2;
sudut3 = angle + (sudut2 + sudut1);
Convert2Angle();
}
int t,x0=-50,y0=240,x1=100,y1=180,last_y=0,last_x=0, itr=100;
float angle;
void Trajectory(){
if(t > itr){
x0 = last_x;
y0 = last_y;
x1 = X[titik];
y1 = Y[titik];
titik ++;
if(titik > jmlTitik)titik = 1;
t = 0;
}else{
last_x = x0 + ((x1 - x0)*t)/itr;
last_y = y0 + ((y1 - y0)*t)/itr;
t++;
}

angle = atan2((float)(last_x-80),(float)(last_y-100))*Rad;
InverseKinematic(last_x, last_y, -angle);
}
void main(void)
{
// Declare your local variables here
// Input/Output Ports initialization
// Port A initialization
// Func7=In Func6=In Func5=In Func4=In Func3=In Func2=In Func1=In Func0=In
// State7=T State6=T State5=T State4=T State3=T State2=T State1=T State0=T
PORTA=0x00;
DDRA=0x00;
// Port B initialization
// Func7=In Func6=In Func5=In Func4=In Func3=In Func2=In Func1=In Func0=In
// State7=T State6=T State5=T State4=T State3=T State2=T State1=T State0=T
PORTB=0x00;
DDRB=0x00;
// Port C initialization
// Func7=In Func6=In Func5=In Func4=In Func3=In Func2=In Func1=In Func0=In
// State7=T State6=T State5=T State4=T State3=T State2=T State1=T State0=T
PORTC=0x00;
DDRC=0x00;
// Port D initialization
// Func7=In Func6=In Func5=In Func4=In Func3=In Func2=In Func1=In Func0=In
// State7=T State6=T State5=T State4=T State3=T State2=T State1=T State0=T
PORTD=0x00;
DDRD=0x00;
// Port E initialization
// Func7=In Func6=In Func5=In Func4=In Func3=In Func2=In Func1=In Func0=In
// State7=T State6=T State5=T State4=T State3=T State2=T State1=T State0=T
PORTE=0x00;
DDRE=0x00;
// Port F initialization
// Func7=In Func6=In Func5=In Func4=In Func3=In Func2=In Func1=In Func0=In
// State7=T State6=T State5=T State4=T State3=T State2=T State1=T State0=T
PORTF=0x00;
DDRF=0x00;
// Port G initialization
// Func4=In Func3=In Func2=In Func1=In Func0=In
// State4=T State3=T State2=T State1=T State0=T
PORTG=0x00;
DDRG=0x00;
// Timer/Counter 0 initialization
// Clock source: System Clock
// Clock value: Timer 0 Stopped
// Mode: Normal top=FFh
// OC0 output: Disconnected
ASSR=0x00;
TCCR0=0x00;
TCNT0=0x00;

OCR0=0x00;
// Timer/Counter 1 initialization
// Clock source: System Clock
// Clock value: Timer1 Stopped
// Mode: Normal top=FFFFh
// OC1A output: Discon.
// OC1B output: Discon.
// OC1C output: Discon.
// Noise Canceler: Off
// Input Capture on Falling Edge
// Timer1 Overflow Interrupt: Off
// Input Capture Interrupt: Off
// Compare A Match Interrupt: Off
// Compare B Match Interrupt: Off
// Compare C Match Interrupt: Off
TCCR1A=0x00;
TCCR1B=0x00;
TCNT1H=0x00;
TCNT1L=0x00;
ICR1H=0x00;
ICR1L=0x00;
OCR1AH=0x00;
OCR1AL=0x00;
OCR1BH=0x00;
OCR1BL=0x00;
OCR1CH=0x00;
OCR1CL=0x00;
// Timer/Counter 2 initialization
// Clock source: System Clock
// Clock value: Timer2 Stopped
// Mode: Normal top=FFh
// OC2 output: Disconnected
TCCR2=0x00;
TCNT2=0x00;
OCR2=0x00;
// Timer/Counter 3 initialization
// Clock source: System Clock
// Clock value: Timer3 Stopped
// Mode: Normal top=FFFFh
// OC3A output: Discon.
// OC3B output: Discon.
// OC3C output: Discon.
// Noise Canceler: Off
// Input Capture on Falling Edge
// Timer3 Overflow Interrupt: Off
// Input Capture Interrupt: Off
// Compare A Match Interrupt: Off
// Compare B Match Interrupt: Off
// Compare C Match Interrupt: Off
TCCR3A=0x00;
TCCR3B=0x00;
TCNT3H=0x00;
TCNT3L=0x00;
ICR3H=0x00;
ICR3L=0x00;
OCR3AH=0x00;
OCR3AL=0x00;

OCR3BH=0x00;
OCR3BL=0x00;
OCR3CH=0x00;
OCR3CL=0x00;
// External Interrupt(s) initialization
// INT0: Off
// INT1: Off
// INT2: Off
// INT3: Off
// INT4: Off
// INT5: Off
// INT6: Off
// INT7: Off
EICRA=0x00;
EICRB=0x00;
EIMSK=0x00;
// Timer(s)/Counter(s) Interrupt(s) initialization
TIMSK=0x00;
ETIMSK=0x00;
// USART0 initialization
// Communication Parameters: 8 Data, 1 Stop, No Parity
// USART0 Receiver: Off
// USART0 Transmitter: On
// USART0 Mode: Asynchronous
// USART0 Baud Rate: 1000000
UCSR0A=0x00;
UCSR0B=0x08;
UCSR0C=0x06;
UBRR0H=0x00;
UBRR0L=0x00;
// USART1 initialization
// Communication Parameters: 8 Data, 1 Stop, No Parity
// USART1 Receiver: On
// USART1 Transmitter: On
// USART1 Mode: Asynchronous
// USART1 Baud Rate: 9600
UCSR1A=0x00;
UCSR1B=0x98;
UCSR1C=0x06;
UBRR1H=0x00;
UBRR1L=0x67;
// Analog Comparator initialization
// Analog Comparator: Off
// Analog Comparator Input Capture by Timer/Counter 1: Off
ACSR=0x80;
SFIOR=0x00;
// LCD module initialization
lcd_init(16);
// Global enable interrupts
#asm("sei")
x0 = 0;
y0 = 200;
x1 = 0;

y1 = 200;
X[0]=0;
Y[0]=200;
titik = 0;
jmlTitik=1;
while (1)
{
// Place your code here
/* if(flag2==0){
if(a>=100){
SetPosition(4,310);
delay_ms(1000);
flag2=1;
a=-100;
}
if(a<=-100) flag=0;
if(flag==0) {
a++;
SetPosition(4,520);
}
leg_kinematic(210,a);
}
if(flag2==1){
if(a>=100) {
SetPosition(4,310);
delay_ms(1000);
flag2=3;
a=-100;
}
if(a<=-100) flag=0;
if(flag==0) {
a++;
SetPosition(4,C);
}
leg_kinematic(260,a);
}
delay_ms(10);
SetPosition(1,512-300+(300/90*femur_ang));
SetPosition(2,512-(300/90*tibia_ang));
SetPosition(3,512);
*/
//InverseKinematic(80,200,0);
if(gerak == 1){
Trajectory();
}
lcd_gotoxy(0,0);
sprintf(buffer,"%d %d %d %d %d %d %d",X[1],X[2],X[3],X[4],X[5],X[6],X[7]
);
//sprintf(buffer,"%d %d %d ",DataX[0],DataX[1],DataX[2]);
lcd_puts(buffer);
lcd_gotoxy(0,1);
sprintf(buffer,"%d %d %d %d %d %d %d",Y[1],Y[2],Y[3],Y[4],Y[5],Y[6],Y[7]
);
lcd_puts(buffer);
delay_ms(1);

};
}