/*    TESTS zur inversen kinematik */



/*					Servotest.c  V1.2 by hopix								*/
/*																			*/
/*		!!! Achtung geänderter prescalar verbesserte auflösung				*/
/*																			*/
/* Interruptroutine Timer 1 für ATmega8 und max 20 Servos  evtl auch max 23 */
/* ATmega8 auf 8 MHZ interner oszilator einstellen 							*/
/* prozessorbelastung ca. 12ms(ohne optimierung)							*/ 
/*                        6 ms (mit optimirung)								*/
/* interrupt alle 20 ms 													*/
/* einfach nach dem letzten servo adresse 0x00 in der funktion 				*/
/* 									servoset_generell angeben 				*/
/* als postitionsangabe servo_rel[servonummer].pos 0-255 angeben (127 Mitte)*/
/* max/minimalausschlag des servos in servoset_generel angeben 				*/
/* eventuelle korrekturen bitte nach initialisierung in servorel[].max / min*/
/*  vornehmen 																*/
/* Port ausgangsbelegung in servo_gen[].adr  die adresse der Ports angeben  */
/* Pin ausgangsbelegung in servo_gen[].adr  die pinnummer angeben 			*/
/* mit int_erfolgt kann man das eigene programm an den interrutpablauf 		*/
/* koppeln																	*/
/* int_erfolgt wir nach jedem interrupt erneut gesetzt bitte im prg löschen */


#include <avr/io.h>
#include <avr/interrupt.h>
#include <math.h>

#define maxservos 20+1 /*anzahl der servos (+1 für endekennzeichnung)*/

#define servofrequenz 0x5000 /* 19,88 ms 0x0a00 bei prescalar 64*/

/* Servovorgaben */
#define servostandard_min 0x408
#define servostandard_max 0x80d
#define carsm_min 0x0280 /*absolut min 0x0050 carson mini 620 uS*/
#define carsm_max 0x0980 /*absolut max 0x0130 carson mini 2360 uS*/

volatile uint8_t int_erfolgt = 0;

struct _Servoliste_generell
{	uint8_t adr;
	uint8_t pin;
	uint16_t absmax;
	uint16_t absmin;
	
} servo_gen[maxservos+1];

struct _Servo_rel
{	uint8_t ein;
	uint8_t pos; /* zum test von 16 auf 8*/
	uint16_t max;
	uint16_t min;

}volatile servo_rel[maxservos+1];

struct _Servo_abs /* wird von interrupt umsortiert deshalb nummer nötig */
{	uint8_t nr;
	uint16_t pos;
	uint16_t start; /*timerzeit beim einschalten */
} servo_abs[maxservos+1];


