MeinHexapod: "SEXCRORA"

**WarChild** · 27.03.2009, 19:30

Also bisher machen die einen recht soliden und zeverlässigen eindruck. Der Schultetservo ist sicher gegengelagert, da passiert nix, der im Oberarm (der neben dem Schulterservo) hat eine gegenlagerung auf sehr kurzem Hebel, vlt muss ich zwei schultern nochmal bauen (*Kotz*), da das loch für die gegenlagerung nicht 100% auf höhe des Achsenzentrums war musste ich diese etwas auffeilen, was zu minimalem Spiel geführt hat. Und die Servos in den Unterarmen halten so lange wie Sie halten, da ist ein jahr Garantie drauf, ich hoffe sie gehen zuvor kaputt, dann hole ich mir schnell nochmal nen Satzt Gratisservos. Der eine hatte schon bei den ersten experiementen den Geist aufgegeben. (Zahnräder gebrochen, aber war garantiefall.)
mfg WarChild

**NXTbotFreak** · 06.04.2009, 07:41

Ich möchte ja nicht unverschämt klingen, aber die Bidqualität ist wirklich nicht gut. Aber trotzdem tut das dem Roboter keinen Abbruch. Der Roboter ist wirklich erste Sahne. Wie ist dein Roboter eigndlich Progammiert und in welcher Programiersprache (C, Assembler oa.)?

**WarChild** · 15.04.2009, 02:00

Teilweise stammen die Bilder aus meinem Handy, da die kamera im Auto auf Reisen war. Der Hauptteil läuft momentan in den Interrupts ab, das wird sich aber noch ändern. Er ist programmiert in C, weil es einfach mehr komfort für mich bietet, auch wenn es mich etwas Kontrolle kostet.

mfg WarChild

**WarChild** · 11.07.2009, 12:11

Hi zusammen!
endlich endet das Smester, die Klausuren folgen ende September, also war mal wieder etwas Zeit zum Weiterbasteln.

Jetzt sind die Berechnungen der IK von den Lokalen Koordinatensystemen, also aus Sicht der jeweiligen Beinaufhängung fertig.
Nun bruach ich nurnoch eine Vor und Rücktransformation der Lokalen Koordinaten in ein Globales System mit Zentrum im Mittelpunkt des Roboters, dann kann ich beliebige Translationen und Rotationen des Roboters im stand erzeugen.
Um einen hübschen Ablauf der Fußfolge beim gehen kümmer ich mich später. Noch ist es die 240 Werte Liste, daher das Rucken beim Anlaufen und Anhalten.

Hier der Code, welchen ich etwas kommentiert habe, und eigentlich sehr übersichtlich finde:

Code:

// Servos ansteuern mit 16MHz Mega32 und 8-Bit Timer0 und Timer2 (als ms stopwatch verwendbar) 

// Die Servosimpulse werden simultan erzeugt. Die Impulsdauer jedes Servos 
// besteht nur aus einem Wert, der zwischen 0 und 255 liegen muss. Der Stellbereich liegt dann bei
// ungefähr zwischen 50 und 230. --> ~180 werte für 180 grad, also eine auflösung bis auf ein Grad.

// In der ISR wird ein 1 ms Timer2 verwendet, um einen Zähler alle 20ms zurückzusetzen und den impulsmessenden Timer0 zu starten. 
// Während Timer0 läuft ist die maximale Auslastung: alle 160 Takte ein interupt, mit maximal 100 Befehlen 63% im Durchschnitt 30% nur für den ISR
// Daher wird der Timer0 auch diret nach der Pulserzeugung gestoppt und erst ~8ms später von Timer2 aufgerufen, da immer nur 9 Impulse simultan erzeugt werden können. 
//Mehr als 14 Signale gleichzeitig führten zu einer Verzerrung, aufgrund des großen Aufwandes im sehr häufg aufgerufenen ISR. 
// Während der Pause läuft also nichts außer einem 1ms Timer. Viel Zeit um andere Dinge zu erldigen. 


//**************************************************//
//Includes:											//
#include <avr/io.h> 								//
#include <avr/interrupt.h>							//
#include <math.h>						 			//
//													//
//**************************************************//

