Ok, das hab ich soweit verstanden. Hab auch gleichmal ein bisschen Code geschrieben:
#define F_CPU 8000000
#include <avr/delay.h>
#include <avr/interrupt.h>
#include <avr/signal.h>
#include <math.h>
#include <stdlib.h>
#include "uart.h"
// METHODEN DEKLARATIONEN
void keep_moving(void);
void move(int servo, int pos);
void calc_steps(void);
int signum(int val);
//VARIABLEN DEKLARATIONEN
volatile uint8_t servo_flag = 0;
volatile uint8_t recalc_flag = 1;
volatile uint8_t shoulder_pos = 127;
volatile uint8_t shoulder_dest = 255;
volatile float shoulder_step = 0;
volatile uint8_t elbow_pos = 127;
volatile uint8_t elbow_dest = 255;
volatile float elbow_step = 0;
int main (void) {
//UART AN
uart_init(1, 0);
//INTERUPT UND TIMER AN
sei();
TCCR0 |= (1<<CS02) | (1<<CS00);
TIMSK |= (1<<TOIE0);
while(1)
{
//WENN DIE EINMALE ANGEGEBENE POSITION ERREICHT WURDE
//DÜRFEN DIE STEPS NEU BERECHNET WERDEN
if(recalc_flag == 1) {
calc_steps();
}
//NUR AUFRUFEN, WENN EIN TIMER INTERUPT DA WAR
if(servo_flag == 1) {
keep_moving();
}
}
return 0;
}
void keep_moving() {
cli();
//NEUE POSITIONEN AUSRECHNEN UND RUNDEN
shoulder_pos = abs(shoulder_pos + shoulder_step);
elbow_pos = abs(elbow_pos + elbow_step);
//POSITIONEN SCHICKEN
move(1, shoulder_pos);
move(2, shoulder_pos);
move(3, elbow_pos);
//WENN DIE POSITIONEN GERUNDET ÜBEREINSTIMMEN
//NEUBERECHNUNG DER STEPS ERLAUBEN UND NEUES ZIEL FÜR ELBOW SETZEN
if(abs(shoulder_pos) == shoulder_dest && abs(elbow_pos) == elbow_dest) {
elbow_dest = 1;
recalc_flag = 1;
} else {
recalc_flag = 0;
}
servo_flag = 0;
sei();
}
void calc_steps(void) {
//BERECHNEN WIE VIEL SICH DIE SERVOS BEWEGEN MÜSSEN
uint16_t shoulder_to_go = shoulder_dest - shoulder_pos;
uint16_t elbow_to_go = elbow_dest - elbow_pos;
//HIER ALS ABSOLUTE WERTE
int16_t shoulder_abs = abs(shoulder_to_go);
int16_t elbow_abs = abs(elbow_to_go);
//STEP ERSTMAL AUF +/- 1 SETZEN, JE NACH RICHTUNG
elbow_step = signum(elbow_to_go);
shoulder_step = signum(shoulder_to_go);
if(elbow_abs < shoulder_abs) {
elbow_step = 1 * signum(elbow_to_go);
shoulder_step = (shoulder_abs / elbow_abs) * signum(shoulder_to_go);
}
if(elbow_abs > shoulder_abs) {
shoulder_step = 1 * signum(shoulder_to_go);
elbow_step = elbow_abs / shoulder_abs * signum(shoulder_to_go);
}
}
void move(int servo, int pos){
loop_until_bit_is_set(UCSRA, UDRE);
UDR = '#';
loop_until_bit_is_set(UCSRA, UDRE);
UDR = 's';
loop_until_bit_is_set(UCSRA, UDRE);
UDR = servo;
loop_until_bit_is_set(UCSRA, UDRE);
UDR = pos;
}
int signum(int val) {
if(val != 0) {
return val/abs(val);
} else {
return 0;
}
}
SIGNAL (SIG_OVERFLOW0){
servo_flag = 1;
}
Der Code bewegt zur Vereinfachung erstmal nur die Schulter und den Ellbogen. Die Bewegung klappt schon ganz gut. Allerdings passt das Neusetzen des Ziels für Ellbogens nicht ("elbow_dest = 1;" in keep_moving() )
Wer Interesse und Zeit hat kann sich das ja mal anschauen warum das nicht klappt. Ich weiß es ist viel Code zum "kurz" anschauen aber ich habs ein wenig auskommentiert, jetzt isses recht gut zu verstehen.
mfg
jagdfalke