Hi,
ich will nun beide meine Testprogramme vereinen. Das eine Bewegt 4 Servos gleichzeitig. Das ganze ist Timer-Interrupt-gesteuert. Der andere Teil in ein RX-Complete Interrupt, der Eingaben speichert bis das Stoppzeichen kommt und dann den String an eine Methode weitergibt, die ihn verarbeitet.. Es sollen z.B. Strings wie "s1100" empfangen werden, was bedeutet "Bewege Servo 1 (s1) zur Position 100".
Das ganze klappt aber nicht so. Wenn ich die Befehle zum Bewegen der Servos auskommentiere funktioniert der Empfang, ansonstem eben nicht.
Das Prinzip soll folgendes sein: Der Controller vergleicht ständig die aktuelle Position und die Soll-Position. Wenn sie nicht übereinstimmen bewegt er sie. Durch den Empfang des Befehls sollen neue Positionen vergeben werden können. Der Controller bewegt dann automatisch die Servos in die gewünschte Position.
Hier ist der Code für das ganze:
Code:
#define F_CPU 8000000
#define BAUD_RATE 9600
#include <avr/delay.h>
#include <avr/interrupt.h>
#include <avr/signal.h>
#include <math.h>
#include <stdlib.h>
#include <string.h>
#include <stdio.h>
#include <avr/io.h>
#include <avr/sfr_defs.h>
#define stopsign '!'
#define startsign '&'
char inStr[500];
int inCnt = 0;
// -- METHODEN DEKLARATIONEN
void keep_moving(void);
void move(int servo, int pos);
void calc_steps(void);
int putChar(char c);
int signum(int val);
void uart_init(int tx, int rx);
uint8_t uart_readChar(void);
void parseInput(void);
// -- SERVO VARIABLES
volatile uint8_t servo_flag = 0;
volatile float shoulder_pos = 127.0;
volatile float elbow_pos = 127.0;
volatile float wrist_pos = 127.0;
volatile float gripper_pos = 127.0;
volatile uint8_t shoulder_dest = 255;
volatile uint8_t elbow_dest = 1;
volatile uint8_t wrist_dest = 127;
volatile uint8_t gripper_dest = 250;
volatile float shoulder_step = 0;
volatile float elbow_step = 0;
volatile float wrist_step = 0;
volatile float gripper_step = 0;
int main (void) {
//TIMER-IINTERRUPT AN
TCCR0 |= (1<<CS01) | (1<<CS00);
TIMSK |= (1<<TOIE0);
//UART-INTERRUPT AN
UCSRB |= (1<<RXCIE);
//UART INITIALISIEREN (RX UND TX ANSCHALTEN)
uart_init(1, 1);
//GLOBAL INTERRUPT AN
sei();
//PRINTF FÜR AUSGABE KONFIGURIEREN
fdevopen(putChar,NULL,0);
printf("\n\n\rSTART !!!\n\r");
//HAUPTSCHLEIFE
while(1)
{
//NUR AUFRUFEN, WENN EIN TIMER INTERUPT DA WAR
if(servo_flag == 1) {
cli();
calc_steps();
keep_moving();
sei();
}
}
return 0;
}
// -- METHODEN ZUM BEWEGEN DER Servos
void keep_moving() {
//CALCULATE NEW POSITIONS
shoulder_pos = shoulder_pos + shoulder_step;
elbow_pos = elbow_pos + elbow_step;
wrist_pos = wrist_pos + wrist_step;
gripper_pos = gripper_pos + gripper_step;
//SET STEP=0 IF DEST. POSITION WAS REACHED
if( (int)(shoulder_pos) == shoulder_dest){ shoulder_step = 0; }
if( (int)(elbow_pos) == elbow_dest) { elbow_step = 0; }
if( (int)(wrist_pos) == wrist_dest) { wrist_step = 0; }
if( (int)(gripper_pos) == gripper_dest) { gripper_step = 0; }
//POSITIONEN SCHICKEN
move(1, (int)shoulder_pos);
move(2, (int)shoulder_pos);
move(3, (int)elbow_pos);
move(4, (int)wrist_pos);
move(5, (int)gripper_pos);
servo_flag = 0;
}
void calc_steps(void) {
float shoulder_to_go = shoulder_dest - shoulder_pos;
float elbow_to_go = elbow_dest - elbow_pos;
float wrist_to_go = wrist_dest - wrist_pos;
float gripper_to_go = gripper_dest - gripper_pos;
float shoulder_absolute = abs(shoulder_to_go);
float elbow_absolute = abs(elbow_to_go);
float wrist_absolute = abs(wrist_to_go);
float gripper_absolute = abs(gripper_to_go);
shoulder_step = 1;
elbow_step = 1;
wrist_step = 1;
gripper_step = 1;
//GET GREATEST VALUE
uint8_t largest_way_servo = shoulder_absolute;
if(elbow_absolute > largest_way_servo) { largest_way_servo = elbow_absolute; }
if(wrist_absolute > largest_way_servo) { largest_way_servo = wrist_absolute; }
if(gripper_absolute > largest_way_servo) { largest_way_servo = gripper_absolute; }
//SET STEP DEPENDING ON GREATEST VALUE (BIGGEST STEP=1)
shoulder_step = (shoulder_absolute/largest_way_servo) * signum(shoulder_to_go);
elbow_step = (elbow_absolute/largest_way_servo) * signum(elbow_to_go);
wrist_step = (wrist_absolute/largest_way_servo) * signum(wrist_to_go);
gripper_step = (gripper_absolute/largest_way_servo) * signum(gripper_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;
}
// INPUT PARSEN
void parseInput(void) {
if(inStr[0] == 's') {
if(inStr[1] == '1') {
int hundred = 100 * ((int)inStr[2] - 48);
int ten = 10 * ((int)inStr[3] - 48);
int one = (int)inStr[4] - 48;
int pos = hundred + ten + one;
elbow_dest = pos;
}
}
}
//SIGNUM WIRD NUR IN calc_steps BENÖTIGT
int signum(int val) {
if(val != 0) {
return val/abs(val);
} else {
return 0;
}
}
//INTERRUPTS
SIGNAL (SIG_OVERFLOW0){
servo_flag = 1;
}
SIGNAL(SIG_USART_RECV) {
cli();
inStr[inCnt] = uart_readChar();
switch (inStr[inCnt])
{
case (stopsign):
parseInput();
inCnt=0;
break;
case (startsign):
inCnt=0;
break;
default:
inCnt++;
break;
}
sei();
}
// -- UART METHODEN
void uart_init(int tx, int rx) {
UBRRL = (F_CPU/(BAUD_RATE*16l)-1);
if(tx == 1) {
UCSRB |= (1<<TXEN);
}
if(rx == 1) {
UCSRB |= (1<<RXEN);
}
}
int putChar(char c){
while ( !( UCSRA & (1<<UDRE)) );
UDR=c;
return 0;
}
uint8_t uart_readChar(void) {
loop_until_bit_is_set(UCSRA, RXC);
return UDR;
}
Aber irgendwas stimmt nicht.. Er reagiert nicht auf Eingaben. Wenn ich die Zeilen mit move() auskommentiere klappt der Empfang.
Wäre super nett wenn sich das jemand anschauen würde. Es ist viel weniger als es aussieht. Ihr könnt ja die methoden keep_moving() und calc_steps komplett auslassen. Sie funktionieren ja einwandfrei.
mfg
jagdfalke
Zitieren
) 
Lesezeichen