//**************************************************//
//variable definitions:								//
#define tmin 		50								//minimale impulsdauer 50*8µs= ~0,4ms //1,5ms, sind offset des calibrierungsarrays
#define ISRdelay 	19								//anzahl der takte, die dem Timer im ISR verloren gehen
#define APLHA		45								//Erstbelegung der Winkel
#define BETA		135								//
#define GAMMA		90								//
#define INITPULSE	125								//Erstbelegung der Servoimpulsdauern
//													//
//**************************************************//


//**************************************************//
// Servoausgänge 1-18 
// PA0 Servo 12, PB0-4 Servo 11/10/7/8/9, PC2-7 Servo 4/5/6/3/2/1, PD2-7 Servo 15/14/13/16/17/18
#define DDRAinit {	DDRA = 0b00000001;DDRB = 0b00011111;DDRC = 0b11111100;DDRD = 0b11111100;} 
#define servo1on  PORTC |=  (1<<PC7) 
#define servo1off PORTC &= ~(1<<PC7) 
#define servo2on  PORTC |=  (1<<PC6) 
#define servo2off PORTC &= ~(1<<PC6) 
#define servo3on  PORTC |=  (1<<PC5) 
#define servo3off PORTC &= ~(1<<PC5) 
#define servo4on  PORTC |=  (1<<PC2) 
#define servo4off PORTC &= ~(1<<PC2) 
#define servo5on  PORTC |=  (1<<PC3)
#define servo5off PORTC &= ~(1<<PC3) 
#define servo6on  PORTC |=  (1<<PC4) 
#define servo6off PORTC &= ~(1<<PC4) 
#define servo7on  PORTB |=  (1<<PB2) 
#define servo7off PORTB &= ~(1<<PB2) 
#define servo8on  PORTB |=  (1<<PB3) 
#define servo8off PORTB &= ~(1<<PB3) 
#define servo9on  PORTB |=  (1<<PB4) 
#define servo9off PORTB &= ~(1<<PB4) 
#define servo10on  PORTB |=  (1<<PB1) 
#define servo10off PORTB &= ~(1<<PB1) 
#define servo11on  PORTB |=  (1<<PB0) 
#define servo11off PORTB &= ~(1<<PB0) 
#define servo12on  PORTA |=  (1<<PA0) 
#define servo12off PORTA &= ~(1<<PA0) 
#define servo13on  PORTD |=  (1<<PD4) 
#define servo13off PORTD &= ~(1<<PD4) 
#define servo14on  PORTD |=  (1<<PD3) 
#define servo14off PORTD &= ~(1<<PD3) 
#define servo15on  PORTD |=  (1<<PD2) 
#define servo15off PORTD &= ~(1<<PD2) 
#define servo16on  PORTD |=  (1<<PD5) 
#define servo16off PORTD &= ~(1<<PD5) 
#define servo17on  PORTD |=  (1<<PD6) 
#define servo17off PORTD &= ~(1<<PD6) 
#define servo18on  PORTD |=  (1<<PD7) 
#define servo18off PORTD &= ~(1<<PD7) 
//**************************************************//



//**************************************************//
//Global variables									//
uint8_t servo1, servo2, servo3, servo4, servo5, servo6; //Array für die calibrierte Signaldauer jedes Servos
uint8_t servo7, servo8, servo9, servo10, servo11, servo12;
uint8_t servo13, servo14, servo15, servo16, servo17, servo18; 
													//Array, in dem die Stellwinkel der Servos liegen
uint8_t angle[7][4];	
int16_t position[7][4];

volatile unsigned char	timer1 = 0;		//10µs Timer, kein konstanter Reset
volatile unsigned char 	timer2 = 20;	//1ms Timer, alle 20ms Reset
volatile uint16_t		timer3 = 0;		//Timer, der durch Timer 2 erhöht wird, für den nächsten schritt im Ab-Laufmodus
volatile unsigned char	groundpulse = 1;//merker ob der Grund- oder Signalpuls erzeugt wird
volatile unsigned char	signals = 0;	//kontrollwert für die momentan erzeugten Pulse
volatile unsigned char	channel = 0;	//Merker welche 9 signale gerade erzeugt werden
//**************************************************//



