Öffentliche Projekte

#include <nibobee/robo.h>
#include <stdio.h>
#include <stdlib.h>
#include <string.h>

#include "uart.h"
#include "globals.h"
#include "werkzeuge.h"

uint8_t fahren=1;
// Variablen für Geschwindigkeit definieren und initialisieren
int16_t i_speed_r=300;
int16_t i_speed_l=300;
int16_t speed;
uint16_t dist;

// Variablen für PID
uint8_t u_ki=10;
uint8_t u_kp=5;
uint8_t u_kd=5;

int16_t i_prop=0;
int16_t i_diff=0;
int16_t i_int=0;
int16_t i_korr=0;

int16_t i_odo_altdiff=0;

int16_t odo_L=0;
int16_t odo_R=0;

void korrigiere_Rtg (int8_t rtg) { // rtg=1 vorwärts rtg=-1 rückwärts
  int16_t i_odo_R=odometry_getRight(1);	// Zählerstand wird ausgelesen und ZURÜCKGESETZT
  int16_t i_odo_L=odometry_getLeft(1);
  int16_t i_odo_diff=i_odo_R-i_odo_L;  
  odo_L+=i_odo_L;	// Für Distanz
  odo_R+=i_odo_R;
  // PID-Berechnung
  i_prop=i_odo_diff;
  i_prop=i_prop*u_kp;
  i_int+=i_int*u_ki;
  i_diff=(i_odo_diff-i_odo_altdiff)*u_kd;
  i_odo_altdiff=i_odo_diff;
  i_korr=i_prop+i_int+i_diff;
/*  uart_puts("Kp: ");
  itoa(i_prop,str,10);
  uart_puts(str);
  uart_puts(" Ki: ");
  itoa(i_int,str,10);
  uart_puts(" Kd: ");
  itoa(i_diff,str,10);
  uart_puts(str);uart_putc('\n'); */
  // Korrektur der Motorgeschwindigkeiten
  i_speed_r -= (i_korr*rtg);
  i_speed_l += (i_korr*rtg);
  motpwm_setLeft(i_speed_l);
  motpwm_setRight(i_speed_r);
}

void fahrmenue(){
  
  uart_puts("Fahrmenue:\n");
  uart_puts("Vorwärts: w\n");
  uart_puts("links: a     rechts: d\n");
//  uart_puts("Stop: s\n");
  uart_puts("Zurueck: x\n");
  uart_puts("Beenden: b\n");
//  uart_puts("Korrektur: k\n");
}

void do_w(){
  uart_puts("Speed: ");
  while (eingabe_fertig==0) {
    delay(50);
  }
  eingabe_fertig=0;
  speed = atoi(eingabe);
  if (speed > 0 && speed < 1000){
    uart_puts("\nDistanz: ");
    while (eingabe_fertig==0) {
      delay(50);
    }
    eingabe_fertig=0;
    dist = atoi(eingabe);
    if ( dist > 0 && dist < 690) {
      i_prop=0;
      i_diff=0;
      i_int=0;
      i_korr=0;
      odo_L=0;
      odo_R=0;
      i_odo_altdiff=0;
      i_speed_r=speed;
      i_speed_l=speed;
      motpwm_setLeft(i_speed_l);
      motpwm_setRight(i_speed_r);
      while (odo_R<dist && odo_L<dist){
/*        itoa(odo_R,str,10);
        uart_puts("odo_R: "); uart_puts(str);
        itoa(odo_L,str,10);
        uart_puts(" odo_L: "); uart_puts(str); uart_putc('\n'); */
        korrigiere_Rtg (1);
        delay(100);
      }
      motpwm_stop();
    } else 
    { 
      uart_puts("ERROR\n");
    }
  } else 
  {
    uart_puts("ERROR\n");
  }
}

void do_x(){
  uart_puts("Speed: ");
  while (eingabe_fertig==0) {
    delay(50);
  }
  eingabe_fertig=0;
  speed = atoi(eingabe);
  if (speed > 0 && speed < 1000){
    uart_puts("\nDistanz: ");
    while (eingabe_fertig==0) {
      delay(50);
    }
    eingabe_fertig=0;
    dist = atoi(eingabe);
    if ( dist > 0 && dist < 690) {
      i_prop=0;
      i_diff=0;
      i_int=0;
      i_korr=0;
      odo_L=0;
      odo_R=0;
      i_odo_altdiff=0;
      i_speed_l= 0-(speed);
      i_speed_r= 0-(speed);
      motpwm_setLeft(i_speed_l);
      motpwm_setRight(i_speed_r);
      while (odo_R>dist && odo_L>dist){
/*        itoa(odo_R,str,10);
        uart_puts("odo_R: "); uart_puts(str);
        itoa(odo_L,str,10);
        uart_puts(" odo_L: "); uart_puts(str); uart_putc('\n'); */
        korrigiere_Rtg (-1);
        delay(100);
      }
      motpwm_stop();
    }
    else 
    { 
      uart_puts("ERROR\n");
    }
  } else 
  {
    uart_puts("ERROR\n");
  }
}