void servoset_generell (void)
{	uint8_t count;

	servo_gen[0].adr =0x15; /*PORTC (als _SFR_IO8(0x15) benutzen*/
	servo_gen[0].pin = PC0;
	servo_gen[0].absmax =carsm_max;
	servo_gen[0].absmin =carsm_min;

	servo_gen[1].adr =0x15; /*PORTC (als _SFR_IO8(0x15) benutzen*/
	servo_gen[1].pin =PC1;
	servo_gen[1].absmax =carsm_max;
	servo_gen[1].absmin =carsm_min;

	servo_gen[2].adr =0x15; /*PORTC (als _SFR_IO8(0x15) benutzen*/
	servo_gen[2].pin =PC2;
	servo_gen[2].absmax =carsm_max;
	servo_gen[2].absmin =carsm_min;

	servo_gen[3].adr =0x15; /*PORTC (als _SFR_IO8(0x15) benutzen*/
	servo_gen[3].pin =PC3;
	servo_gen[3].absmax =carsm_max;
	servo_gen[3].absmin =carsm_min;

	servo_gen[4].adr =0x15; /*PORTC (als _SFR_IO8(0x15) benutzen*/
	servo_gen[4].pin =PC4;
	servo_gen[4].absmax =carsm_max;
	servo_gen[4].absmin =carsm_min;

	servo_gen[5].adr =0x15; /*PORTC (als _SFR_IO8(0x15) benutzen*/
	servo_gen[5].pin =PC5;
	servo_gen[5].absmax =carsm_max;
	servo_gen[5].absmin =carsm_min;
	
	servo_gen[6].adr =0x18; /*PORTB (als _SFR_IO8(0x18) benutzen*/
	servo_gen[6].pin =PB0;
	servo_gen[6].absmax =carsm_max;
	servo_gen[6].absmin =carsm_min;
	
	servo_gen[7].adr =0x18; /*PORTB (als _SFR_IO8(0x18) benutzen*/
	servo_gen[7].pin =PB1;
	servo_gen[7].absmax =carsm_max;
	servo_gen[7].absmin =carsm_min;

	servo_gen[8].adr =0x18; /*PORTB (als _SFR_IO8(0x18) benutzen*/
	servo_gen[8].pin = PB2;
	servo_gen[8].absmax =carsm_max;
	servo_gen[8].absmin =carsm_min;

	servo_gen[9].adr =0x18; /*PORTB (als _SFR_IO8(0x18) benutzen*/
	servo_gen[9].pin =PB3;
	servo_gen[9].absmax =carsm_max;
	servo_gen[9].absmin =carsm_min;

	servo_gen[10].adr =0x18; /*PORTB (als _SFR_IO8(0x18) benutzen*/
	servo_gen[10].pin =PB4;
	servo_gen[10].absmax =carsm_max;
	servo_gen[10].absmin =carsm_min;

	servo_gen[11].adr =0x18; /*PORTB (als _SFR_IO8(0x18) benutzen*/
	servo_gen[11].pin =PB5;
	servo_gen[11].absmax =carsm_max;
	servo_gen[11].absmin =carsm_min;

	servo_gen[12].adr =0x18; /*PORTB (als _SFR_IO8(0x18) benutzen*/
	servo_gen[12].pin =PB6;
	servo_gen[12].absmax =carsm_max;
	servo_gen[12].absmin =carsm_min;

	servo_gen[13].adr =0x18; /*PORTB (als _SFR_IO8(0x18) benutzen*/
	servo_gen[13].pin =PB7;
	servo_gen[13].absmax =carsm_max;
	servo_gen[13].absmin =carsm_min;
	
	servo_gen[14].adr =0x18; /*PORTD (als _SFR_IO8(0x12) benutzen*/
	servo_gen[14].pin =PD2;  /* mit adresse getrickst*/
	servo_gen[14].absmax =carsm_max;
	servo_gen[14].absmin =carsm_min;
	
	servo_gen[15].adr =0x18; /*PORTD (als _SFR_IO8(0x12) benutzen*/
	servo_gen[15].pin =PD3;/* mit adresse getrickst*/
	servo_gen[15].absmax =carsm_max;
	servo_gen[15].absmin =carsm_min;

	servo_gen[16].adr =0x18; /*PORTB (als _SFR_IO8(0x12) benutzen*/
	servo_gen[16].pin =PD4;/* mit adresse getrickst*/
	servo_gen[16].absmax =carsm_max;
	servo_gen[16].absmin =carsm_min;

	servo_gen[17].adr =0x18; /*PORTB (als _SFR_IO8(0x12) benutzen*/
	servo_gen[17].pin =PD5;/* mit adresse getrickst*/
	servo_gen[17].absmax =carsm_max;
	servo_gen[17].absmin =carsm_min;
	
	servo_gen[18].adr =0x18; /*PORTD (als _SFR_IO8(0x12) benutzen*/
	servo_gen[18].pin =PD6;/* mit adresse getrickst*/
	servo_gen[18].absmax =carsm_max;
	servo_gen[18].absmin =carsm_min;
	
	servo_gen[19].adr =0x18; /*PORTD (als _SFR_IO8(0x12) benutzen*/
	servo_gen[19].pin =PD7;/* mit adresse getrickst*/
	servo_gen[19].absmax =carsm_max;
	servo_gen[19].absmin =carsm_min;

	
	servo_gen[20].adr =0x00; /*adresse 0x00 als ende kennzeichen */
	servo_gen[20].pin =PC0;/* mit adresse getrickst*/
	servo_gen[20].absmax =carsm_max;
	servo_gen[20].absmin =carsm_min;
	
	/* hier erstmal die grundeistellung machen */
	for (count = 0;servo_gen[count].adr != 0x00; count++)
	{	servo_rel[count].ein =1;
		servo_rel[count].max =servo_gen[count].absmax;
		servo_rel[count].min =servo_gen[count].absmin;
		servo_rel[count].pos = (uint16_t) 127; /*alle servos auf mitte*/
	}
	servo_rel[count].ein =0x00;
}


uint16_t serv_abs(uint16_t max, uint16_t min, uint16_t pos);

void DPinReset (uint8_t adr,uint8_t pin);
void DPinSet (uint8_t adr,uint8_t pin);