//**************************************************//
//Funktions:										//
void initiator(void);
void pulsecalculator(void);
void position2angle(void);
uint8_t setposition(int16_t x, int16_t y, int16_t z, uint16_t speed);
void walk(uint8_t speed, uint16_t footsteps);
inline float degree(float rad);
//**************************************************//



//**************************************************//
inline float degree(float rad)
{
	return ((rad/M_PI)*180);
}
//**************************************************//

//**************************************************//
inline int8_t sign(float number)
{
	if (number>=0)
		return 1;
	else
		return -1;
}
//**************************************************//


//**************************************************//
void pulsecalculator(void)							//Wandelt winkel in die entsprechenden Impulse um
{
									//15 sind vorausgesetzt (Offset, MinimalSignal + Grundimpuls)
	static uint8_t	calibrate[19] = {15,16,16,15,23,7,11,9,17,7,14,15,9,14,21,19,14,11,8};	//Mit diesem Array lassen sich minimale Fehlstelungen korrigieren
	static uint8_t	scaler = 170;		// scaler/128 ist das Verhältnis zwischen Grad und Impulsdauer
	servo1=calibrate[1] + ((180-angle[1][1])*scaler)/128;
	servo2=calibrate[2] + ((180-angle[1][2])*scaler)/128;
	servo3=calibrate[3] + ((180-angle[1][3])*scaler)/128;
	servo4=calibrate[4] + (angle[2][1]*scaler)/128;
	servo5=calibrate[5] + (angle[2][2]*scaler)/128;
	servo6=calibrate[6] + (angle[2][3]*scaler)/128;
	servo7=calibrate[7] + ((180-angle[3][1])*scaler)/128;
	servo8=calibrate[8] + ((180-angle[3][2])*scaler)/128;
	servo9=calibrate[9] + ((180-angle[3][3])*scaler)/128;
	servo10=calibrate[10] + (angle[4][1]*scaler)/128;
	servo11=calibrate[11] + (angle[4][2]*scaler)/128;
	servo12=calibrate[12] + (angle[4][3]*scaler)/128;
	servo13=calibrate[13] + ((180-angle[5][1])*scaler)/128;
	servo14=calibrate[14] + ((180-angle[5][2])*scaler)/128;
	servo15=calibrate[15] + ((180-angle[5][3])*scaler)/128;
	servo16=calibrate[16] + (angle[6][1]*scaler)/128;
	servo17=calibrate[17] + (angle[6][2]*scaler)/128;
	servo18=calibrate[18] + (angle[6][3]*scaler)/128;
}
//**************************************************//



