548 lines
11 KiB
C
548 lines
11 KiB
C
/**
|
||
* Main loop
|
||
*
|
||
* @copyright 2012 Institute Automation of Complex Power Systems (ACS), RWTH Aachen University
|
||
* @license http://www.gnu.org/licenses/gpl.txt GNU Public License
|
||
* @author Steffen Vogel <info@steffenvogel.de>
|
||
*/
|
||
|
||
#include <stdlib.h>
|
||
#include <string.h>
|
||
#include <math.h>
|
||
|
||
#include <avr/io.h>
|
||
#include <avr/interrupt.h>
|
||
#include <avr/eeprom.h>
|
||
#include <avr/pgmspace.h>
|
||
#include <avr/wdt.h>
|
||
|
||
#include "lcd.h"
|
||
#include "rotary.h"
|
||
#include "adc.h"
|
||
#include "pid.h"
|
||
#include "uart.h"
|
||
|
||
#define DISPLAY_MODI 11
|
||
#define BAUDRATE 57600
|
||
|
||
#define MAGIC_BYTE 0xca
|
||
#define MAGIC_LEN 16
|
||
|
||
const char *mode_str[] = {
|
||
"Stop ",
|
||
"Auto ",
|
||
"Manual",
|
||
"UART "
|
||
};
|
||
|
||
const char *display_str[] = {
|
||
NULL,
|
||
"PWM Motor",
|
||
"PWM Servo",
|
||
"PID Drive: P",
|
||
"PID Drive: I",
|
||
"PID Stering: P",
|
||
"PID Stering: I",
|
||
"ADC Inductor: L",
|
||
"ADC Inductor: R",
|
||
"ADC Batt: Logic",
|
||
"ADC Batt: Drive"
|
||
};
|
||
|
||
enum channel {
|
||
STERING_LEFT,
|
||
STERING_RIGHT,
|
||
BATT_LOGIC,
|
||
BATT_DRIVE
|
||
};
|
||
|
||
enum state {
|
||
HALT,
|
||
AUTO,
|
||
MANUAL
|
||
};
|
||
|
||
enum menu {
|
||
OVERVIEW,
|
||
PWM_DRIVE,
|
||
PWM_STERING,
|
||
PID_DRIVE_P,
|
||
PID_DRIVE_I,
|
||
PID_STERING_P,
|
||
PID_STERING_I,
|
||
ADC_STERING_LEFT,
|
||
ADC_STERING_RIGHT,
|
||
ADC_BATT_LOGIC,
|
||
ADC_BATT_DRIVE,
|
||
};
|
||
|
||
enum cmd {
|
||
SET_STATUS, /* Mode, Display */
|
||
SET_PWM, /* Drive & Stering */
|
||
SET_PID, /* Controller state */
|
||
GET_PID /* Controller state */
|
||
};
|
||
|
||
// EEPROM
|
||
int16_t pid_drive_p_ee EEMEM = 0;
|
||
int16_t pid_drive_i_ee EEMEM = 0;
|
||
int16_t pid_stering_p_ee EEMEM = 39;
|
||
int16_t pid_stering_i_ee EEMEM = 0;
|
||
|
||
int16_t pwm_drive_ee EEMEM = 0;
|
||
int16_t pwm_stering_ee EEMEM = 0;
|
||
|
||
// Globale Daten
|
||
int16_t pwm_stering;
|
||
int16_t pwm_drive;
|
||
|
||
int8_t out_stering;
|
||
uint8_t out_drive;
|
||
|
||
int16_t adc_stering_left;
|
||
int16_t adc_stering_right;
|
||
int16_t adc_batt_logic;
|
||
int16_t adc_batt_drive;
|
||
|
||
uint8_t speed;
|
||
uint16_t speed_cnt;
|
||
bool speed_ovf = true;
|
||
|
||
struct pid pid_drive;
|
||
struct pid pid_stering;
|
||
|
||
enum state mode;
|
||
enum menu display;
|
||
|
||
bool edit = false;
|
||
bool redraw = true;
|
||
|
||
/**
|
||
* Parameter editieren
|
||
*/
|
||
void parameter_edit(int16_t *parameter, int16_t *parameter_ee, enum taster input) {
|
||
switch (input) {
|
||
case DGB_SW: // Editier-Modus togglen
|
||
edit = !edit;
|
||
break;
|
||
|
||
case DGB_CW: // Wert <20>ndern
|
||
if (edit) *parameter = *parameter + 1;
|
||
break;
|
||
|
||
case DGB_CCW: // Wert <20>ndern
|
||
if (edit) *parameter = *parameter - 1;
|
||
break;
|
||
|
||
case SW_GRUEN: // Laden aus EEPROM
|
||
*parameter = eeprom_read_word((uint16_t *) parameter_ee);
|
||
break;
|
||
|
||
case SW_BLAU: // Speichern im EEPROM
|
||
eeprom_write_word((uint16_t *) parameter_ee, *parameter);
|
||
break;
|
||
}
|
||
}
|
||
|
||
/**
|
||
* Startsequenz
|
||
*/
|
||
void greeter() {
|
||
// UART
|
||
uart_puts("Donaudampfschiff\r\n");
|
||
|
||
// LCD
|
||
lcd_clear();
|
||
lcd_string("Donaudampfschiff");
|
||
lcd_setcursor(0, 1);
|
||
lcd_string(" 4.Sem.Proj. ");
|
||
_delay_ms(1500);
|
||
lcd_clear();
|
||
}
|
||
|
||
/**
|
||
* Initialize Timers and IO Ports
|
||
*/
|
||
void init() {
|
||
DDRB = 0xff;
|
||
DDRC = 0xff;
|
||
DDRD = 0b11111011;
|
||
|
||
// Geschwindigkeitssensor
|
||
// Falling Edge on INT0 triggers Interrupt
|
||
MCUCR = (1<<ISC01) | (0<<ISC00);
|
||
GICR = (1<<INT0);
|
||
|
||
// Timer Counter 0 init - 8 bit
|
||
// f = 7.812 kHz
|
||
// Verwendung fuer Auswertung der Taster und analog Signal Erzeugung
|
||
TCCR0 = (0<<FOC0) | (1<<WGM00) | (0<<COM01) | (0<<COM00) | (1<<WGM01) | (0<<CS02) | (1<<CS01) | (0<<CS00);
|
||
TIMSK |= (1<<TOIE0); // Timer 0 OVF Interrupt einschalten
|
||
|
||
// Timer Counter 1 - Servo - 16 bit Counter - UINT16_MAX = 65535
|
||
// Frequenz 50 Hz
|
||
// Timer 1: f_PWM=f_Clk/(Prescaler1*(1+ICR1))
|
||
TCCR1A = (1<<COM1A1) | (0<<COM1A0) | (1<<COM1B1) | (0<<COM1B0) | (1<<WGM11) | (0<<WGM10);
|
||
TCCR1B = (1<<WGM13) | (1<<WGM12) | (0<<CS12) | (1<<CS11) | (0<<CS10); // Prescaler1 = 8
|
||
TIMSK |= (1<<TOIE1);
|
||
|
||
ICR1 = 40000; // F<>r welchen Wert im Register ICR1 ergibt sich eine Grundfrequenz von 50 Hz
|
||
OCR1A = 0; // Register zum Einstellen des Servo Tastgrades
|
||
|
||
// Timer Counter 2 init - 8 bit
|
||
// Verwendung fuer Motor-PWM
|
||
// Timer 2: f = f_Clk / (256 * Prescaler2)
|
||
// f2 = 976 Hz
|
||
TCCR2 = (0<<FOC2) | (1<<WGM20) | (1<<COM21) | (0<<COM20) | (1<<WGM21) | (1<<CS22) | (0<<CS21) | (0<<CS20); // Prescaler2 = 64
|
||
OCR2 = 0; // Register zum Einstellen des Motor Tastgrades
|
||
TIMSK |= (1<<TOIE2); // Timer 2 OVF Interrupt f<>r Eingabe Polling einschalten
|
||
}
|
||
|
||
|
||
/**
|
||
* Interupt Subroutine f<>r Eingabe Polling
|
||
*/
|
||
ISR(TIMER0_OVF_vect) {
|
||
enum taster input = dgb_read(); /* Drehgeber wird periodisch ausgelesen */
|
||
|
||
if (edit == false && input) { /* Menu wechseln */
|
||
if (input == DGB_CW && display < DISPLAY_MODI-1) {
|
||
display++;
|
||
}
|
||
else if (input == DGB_CCW && display != OVERVIEW) {
|
||
display--;
|
||
}
|
||
|
||
redraw = true;
|
||
}
|
||
|
||
switch (display) {
|
||
case OVERVIEW:
|
||
switch (input) {
|
||
case SW_BLAU:
|
||
mode = HALT;
|
||
break;
|
||
|
||
case SW_GRUEN:
|
||
switch (mode) {
|
||
case HALT:
|
||
mode = MANUAL;
|
||
break;
|
||
|
||
case MANUAL:
|
||
mode = AUTO;
|
||
break;
|
||
|
||
case AUTO:
|
||
mode = MANUAL;
|
||
break;
|
||
|
||
default:
|
||
mode = HALT;
|
||
}
|
||
break;
|
||
|
||
case DGB_CW:
|
||
case DGB_CCW:
|
||
case DGB_SW:
|
||
;
|
||
}
|
||
break;
|
||
|
||
case PWM_DRIVE:
|
||
parameter_edit(&pwm_drive, &pwm_drive_ee, input);
|
||
break;
|
||
|
||
case PWM_STERING:
|
||
parameter_edit(&pwm_stering, &pwm_stering_ee, input);
|
||
break;
|
||
|
||
case PID_STERING_P:
|
||
parameter_edit(&pid_stering.pFactor, &pid_stering_p_ee, input);
|
||
break;
|
||
|
||
case PID_STERING_I:
|
||
parameter_edit(&pid_stering.iFactor, &pid_stering_i_ee, input);
|
||
break;
|
||
|
||
case PID_DRIVE_P:
|
||
parameter_edit(&pid_drive.pFactor, &pid_drive_p_ee, input);
|
||
break;
|
||
|
||
case PID_DRIVE_I:
|
||
parameter_edit(&pid_drive.iFactor, &pid_drive_i_ee, input);
|
||
break;
|
||
|
||
case ADC_STERING_LEFT:
|
||
case ADC_STERING_RIGHT:
|
||
case ADC_BATT_LOGIC:
|
||
case ADC_BATT_DRIVE:
|
||
/* ADC Werte k<>nnen nicht ge<67>ndert werden */;
|
||
}
|
||
}
|
||
|
||
/**
|
||
* Interupt Subroutine f<>r UART Kommunikation
|
||
*/
|
||
ISR(TIMER1_OVF_vect) {
|
||
static uint8_t cnt, cnt2, magic, dat = 1;
|
||
char buffer[32];
|
||
|
||
/* Send Data */
|
||
/*if (cnt++ == 25) {
|
||
|
||
switch (cnt2++ & 0x01) {
|
||
case 0:
|
||
sprintf(buffer,
|
||
"spd=%i, adc_r=%i, adc_l=%i, ",
|
||
speed,
|
||
adc_stering_right,
|
||
adc_stering_left
|
||
);
|
||
break;
|
||
|
||
case 1:
|
||
sprintf(buffer,
|
||
"stering=%i, drive=%i\r\n",
|
||
out_stering,
|
||
out_drive
|
||
);
|
||
break;
|
||
}
|
||
|
||
uart_puts(buffer);
|
||
|
||
cnt = 0;
|
||
}*/
|
||
|
||
/*uint16_t byte = uart_getc();
|
||
if (byte != UART_NO_DATA) {
|
||
if (byte & 0x01) {
|
||
pwm_stering = *((int8_t *) &byte);
|
||
}
|
||
else {
|
||
pwm_drive = (uint8_t) byte;
|
||
}
|
||
}*/
|
||
}
|
||
|
||
/**
|
||
* Interupt Subroutine f<>r Regelung
|
||
*/
|
||
ISR(TIMER2_OVF_vect) {
|
||
speed_cnt++;
|
||
speed_ovf |= (speed_cnt == INT16_MAX);
|
||
|
||
/**
|
||
* Berechnung der neuen Sollwerte
|
||
*/
|
||
int16_t diff = adc_stering_right - adc_stering_left;
|
||
uint16_t sum = adc_stering_right + adc_stering_left;
|
||
|
||
/**
|
||
|
||
* Schreiben der Compare-Register
|
||
* Motor Tastgrad
|
||
*/
|
||
switch (mode) {
|
||
case MANUAL:
|
||
out_drive = pwm_drive;
|
||
out_stering = pwm_stering;
|
||
break;
|
||
|
||
case AUTO:
|
||
if (sum < 30) { /* Von Strecke abgekommen */
|
||
mode = HALT;
|
||
display = OVERVIEW;
|
||
edit = false;
|
||
}
|
||
else {
|
||
out_stering = pid_controller(0 , diff , &pid_stering);
|
||
|
||
// output_drive = pid_controller(pwm_drive, set_drive, &pid_drive); // funktioniert noch nicht
|
||
// output_drive = pid_controller(pwm_drive, speed, &pid_drive); // funktioniert noch nicht
|
||
out_drive = (pwm_drive>>1) + (( pwm_drive * (128 - abs(out_stering))) >> 8);
|
||
}
|
||
break;
|
||
|
||
case HALT:
|
||
default:
|
||
out_drive = 0;
|
||
out_stering = 0;
|
||
}
|
||
|
||
/**
|
||
* Servo Tastgrad
|
||
* Links 2.1ms => 4200
|
||
* Mitte 1.5ms => 3000
|
||
* Rechts 0.9ms => 1800
|
||
*/
|
||
OCR1A = 3000 + (out_stering * 9);
|
||
OCR2 = out_drive;
|
||
}
|
||
|
||
|
||
/**
|
||
* Interupt Subroutine f<>r ADC
|
||
*/
|
||
ISR(ADC_vect) {
|
||
enum channel ch = ADMUX & ADMUX_MASK;
|
||
|
||
switch (ch) {
|
||
case STERING_LEFT:
|
||
adc_stering_left = ADC;
|
||
break;
|
||
|
||
case STERING_RIGHT:
|
||
adc_stering_right = ADC;
|
||
break;
|
||
|
||
case BATT_LOGIC:
|
||
|
||
adc_batt_logic = ADC;
|
||
break;
|
||
|
||
case BATT_DRIVE:
|
||
adc_batt_drive = ADC;
|
||
break;
|
||
}
|
||
|
||
ADMUX = (ADMUX & ~ADMUX_MASK);
|
||
ADMUX |= (ch >= 1) ? 0 : ++ch;
|
||
|
||
ADCSRA |= (1<<ADSC); // starte n<>chste Messung
|
||
}
|
||
|
||
/**
|
||
* Interrupt Subroutine f<>r Geschwindigkeitssensor
|
||
*/
|
||
ISR(INT0_vect) {
|
||
if (speed_ovf) {
|
||
speed = 0;
|
||
speed_ovf = false;
|
||
}
|
||
else {
|
||
speed = 2e4 / speed_cnt;
|
||
}
|
||
speed_cnt = 0;
|
||
}
|
||
|
||
int main() {
|
||
|
||
// Gespeicherte Werte einlesen
|
||
pwm_drive = eeprom_read_word((uint16_t *) &pwm_drive_ee);
|
||
pwm_stering = eeprom_read_word((uint16_t *) &pwm_stering_ee);
|
||
|
||
// Initialisierung
|
||
init();
|
||
adc_init();
|
||
dgb_init();
|
||
lcd_init();
|
||
uart_init(UART_BAUD_SELECT(BAUDRATE, F_CPU));
|
||
|
||
// PID mit EEPROM-Werten initialisieren
|
||
pid_init(
|
||
eeprom_read_word((uint16_t *) &pid_drive_p_ee),
|
||
eeprom_read_word((uint16_t *) &pid_drive_i_ee),
|
||
0, &pid_drive);
|
||
pid_init(
|
||
eeprom_read_word((uint16_t *) &pid_stering_p_ee),
|
||
eeprom_read_word((uint16_t *) &pid_stering_i_ee),
|
||
0, &pid_stering);
|
||
|
||
// Interrupts aktivieren
|
||
sei();
|
||
|
||
// Startsqeuenz
|
||
greeter();
|
||
|
||
// Watchdog Timer aktivieren
|
||
wdt_enable(WDTO_1S);
|
||
|
||
// Men<65>
|
||
while (1) {
|
||
wdt_reset();
|
||
|
||
lcd_home(); /* set cursor to top-leftmost postion */
|
||
|
||
if (redraw) {
|
||
lcd_clear();
|
||
|
||
if (display_str[display]) {
|
||
lcd_string(display_str[display]);
|
||
}
|
||
|
||
redraw = false;
|
||
}
|
||
|
||
switch (display) {
|
||
case OVERVIEW:
|
||
lcd_string(mode_str[mode]);
|
||
lcd_string(" S:");
|
||
lcd_int(out_stering, 6);
|
||
|
||
lcd_setcursor(0, 1);
|
||
lcd_string("V:");
|
||
lcd_int(speed, 5);
|
||
|
||
lcd_string(" D:");
|
||
lcd_int(out_drive, 6);
|
||
break;
|
||
|
||
case PWM_DRIVE:
|
||
lcd_setcursor(0, 1);
|
||
lcd_int(pwm_drive, 16);
|
||
break;
|
||
|
||
case PWM_STERING:
|
||
lcd_setcursor(0, 1);
|
||
lcd_int(pwm_stering, 16);
|
||
break;
|
||
|
||
case PID_STERING_P:
|
||
lcd_setcursor(0, 1);
|
||
lcd_int(pid_stering.pFactor, 16);
|
||
break;
|
||
|
||
case PID_STERING_I:
|
||
lcd_setcursor(0, 1);
|
||
lcd_int(pid_stering.iFactor, 16);
|
||
break;
|
||
|
||
case PID_DRIVE_P:
|
||
lcd_setcursor(0, 1);
|
||
lcd_int(pid_drive.pFactor, 16);
|
||
break;
|
||
|
||
case PID_DRIVE_I:
|
||
lcd_setcursor(0, 1);
|
||
lcd_int(pid_drive.iFactor, 16);
|
||
break;
|
||
|
||
case ADC_STERING_LEFT:
|
||
lcd_setcursor(0, 1);
|
||
lcd_int(adc_stering_left, 5);
|
||
lcd_bar(6, 1, 10, adc_stering_left / 10);
|
||
break;
|
||
|
||
case ADC_STERING_RIGHT:
|
||
lcd_setcursor(0, 1);
|
||
lcd_int(adc_stering_right, 5);
|
||
lcd_bar(6, 1, 10, adc_stering_right / 10);
|
||
break;
|
||
|
||
case ADC_BATT_LOGIC:
|
||
lcd_setcursor(10, 1);
|
||
lcd_int(adc_batt_logic, 6);
|
||
break;
|
||
|
||
case ADC_BATT_DRIVE:
|
||
lcd_setcursor(10, 1);
|
||
lcd_int(adc_batt_drive, 6);
|
||
break;
|
||
}
|
||
}
|
||
|
||
return 0;
|
||
}
|
||
|