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:
// 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,5 9,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,6 7,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,3 9,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,7 0,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,5 9,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,1 04,104,104,104,104,104,104,104,104,105,105,105,105 ,105,105,105,105,105,105,105,105,105,105,105,105,1 05,106,106,106,106,106,106,106,106,106,106,106,106 ,106,106,106,106,107,107,107,107,107,107,107,107,1 07,107,107,107,107,107,107,107,107,107,107,107,107 ,107,107,107,107,107,107,107,107,107,107,107,107,1 07,107,107,107,107,107,107,107,107,107,107,107,107 ,107,109,111,113,115,117,119,121,123,125,127,129,1 31,134,136,139,141,144,147,150,153,150,147,144,141 ,139,136,134,131,129,127,125,123,121,119,117,115,1 13,111,109,107,107,107,107,107,107,107,107,107,107 ,107,107,107,107,107,107,107,107,107,107,107,107,1 07,107,107,107,107,107,107,107,107,107,107,107,107 ,107,107,107,107,107,107,107,107,107,107,107,107,1 06,106,106,106,106,106,106,106,106,106,106,106,106 ,106,106,106,105,105,105,105,105,105,105,105,105,1 05,105,105,105,105,105,105,105,104,104,104,104,104 ,104,104,104,104,104,104,104,104,104,104,104,104,1 04,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,1 09,110,111,111,112,112,113,114,114,115,115,116,117 ,117,118,118,119,119,120,120,121,121,122,123,123,1 24,124,125,125,125,126,126,127,127,128,128,129,129 ,130,130,130,131,131,132,132,132,133,133,134,134,1 34,135,135,135,136,136,136,137,137,137,138,138,138 ,139,139,139,140,140,140,140,141,141,141,140,138,1 37,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,4 1,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,6 2,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 (http://www.youtube.com/watch?v=GsiAz_v4yJ4)
[flash width=425 height=350 loop=false:4126145f67]http://www.youtube.com/v/GsiAz_v4yJ4[/flash:4126145f67]
mfg WarChild