/* vorgaben zum start */
double b0_winkel1 =0;
double b0_winkel2 =0;
double b0_winkel3 =90;
double b1_winkel1 =0;
double b1_winkel2 =0;
double b1_winkel3 =90;
double b2_winkel1 =0;
double b2_winkel2 =0;
double b2_winkel3 =90;
double b3_winkel1 =0;
double b3_winkel2 =0;
double b3_winkel3 =90;
double b4_winkel1 =0;
double b4_winkel2 =0;
double b4_winkel3 =90;
double b5_winkel1 =0;
double b5_winkel2 =0;
double b5_winkel3 =90;
/* feste werte*/
double schenkel2 = 100;
double offsetb = 20;
static double faktor_degree = (180/M_PI);
/* startpositionen*/
double b0_posb = 100;
double b0_posh = 100;
double b0_posl = 0;
double b1_posb = 100;
double b1_posh = 100;
double b1_posl = 0;
double b2_posb = 100;
double b2_posh = 100;
double b2_posl = 0;
double b3_posb = 100;
double b3_posh = 100;
double b3_posl = 0;
double b4_posb = 100;
double b4_posh = 100;
double b4_posl = 0;
double b5_posb = 100;
double b5_posh = 100;
double b5_posl = 0;



void winkel_berechnen (double l,double b,double h,char bein)
{	/* in länge breite höhe */
	double w1,w2,w3, ZW;
	
	
	ZW = sqrt(l*l+(b-offsetb)*(b-offsetb));
	w1 = asin (l/ZW)*faktor_degree; /*erscheint IO*/
	
	ZW = sqrt(h*h+ZW*ZW);/* schräge lage b-h*/
	
	w3 = 2*asin((ZW/2)/schenkel2)*faktor_degree; /*erscheint IO*/
	w2 = 180-90-(w3/2)-asin(h/ZW)*faktor_degree;/* erscheint IO*/
	
	switch (bein)
	{	case 0:
			b0_winkel1 = w1;
			b0_winkel2 = w2;
			b0_winkel3 = w3;
			break;
		case 1:
			b1_winkel1 = w1;
			b1_winkel2 = w2;
			b1_winkel3 = w3;
			break;
		case 2:
			b2_winkel1 = w1;
			b2_winkel2 = w2;
			b2_winkel3 = w3;
			break;
		case 3:
			b3_winkel1 = w1;
			b3_winkel2 = w2;
			b3_winkel3 = w3;
			break;
		case 4:
			b4_winkel1 = w1;
			b4_winkel2 = w2;
			b4_winkel3 = w3;
			break;
		case 5:
			b5_winkel1 = w1;
			b5_winkel2 = w2;
			b5_winkel3 = w3;
			break;
		default:
			;

	}
}

void servopos(char bein)
{
	switch (bein)
	{	case (0):
			servo_rel[0].pos =(b0_winkel1/180*255)+127; /* 0grad in mittellage*/
			servo_rel[1].pos =(b0_winkel2/180*255)+127; /* 0grad in mittellage */
			servo_rel[2].pos =(b0_winkel3/180*255)*-1; /* drehrichtungsumkehr */
			break;
		case (1):
			servo_rel[3].pos =(b1_winkel1/180*255)+127; /* 0grad in mittellage*/
			servo_rel[4].pos =(b1_winkel2/180*255)+127; /* 0grad in mittellage */
			servo_rel[5].pos =(b1_winkel3/180*255)*-1; /* drehrichtungsumkehr */
			break;
		case (2):
			servo_rel[6].pos =(b2_winkel1/180*255)+127; /* 0grad in mittellage*/
			servo_rel[7].pos =(b2_winkel2/180*255)+127; /* 0grad in mittellage */
			servo_rel[8].pos =(b2_winkel3/180*255)*-1; /* drehrichtungsumkehr */
			break;
		case (3):
			servo_rel[9].pos =(b3_winkel1/180*255)+127; /* 0grad in mittellage*/
			servo_rel[10].pos =(b3_winkel2/180*255)+127; /* 0grad in mittellage */
			servo_rel[11].pos =(b3_winkel3/180*255)*-1; /* drehrichtungsumkehr */
			break;
		case (4):
			servo_rel[12].pos =(b4_winkel1/180*255)+127; /* 0grad in mittellage*/
			servo_rel[13].pos =(b4_winkel2/180*255)+127; /* 0grad in mittellage */
			servo_rel[14].pos =(b4_winkel3/180*255)*-1; /* drehrichtungsumkehr */
			break;
		case (5):
			servo_rel[12].pos =(b5_winkel1/180*255)+127; /* 0grad in mittellage*/
			servo_rel[13].pos =(b5_winkel2/180*255)+127; /* 0grad in mittellage */
			servo_rel[14].pos =(b5_winkel3/180*255)*-1; /* drehrichtungsumkehr */
			break;
			

		default:
			;
	}

}