//**************************************************//
void walk(uint8_t speed,uint16_t footsteps)							// speed should be between 10 and 50				
{
	static uint8_t alpha[241] = {58,58,58,58,58,58,58,58,58,58,58,58,58,58,58,59,59,59,59,59,59,59,59,59,59,59,59,60,60,60,60,60,60,60,60,61,61,61,61,61,61,62,62,62,62,62,63,63,63,63,63,64,64,64,64,64,65,65,65,65,66,66,66,66,67,67,67,67,68,68,68,69,69,69,70,70,70,70,71,71,71,72,72,72,73,73,74,74,74,75,75,75,76,76,77,77,77,78,78,78,79,76,72,69,66,63,61,58,55,53,50,48,46,44,42,40,39,37,36,35,34,35,36,37,39,40,42,44,46,48,50,53,55,58,61,63,66,69,72,76,79,78,78,78,77,77,77,76,76,75,75,75,74,74,74,73,73,72,72,72,71,71,71,70,70,70,70,69,69,69,68,68,68,67,67,67,67,66,66,66,66,65,65,65,65,64,64,64,64,64,63,63,63,63,63,62,62,62,62,62,61,61,61,61,61,61,60,60,60,60,60,60,60,60,59,59,59,59,59,59,59,59,59,59,59,59,58,58,58,58,58,58,58,58,58,58,58,58,58,58,58};
	static uint8_t beta[241]  = {104,104,104,104,104,104,104,104,104,104,104,104,104,104,104,104,104,104,104,104,104,105,105,105,105,105,105,105,105,105,105,105,105,105,105,105,105,105,106,106,106,106,106,106,106,106,106,106,106,106,106,106,106,106,107,107,107,107,107,107,107,107,107,107,107,107,107,107,107,107,107,107,107,107,107,107,107,107,107,107,107,107,107,107,107,107,107,107,107,107,107,107,107,107,107,107,107,107,107,107,107,109,111,113,115,117,119,121,123,125,127,129,131,134,136,139,141,144,147,150,153,150,147,144,141,139,136,134,131,129,127,125,123,121,119,117,115,113,111,109,107,107,107,107,107,107,107,107,107,107,107,107,107,107,107,107,107,107,107,107,107,107,107,107,107,107,107,107,107,107,107,107,107,107,107,107,107,107,107,107,107,107,107,107,107,107,107,106,106,106,106,106,106,106,106,106,106,106,106,106,106,106,106,105,105,105,105,105,105,105,105,105,105,105,105,105,105,105,105,105,104,104,104,104,104,104,104,104,104,104,104,104,104,104,104,104,104,104,104,104,104};
	static uint8_t gamma[241] = {90,91,91,92,93,94,94,95,96,96,97,98,99,99,100,101,101,102,103,103,104,105,105,106,107,107,108,109,109,110,111,111,112,112,113,114,114,115,115,116,117,117,118,118,119,119,120,120,121,121,122,123,123,124,124,125,125,125,126,126,127,127,128,128,129,129,130,130,130,131,131,132,132,132,133,133,134,134,134,135,135,135,136,136,136,137,137,137,138,138,138,139,139,139,140,140,140,140,141,141,141,140,138,137,135,133,131,129,127,125,122,119,117,114,111,107,104,101,97,94,90,86,83,79,76,73,69,66,63,61,58,55,53,51,49,47,45,43,42,40,39,39,39,40,40,40,40,41,41,41,42,42,42,43,43,43,44,44,44,45,45,45,46,46,46,47,47,48,48,48,49,49,50,50,50,51,51,52,52,53,53,54,54,55,55,55,56,56,57,57,58,59,59,60,60,61,61,62,62,63,63,64,65,65,66,66,67,68,68,69,69,70,71,71,72,73,73,74,75,75,76,77,77,78,79,79,80,81,81,82,83,84,84,85,86,86,87,88,89,90,90};
	
	
	static int16_t step[7] = {0};
	uint8_t i,j;
	uint16_t iterations=0;
	
	while (iterations<footsteps)
	{
		if(timer3 >= speed)
		{
			timer3=0;
			step[1]++;
			if(step[1] >= 240){step[1] = 0;iterations++;}
														//6 steps erzeugen    Beinfolge definieren
			step[2]=step[1]+3*40;
			step[3]=step[1]+4*40;
			step[4]=step[1]+1*40;
			step[5]=step[1]+2*40;
			step[6]=step[1]+5*40;
			for(i=2;i<=6;i++)							//Abfolge normieren auf 0-240	
			{
				if(step[i]>=240){step[i]=step[i]-240;}
			}
			for(j=1;j<=6;j++)		//durchlauf der Beine 1 bis 6 und belegung der winkel Alpha, Beta und Gamma
			{
				angle[j][1] = alpha[step[j]];
				angle[j][2] = beta[step[j]];
				angle[j][3] = gamma[step[j]];
			}
		}
		pulsecalculator();
	}
}
//**************************************************//



//**************************************************//
//Intitialisierung									//
void initiator(void)								//
{													//
	DDRAinit; 										// Datenrichtung der Servopins einstellen 
													//
	//Timer 0
	TCCR0 |= (1 << CS00);							//normal mode, prescaler 1
	//TIMSK |= (1 << TOIE0);						//Timer0 Interrupt freigegeben (wird von timer 1 freigegeben und vorgeladen)
	//TCNT0 = 256 - 128 + ISRdelay;					//Timer0 mit 147 neu vorladen 19 takte korrektur für den ISR: 128/16MHz = 8µs
													//
	//Timer 2
	TCCR2 |= (1 << CS22);							//normal mode, prescaler 64
	TCNT2  = 256-250;								//Timer2 mit 6 vorladen, 64/16MHz*250=1ms
	TIMSK |= (1 << TOIE2); 							//Timer2 Interrupt freigeben
	sei();											//Interrupts freigegeben
	//Impulsdauern initialisieren
	servo1=INITPULSE ; servo2=INITPULSE; servo3=INITPULSE; servo4=INITPULSE; servo5=INITPULSE; servo6=INITPULSE; 
	servo7=INITPULSE; servo8=INITPULSE; servo9=INITPULSE; servo10=INITPULSE; servo11=INITPULSE; servo12=INITPULSE; 
	servo13=INITPULSE; servo14=INITPULSE; servo15=INITPULSE; servo16=INITPULSE; servo17=INITPULSE; servo18=INITPULSE;
	//Positon initialiseren
	for(int j=1;j<=6;j++)
	{
		position[j][1] = 0;
		position[j][2] = 70;
		position[j][3] = -20;
	}
	position2angle();
}													//
//**************************************************//



