main.c

gehe zur Dokumentation dieser Datei
00001 /*************************************************************************/
00013 #include <avr/io.h>   //I/O-Register 
00014 #include <stdint.h>   //standart Integertypen
00015 #include <stdlib.h>   // itoa
00016 #include <avr/interrupt.h>
00017 
00018 
00019 #include "../nbot_lib/uart.h"
00020 #include "../nbot_lib/timer.h"
00021 #include "../nbot_lib/encoder.h"
00022 #include "../nbot_lib/motor.h"
00023 #include "pcc.h"
00024 
00025 //**** defines ****
00026 #define ki 2  
00027 #define kd 0  
00028 #define kp 2  
00030 //**** variablen ****
00031 uint32_t zeit = 0;  
00032 int16_t sollwert[] = {0,0}; 
00033 int16_t istwert[] = {0,0};
00034 int16_t fehler[] = {0,0};
00035 int16_t fehler_alt[] = {0,0};
00036 int16_t integrator[] = {0,0};
00037 int16_t differenzierer[] = {0,0};
00038 int16_t stellwert[] = {0,0};
00039 pcc_t pcc_daten;
00040 
00041 
00042 
00043 //**** funktionen ****
00044 void regelung(uint8_t motor);
00045 
00046 //*** main ***
00047 int main(void)
00048 {
00049   uart_init();
00050   timer2_init();
00051   encoder_init();
00052   motor_init();
00053   pcc_init(&pcc_daten);
00054   
00055   uint8_t buf;
00056   
00057     
00058   pcc_daten.time = &zeit;
00059   pcc_daten.encoder[LEFT] = &istwert[LEFT];
00060   pcc_daten.istwert[LEFT] = &istwert[LEFT];
00061   pcc_daten.sollwert[LEFT] = &sollwert[LEFT];
00062   pcc_daten.encoder[RIGHT] = &istwert[RIGHT];
00063   pcc_daten.istwert[RIGHT] = &istwert[RIGHT];
00064   pcc_daten.sollwert[RIGHT] = &sollwert[RIGHT];
00065   
00066   DDRB |= (1<<PB0) | (1<<PB1);
00067   PORTB |= (1<<PB0) | (1<<PB1);
00068   sei();
00069   for(;;)
00070   {
00071     if (Gettime() >= (zeit + 100))    // alle 100ms
00072     { 
00073       zeit = Gettime();
00074       
00075       // *************  Motorregelung ***************
00076       regelung(LEFT);     // linken Motor regeln
00077       regelung(RIGHT);    // rechten Motor regeln   
00078 
00079       if(uart_getc(&buf) == 1)
00080         pcc_rx(buf);    // PC Kommunikation
00081       pcc();
00082     
00083     }
00084   }
00085   return 0;
00086 }
00087 
00088 /**************************************************************************/
00100 void regelung(uint8_t motor)
00101 {
00102       // alle werte zurücksetzten wenn Motor still stehen soll
00103       if(sollwert[motor] == 0){
00104         fehler[motor]=0;
00105         fehler_alt[motor]=0;
00106         integrator[motor]=0;
00107         differenzierer[motor]=0;
00108         stellwert[motor]=0;
00109         motor_speed(0, 0);      
00110         return;
00111       }
00112       
00113       istwert[motor] = encoderGet(motor);
00114       
00115       
00116       //****** linke Regelung ************************************************ 
00117       
00118       fehler[motor] = sollwert[motor] - istwert[motor];           // Sollwertabweichung
00119       
00120       integrator[motor] += fehler[motor];                       // Integrator
00121       // Integrator begrenzen
00122       if(integrator[motor] > 1023)
00123         integrator[motor] = 1023;
00124       if(integrator[motor] < (-1023))
00125         integrator[motor] = (-1023);      
00126       
00127       differenzierer[motor] = fehler[motor] - fehler_alt[motor];  // Differenzierer
00128       
00129       stellwert[motor]  = integrator[motor]/ki;                 // Stellgröße
00130       stellwert[motor] += kd * differenzierer[motor];
00131       stellwert[motor] += kp * fehler[motor];
00132       
00133       // Stellwertbegrenzung
00134       if(stellwert[motor]>255)
00135         stellwert[motor] = 255;
00136       if(stellwert[motor]< (-255))
00137         stellwert[motor] = (-255);
00138         
00139       fehler_alt[motor] = fehler[motor];                          // Sollwertabweichung merken
00140     
00141       switch (motor)
00142       {
00143         case LEFT:
00144           // Motordrehrichtung
00145           if(stellwert[motor]>0)
00146             motor_dir(FWD,FREE);
00147           if(stellwert[motor]<0){
00148             stellwert[motor] = -stellwert[motor];
00149             motor_dir(RWD,FREE);
00150           }
00151           break;
00152         case RIGHT:
00153           // Motordrehrichtung
00154           if(stellwert[motor]>0)
00155             motor_dir(FREE,FWD);
00156           if(stellwert[motor]<0){
00157             stellwert[motor] = -stellwert[motor];
00158             motor_dir(FREE,RWD);
00159           }
00160           break;
00161       }
00162       
00163       // Motorspeed setzen 
00164       motor_speed(stellwert[LEFT], stellwert[RIGHT]);
00165 }

Erzeugt am Sun Apr 22 20:00:39 2007 für nBot von  doxygen 1.5.1-p1