void do_a(){
  uart_puts("Speed: ");
  while (eingabe_fertig==0) {
    delay(50);
  }
  eingabe_fertig=0;
  speed = atoi(eingabe);
  if (speed > 0 && speed < 1000){
    uart_puts("\nDistanz: ");
    while (eingabe_fertig==0) {
      delay(50);
    }
    eingabe_fertig=0;
    dist = atoi(eingabe);
    if ( dist > 0 && dist < 345) {
      i_prop=0;
      i_diff=0;
      i_int=0;
      i_korr=0;
      odo_L=0;
      odo_R=0;
      i_speed_r= (speed);
      i_speed_l= (speed)/2;
      motpwm_setLeft(i_speed_l);
      motpwm_setRight(i_speed_r);
      while (odo_R<dist && odo_L<dist){
        odo_R+=odometry_getRight(1);
        odo_L+=odometry_getLeft(1);
        //korrigiere_Rtg (-1);
        delay(100);
      }
      motpwm_stop();      
    }
    else { 
      uart_puts("ERROR\n");
    }
  } else {
    uart_puts("ERROR\n");
  }
}

void do_d(){
  uart_puts("Speed: ");
  while (eingabe_fertig==0) {
    delay(50);
  }
  eingabe_fertig=0;
  speed = atoi(eingabe);
  if (speed > 0 && speed < 1000){
    uart_puts("\nDistanz: ");
    while (eingabe_fertig==0) {
      delay(50);
    }
    eingabe_fertig=0;
    dist = atoi(eingabe);
    if ( dist > 0 && dist < 345) {
      odo_R=0;
      odo_L=0;
      i_speed_r= (speed)/2;
      i_speed_l= (speed);
      motpwm_setLeft(i_speed_l);
      motpwm_setRight(i_speed_r);
      while (odo_R<dist && odo_L<dist){
        //korrigiere_Rtg (-1);
        odo_R+=odometry_getRight(1);
        odo_L+=odometry_getLeft(1);
        delay(100);
      }
      motpwm_stop();
    }
    else { 
      uart_puts("ERROR\n");
    }
  } else {
    uart_puts("ERROR\n");
  }
}  
/*
void korrektur(){
  uart_puts("{Kp|Ki|Kd]} {plus10|plus1|minus1|minus10} ? ");
    while (eingabe_fertig==0) {
    delay(50);
  }
  eingabe_fertig=0;
  if (strcmp(eingabe,"Kp plus10")==0){
    plus10(&u_kp);
  } else 
  if (strcmp(eingabe,"Kp plus1")==0){
    plus1(&u_kp);
  } else     
  if (strcmp(eingabe,"Kp minus1")==0){
    minus1(&u_kp);
  } else   
  if (strcmp(eingabe,"Kp minus10")==0){
    minus10(&u_kp);
  } else 
  if (strcmp(eingabe,"Ki plus10")==0){
    plus10(&u_ki);
  } else 
  if (strcmp(eingabe,"Ki plus1")==0){
    plus1(&u_ki);
  } else     
  if (strcmp(eingabe,"Ki minus1")==0){
    minus1(&u_ki);
  } else   
  if (strcmp(eingabe,"Ki minus10")==0){
    minus10(&u_ki);
  } else    
  if (strcmp(eingabe,"Kd plus10")==0){
    plus10(&u_kd);
  } else 
  if (strcmp(eingabe,"Kd plus1")==0){
    plus1(&u_kd);
  } else     
  if (strcmp(eingabe,"Kd minus1")==0){
    minus1(&u_kd);
  } else   
  if (strcmp(eingabe,"Kd minus10")==0){
    minus10(&u_kd);
  } else    
  {
    uart_puts("ERROR\n");
  }
}
*/
void interpret_fahren(){
  if (strcmp(eingabe,"w")==0) {
    do_w();
  } else 
  if (strcmp(eingabe,"a")==0) {
    do_a();
  } else
  if (strcmp(eingabe,"d")==0) {
    do_d();
  } else /*
  if (strcmp(eingabe,"s")==0) {
    do_s();
  } else */
 if (strcmp(eingabe,"x")==0) {
    do_x();
  } else 
  if (strcmp(eingabe,"b")==0) {
    fahren=0;
    motpwm_stop();
  } else {
    fahrmenue();
  }
/*  if (strcmp(eingabe,"k")==0) {
    korrektur();
  } */
}

void warte_befehl(){
  while (fahren==1) {
    while (eingabe_fertig==0){
     delay(50);
    }
    eingabe_fertig=0;
    interpret_fahren();
  }
}

void start_fahren(){
  fahren=1;
  odometry_init();
  motpwm_init();
  fahrmenue();
  warte_befehl();
}

Öffentliche Projekte

Welcome

Projektverwaltung

Nachrichten

Neuigkeiten

Site-Statistic

zusammen_v4_1/fahren.c [read only]

Werbung

Online