//**************************************************//
uint8_t setposition(int16_t x, int16_t y, int16_t z, uint16_t speed)
{
	while (position[1][1] != x || position[1][2] != y || position[1][3] != z)
	{
		if(timer3 > speed)
		{
			timer3 = 0;
			if(x > position[1][1]) position[1][1]++;
			if(x < position[1][1]) position[1][1]--;
			if(y > position[1][2]) position[1][2]++;
			if(y < position[1][2]) position[1][2]--;
			if(z > position[1][3]) position[1][3]++;
			if(z < position[1][3]) position[1][3]--;
			
			for(uint8_t j=2;j<=6;j++)
			{
				position[j][1]=position[1][1];
				position[j][2]=position[1][2];
				/*if(j % 2 == 0)
					position[j][3]=-position[1][3];
				else*/
					position[j][3]=position[1][3];
			}
			
			position2angle();
		}
		pulsecalculator();
	}
	return 1;
}
//**************************************************//

//**************************************************//
inline void keepposition(uint16_t timems)
{
	timer3=0;
	while(timer3<timems)
		pulsecalculator();
	timer3=0;
}
//**************************************************//

//**************************************************//
void position2angle(void)
{
	const uint8_t l1=20, l2=100, l3=120;
	int16_t x,y,z;
	
	float b2,b1,s,q,s2,x2,y2,z2;
	for(uint8_t j=1;j<=6;j++)
	{
		x=position[j][1];
		y=position[j][2];
		z=position[j][3];
		x2=(float)x*x;
		y2=(float)y*y;
		z2=(float)z*z;
		q= sign((float)y)*sqrt(y2)-l1;
		s2= q*q+z2;	
		s= sqrt(s2);
		
		b1= degree(atan(q/z))+90*(1-sign(q*z));
		b2= degree(acos((s2+l2*l2-l3*l3)/(2*s*l2)));
		
		angle[j][1]= degree(acos((l2*l2+l3*l3-s2)/(2*l2*l3)));
		angle[j][2]= (1+sign(q))*90+b2-b1;
		angle[j][3]= degree(atan2(y,x));
		if(angle[j][3]>180 || angle[j][3]<0)
			angle[j][3]=90;
		
	}
}
//**************************************************//





int main(void) 
{ 

	initiator();
	keepposition(2000);
	
	for(uint8_t i=0;i<1;i++) 				// Hauptschleife 
	{ 
		/* 
		setposition(100,140,40,20);		// Viereck in die Luft malen
		keepposition(2000);
		setposition(100,140,-80,20);
		keepposition(2000);
		setposition(-100,140,-80,20);
		keepposition(2000);
		setposition(-100,140,40,20);
		keepposition(2000);*/			//Viereck ende
		

		setposition(0,100,-20,20);
		setposition(0,100,-80,40);
		keepposition(1000);
		setposition(0,100,-150,40);
		keepposition(2000);
		setposition(0,100,-80,20);
		setposition(50,100,-80,20);
		setposition(-50,100,-80,20);
		keepposition(2000);
		setposition(0,100,-80,20);
		setposition(0,100,-20,40);
		
		
		
		keepposition(4000);
		setposition(0,80,-20,20);
		setposition(0,80,-90,20);
		keepposition(2000);
		walk(20,4);			// standard quick walking mode (10 < speed < 50)
		position2angle();
		keepposition(2000);
		setposition(0,80,-20,60);
		

		
	} 
	while(1)
	{
	;
	}
	return 1; 
}


//***********************************Pulserzeugung + Timer***********************************//

