Also irgendwie kommt immer nur noch Müll dabei raus. Das was jetzt dabei raus kommt macht irgendwie überhaupt keinen Sinn und ein Muster ist auch nicht zu erkennen:
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 '&'
volatile char inStr[500];
volatile 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);
void input(void);
volatile uint8_t char_flag = 0;
// -- 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) {
calc_steps();
keep_moving();
}
if(char_flag == 1) {
cli();
input();
sei();
}
}
return 0;
}
void parseInput(void) {
// ANZAHL DER ZEICHEN: inCnt
// INDEX DES LETZTEN ZEICHENS inCnt-1
cli();
printf("\n\rPARSING %s ...\n\r", inStr);
int cnt = 0;
if(inStr[0] == 's') {
printf("SERVO COMMAND FOUND\n\r");
int hundreds = (int)inStr[2]-48;
int tens = (int)inStr[3]-48;
int ones = (int)inStr[4]-48;
int number = hundreds*100 + tens*10 + ones;
printf("NUMBER: %i\n\r", number);
int servo = (int)inStr[1] - 48;
switch(servo) {
case(1):
shoulder_dest = number;
printf("\n\rSHOULDER_DEST CHANGED\n\r");
break;
case(2):
elbow_dest = number;
printf("\n\rELBOW_DEST CHANGED\n\r");
break;
case(3):
wrist_dest = number;
printf("\n\rWRIST_DEST CHANGED\n\r");
break;
case(4):
gripper_dest = number;
printf("\n\rGRIPPTER_DEST CHANGED\n\r");
}
}
sei();
}
void input(void) {
inStr[inCnt] = uart_readChar();
switch (inStr[inCnt])
{
case (stopsign):
parseInput();
inCnt=0;
break;
case (startsign):
inCnt=0;
break;
default:
inCnt++;
break;
}
char_flag = 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);
printf("%c%c", 15, 17);
printf("POSITION:\tSHOULDER:%i \tELBOW:%i \tWRIST:%i \t GRIPPER:%i\n\r", (int)shoulder_pos,(int)elbow_pos,(int)wrist_pos,(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;
}
//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) {
char_flag = 1;
}
// -- 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) {
return UDR;
}
Zitieren
Berechtigungen
Lesezeichen