int main(void)
{
/*testvariabelen */
	DDRB = 0xff;
	DDRD = 0x00;

	winkel_berechnen(0,100,100,1);


	servoset_generell ();	/* servovoreinstellungen */
  
  	OCR1A = servofrequenz ; /* compare A 5000*/
	TCCR1B = 0b00001010; 	/* timer = 1 bei compare A prescalar 1 00001010 */
	TIMSK  = 0b00010000; 	/* interrupt bei compare A*/
	sei ();
  
	while (1)
  	{	/* hier steht euer code */
	
		winkel_berechnen(b0_posl,b0_posb,b0_posh,0);
		servopos (0);
		winkel_berechnen(b1_posl,b1_posb,b1_posh,1);
		servopos (1);
		winkel_berechnen(b2_posl,b2_posb,b2_posh,2);
		servopos (2);
		winkel_berechnen(b3_posl,b3_posb,b3_posh,3);
		servopos (3);
		winkel_berechnen(b4_posl,b4_posb,b4_posh,4);
		servopos (4);
		winkel_berechnen(b5_posl,b5_posb,b5_posh,5);
		servopos (5);
		switch (PIND)
		{	case 0b11111110:
				b0_posl +=4;
				break;
			case 0b11111101:
				b0_posl -=4;
				break;
			case 0b11111011:
				b0_posb +=4;
				break;
			case 0b11110111:
				b0_posb -=4;
				break;
			case 0b11101111:
				b0_posh +=4;
				break;
			case 0b11011111:
				b0_posh -=4;
				break;
			case 0b10111111:
				b0_posb = 100;
				b0_posh = 100;
				b0_posl = 0;
				break;


		}


	}
}


/* code bei hopix */
ISR (TIMER1_COMPA_vect)
{	static uint8_t first =1;
	uint8_t count;
	uint8_t zw8;
	uint16_t zw16;
	uint8_t getauscht;

	if (first ==0)
	{	/* alle ports als ausgang und einschalten*/
		count = 0;
		while (servo_abs[count].nr != 0xff)
		{	DPinSet(servo_gen[servo_abs[count].nr].adr-1,servo_gen[servo_abs[count].nr].pin); /*Data Direction Register */
			DPinSet(servo_gen[servo_abs[count].nr].adr  ,servo_gen[servo_abs[count].nr].pin); /*Data Port Register */
			servo_abs[count].start = TCNT1;
			count++;
		}
		count =0;
		while (servo_abs[count].nr != 0xff)
		{	while (TCNT1 < (servo_abs[count].pos+servo_abs[count].start))
			{	;
			}
			DPinReset(servo_gen[servo_abs[count].nr].adr  ,servo_gen[servo_abs[count].nr].pin); /*Data Port Register */
			count++;
		}
	}
	
		
	 
	/* Werte umrechnen */ 

	for (count = 0;servo_gen[count].adr != 0x00; count++)
	{	if (servo_rel[count].ein != 0)
		{	servo_abs[count].nr = count;
			servo_abs[count].pos = serv_abs (servo_rel[count].max,servo_rel[count].min,servo_rel[count].pos);
		}
		else
		{	servo_abs[count].nr = 0xff;
			servo_abs[count].pos = 0xffff;
		}
	}
	servo_abs[count].nr = 0xff;
	servo_abs[count].pos = 0xffff;
	
	/* Werte sortieren bubblesortmethode*/
	getauscht =1; /* sollte raus könen*/
	do
	{	getauscht = 0;
		for (count = 0; count != maxservos-1; count++) 
		{	if (servo_abs[count].pos>servo_abs[count+1].pos)
			{	zw16=servo_abs[count].pos;
				zw8 =servo_abs[count].nr;
				servo_abs[count].pos=servo_abs[count+1].pos;
				servo_abs[count].nr =servo_abs[count+1].nr;
				servo_abs[count+1].pos=zw16;
				servo_abs[count+1].nr =zw8;
				getauscht ++;
			}
			else
				zw8=8;
		}
	}while (getauscht !=0);
	first = 0;/* markieren erster durchlauf erfolgt  servos einsatzbereit*/
	int_erfolgt = 1; /* zeigt dem hauptprogramm das der interrupt beendet wurde*/
					/* verwendung zum synchronisieren der servopositionen*/
}

uint16_t serv_abs(uint16_t max, uint16_t min, uint16_t pos)
{	return ((uint16_t)(((long)(max-min)*(long)pos/(long)255)+(long)min));
}

void DPinSet (uint8_t adr,uint8_t pin)
{	_SFR_IO8(adr) |= (1 << pin);
}

void DPinReset (uint8_t adr,uint8_t pin)
{	_SFR_IO8(adr) &= ~(1 << pin);
}