SIGNAL (SIG_OVERFLOW2)		// 1ms Interrrupt
{
	TCNT2 = 256 - 250;		//Timer2 mit 6 neu vorladen
	timer2--;
	timer3++;
	if(timer2 == 0)
	{
		timer2 = 10;				//timer2 endet bei 10ms und startet je 9 signale im wechsel
		timer1 = 0;					//timer1 für den schnellen Timer0 zur Pulsgenerierung zurücksetzen
		TCNT0 = 256 - 128 + ISRdelay;		//Timer0 mit 147 neu vorladen 19 takte korrektur für den ISR: 128/16MHz = 8µs
		//alle Impulse starten
		if(channel)
		{
			servo1on;
			servo2on;
			servo3on;
			servo4on;
			servo5on;
			servo6on;
			servo7on;
			servo8on;
			servo9on;
		}
		else
		{
			servo10on;
			servo11on;
			servo12on;
			servo13on;
			servo14on;
			servo15on;
			servo16on;
			servo17on;
			servo18on;
		}
		TIMSK |= (1 << TOIE0);	//Timer0 Interrupt freigegeben
		signals = 9;			//Anzahl der laufenden Signale
	}
}



//**************************************************//
//Wird von Timer 2 freigegeben und beendet die durch Timer2 gestarteten Pulse

SIGNAL (SIG_OVERFLOW0)					//frisst ~70% der ProzessorLeistung
{
	TCNT0 = 256 - 128 + ISRdelay;		//Timer0 mit 147 neu vorladen 19 takte korrektur für den ISR 
	timer1++;							//timer1 Zähler misst die impulsdauer in 11µs Schritten (9,9µs timer den Rest schlucken die Instruktionen im ISR)
										// bei erreichen der Impulsdauer wird der jeweilige Impuls beendet
	if(groundpulse)
	{
		if(timer1 == tmin){groundpulse = 0; timer1 = 0;}				//wenn der grundpuls erzeugt wurde, wird der timer1 resettet
	}
	else
	{
		if(channel)
		{
			if(timer1 == servo1){servo1off;signals--;}
			if(timer1 == servo2){servo2off;signals--;}
			if(timer1 == servo3){servo3off;signals--;}
			if(timer1 == servo4){servo4off;signals--;}
			if(timer1 == servo5){servo5off;signals--;}
			if(timer1 == servo6){servo6off;signals--;}
			if(timer1 == servo7){servo7off;signals--;}
			if(timer1 == servo8){servo8off;signals--;}
			if(timer1 == servo9){servo9off;signals--;}
			if(signals == 0){TIMSK &= ~(1 << TOIE0);channel = 0;groundpulse = 1;}		//Timer0 stoppen und Kanal wechseln, sobald alle Impulse erzeugt sind
		}
		else
		{
			if(timer1 == servo10){servo10off;signals--;}
			if(timer1 == servo11){servo11off;signals--;}
			if(timer1 == servo12){servo12off;signals--;}
			if(timer1 == servo13){servo13off;signals--;}
			if(timer1 == servo14){servo14off;signals--;}
			if(timer1 == servo15){servo15off;signals--;}
			if(timer1 == servo16){servo16off;signals--;}
			if(timer1 == servo17){servo17off;signals--;}
			if(timer1 == servo18){servo18off;signals--;}
			if(signals == 0){TIMSK &= ~(1 << TOIE0);channel = 1;groundpulse = 1;}		//Timer0 stoppen und Kanal wechseln, sobald alle Impulse erzeugt sind
		}
	}
}
//**************************************************//

und ein video:

LINK
[flash width=425 height=350 loop=false:4126145f67]http://www.youtube.com/v/GsiAz_v4yJ4[/flash:4126145f67]
mfg WarChild

**BlackDevil** · 11.07.2009, 20:32

Die Bewegung gefällt mir sehr, schön gleichmäsig und das Baby hüpft nicht mit dem Rücken

**WarChild** · 13.07.2009, 00:21

Danke, aber was meinst du mit "das Baby hüpft nicht mit dem Rücken"?

Ich finde nur, die Auflösung der Signale könnte noch höher sein. Ein Grad ist echt für langsame Bewegungen etwas grobmotorisch.

mfg WarChild