00001
00013 #include <avr/io.h>
00014 #include <stdint.h>
00015 #include <stdlib.h>
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
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
00044 void regelung(uint8_t motor);
00045
00046
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))
00072 {
00073 zeit = Gettime();
00074
00075
00076 regelung(LEFT);
00077 regelung(RIGHT);
00078
00079 if(uart_getc(&buf) == 1)
00080 pcc_rx(buf);
00081 pcc();
00082
00083 }
00084 }
00085 return 0;
00086 }
00087
00088
00100 void regelung(uint8_t motor)
00101 {
00102
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
00117
00118 fehler[motor] = sollwert[motor] - istwert[motor];
00119
00120 integrator[motor] += fehler[motor];
00121
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];
00128
00129 stellwert[motor] = integrator[motor]/ki;
00130 stellwert[motor] += kd * differenzierer[motor];
00131 stellwert[motor] += kp * fehler[motor];
00132
00133
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];
00140
00141 switch (motor)
00142 {
00143 case LEFT:
00144
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
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
00164 motor_speed(stellwert[LEFT], stellwert[RIGHT]);
00165 }