Hallo Zusammen!
so, ich bin wieder ein stück weiter gekommen, konnte die Genauigkeit aber noch nicht genau Messen.
hier der Code:
#include <inttypes.h>
#include <avr/interrupt.h>
#include "twislave.h"
//#include <util/delay.h> /* definiert _delay_ms() ab avr-libc Version 1.2.0 */
#include "L6229.h"
#include "ADIO.h"
#define MAX_Power 150 //MAX Power, 120 Max, 250 min
#define I2CAdresse 12 //MAX Power, 120 Max, 250 min
#define MaxStallTime 200 //MAX Power, 120 Max, 250 min
#define MAX_I 2000
#define time_on 150// 3
#define time_off time_on-50//125 ca. 12ms Periodendauer
#define Ta 15 //=1/Periodendauer;
volatile uint8_t regler1,regler2;
volatile uint8_t speed1,speed2;
volatile uint8_t dir1,dir2;
volatile int sollWert1,sollWert2;
volatile int sollWert1_eff,sollWert2_eff;
volatile float q1,q2;
volatile unsigned int strom2,strom1;
volatile uint8_t verzoegerung;
volatile int16_t stall;
volatile float e1,e2,e1alt,e2alt;
volatile int16_t winkel1, winkel2;
volatile float kp1,kp2,ki1,ki2,kd1,kd2; //p,i,d-Anteile
volatile float isum1,isum2;
volatile uint8_t modus1,modus2; //Modus in dem sich der Regler befindet, Modus = 0 Strompeak suchen, Modus = 1 normalbetrieb
ISR(SIG_INTERRUPT0){
if (PINC & (1<<PINC2)){winkel1--; /*LED(2,1);LED(3,0);*/}
else{winkel1++;/* LED(2,0); LED(3,1); if ( !(PINC & (1<<PINC2)) )*/}
}
ISR(SIG_INTERRUPT1){
if (PINC & (1<<PINC3)){winkel2--; /*LED(2,1);LED(3,0);*/}
else{winkel2++;/* LED(2,0); LED(3,1); if ( !(PINC & (1<<PINC3)) )*/}
}
void stromUeberwachen(){
strom2 = ReadChannel(1);
strom1 = ReadChannel(0);
if(strom1>110 || strom2>105){ LED(1,1);stall++;}
else {LED(1,0); stall=0;}
if(stall>MaxStallTime){enable(0,0);enable(1,0); regler1=0;regler2=0; } //Strombegrenzer
}
void reglerMot1(){
e1=(sollWert1-winkel1)/2000.0f;
isum1+=e1/200.0f;
if(e1<0){dir1 = 1; e1*=-1;} else dir1 = 0;
fwd_rev(0,dir1);
q1 = kp1 * e1; //P-Anteil
q1 += isum1*ki1*Ta; //I-Antei
q1 += (e1 - e1alt)/Ta * kd1; //D Anteil
e1alt=e1;
if(q1<0.0f)q1*=-1;
if(q1>1.0f)q1=1.0f;//0 = 5%
q1=(1.0f-q1*1.0f)*255.0f;
if(q1>255) OCR1A = 255; //Anti Wind-Up
else if(q1<MAX_Power) OCR1A = MAX_Power; //Anti Wind-Up
else OCR1A =q2;
if(e1<=0.001 && e1 >=-0.001){ enable(0,0);isum1=0; } else enable(0,1);
}
void reglerMot2(){
e2=(sollWert2-winkel2)/2000.0f;
isum2+=e2/200.0f;
if(e2<0){dir2 = 1; e2*=-1;} else dir2 = 0;
fwd_rev(1,dir2);
q2 = kp2 * e2; //P-Anteil
q2 += isum2*ki2*Ta; //I-Antei
q2 += (e2 - e2alt)/Ta * kd2; //D Anteil
e2alt=e2;
// if(isum2>1 || isum2<-1) LED(2,1); else LED(2,0);
if(q2<0.0f)q2*=-1;
if(q2>1.0f)q2=1.0f;//0 = 5%
q2=(1.0f-q2*1.0f)*255.0f;
if(q2>255) OCR1B = 255; //Anti Wind-Up
else if(q2<MAX_Power) OCR1B = MAX_Power; //Anti Wind-Up
else OCR1B =q2;
if(e2<=0.001 && e2 >=-0.001){ enable(1,0);isum2=0; } else enable(1,1);
}
void mainSchlaufe(){
switch(rxbuffer[0]){
case 1:
regler1=0;
enable(0,0);
rxbuffer[0] = 0;
break;
case 4:
enable(0,0);
if(rxbuffer[1]==1)kp1=rxbuffer[2];
if(rxbuffer[1]==2)ki1=rxbuffer[2];
if(rxbuffer[1]==3)kd1=rxbuffer[2];
rxbuffer[0] = 0;
break;
case 7:
sollWert1=(((rxbuffer[2])<<7) & 0b11111110000000); sollWert1+=rxbuffer[1]&0b00000001111111;
regler1=1;
rxbuffer[0] = 0;
break;
case 9:
winkel1=(((rxbuffer[2])<<7) & 0b11111110000000); winkel1+=rxbuffer[1]&0b00000001111111;
sollWert1=winkel1;
rxbuffer[0] = 0;
break;
}
switch(rxbuffer[3]){
case 1:
regler2=0;
enable(1,0);
rxbuffer[3] = 0;
break;
case 4:
enable(1,0);
if(rxbuffer[4]==1)kp2=rxbuffer[5];
if(rxbuffer[4]==2)ki2=rxbuffer[5];
if(rxbuffer[4]==3)kd2=rxbuffer[5];
rxbuffer[3] = 0;
break;
case 7:
sollWert2=(((rxbuffer[5])<<7) & 0b11111110000000); sollWert2+=rxbuffer[4]&0b00000001111111;
regler2=1;
rxbuffer[3] = 0;
break;
case 9:
winkel2=(((rxbuffer[5])<<7) & 0b11111110000000); winkel2+=rxbuffer[4]&0b00000001111111;
sollWert2=winkel2;
rxbuffer[3] = 0;
break;
}
}
// ISR zum auffangen der Interrupts:
SIGNAL(TIMER2_COMPA_vect)
{
if (OCR2A == time_off){ // lange geschlafen, jetzt ausgang aktivieren
OCR2A = time_on;
// Einschaltdauer einstellen
}
else{
if(verzoegerung >= 2){ //6 = ca. 70ms Abtastzeit 2 ca. 20ms
if(regler1==1)reglerMot1(); //Linker Oberschenkel gliedL[2]
if(regler2==1)reglerMot2(); //Linker Unterschenkel gliedL[1]
stromUeberwachen();
verzoegerung=0;
}
mainSchlaufe();
verzoegerung++;
PORTD &= ~(1<<PD4);
OCR2A = time_off; // Wecker stellen
}
}
void init_Ports(){
DDRB = 0xFF;
DDRC = 0xFF;
DDRD = 0xFF; //Alles Ausgang
DDRD &= ~(1 << DDD2); //Hallsensoren als Eingänge
DDRD &= ~(1 << DDD3); //Hallsensoren als Eingänge
DDRC &= ~(1 << DDC2); //Hallsensoren als Eingänge
DDRC &= ~(1 << DDC3); //Hallsensoren als Eingänge
PORTD |= (1<<PD2); /* internen Pull-Up an PD2 aktivieren */
PORTD |= (1<<PD3); /* internen Pull-Up an PD3 aktivieren */
PORTC |= (1<<PC2); /* internen Pull-Up an PC2 aktivieren */
PORTC |= (1<<PC3); /* internen Pull-Up an PC2 aktivieren */
EICRA = (1<<ISC01 | 0<<ISC00 | 1<<ISC11 | 0<<ISC10); //Auf sinkende Flanke von Hall1 achten
EIMSK = (1<<INT1 | 1<<INT0 ); //ISR Aktivieren
TCCR2A = (1<<WGM21);
TCCR2B = (1<<CS22) | (1<<CS21) | (1<<CS20); //prescaler 1024, 8 bit count, 1 count = 8ms, 125 count = 1000ms
OCR2A = time_off;
TIMSK2 = (1<<OCIE2A);
}
int main (void){
init_Ports(); //PORT & Timer init
l6229_init();
stall = 0;
sollWert1=0; sollWert2=0;
kp1=10; kp2=10;
ki1=2; ki2=2;
kd1=2; kd2=2;
regler1=regler2=0;
winkel1=winkel2=0;
dir1 = 0;dir2 = 1;
isum2=isum1=0.0f;
q1=q2=0;
e1=e2=0;
modus1=1;modus2=1;
strom2=0;strom1=0;
verzoegerung=0;
fwd_rev(0,dir1);fwd_rev(1,dir2);
bremse(1,1); bremse(0,1);
init_twi_slave(I2CAdresse);
LED(2,1);
for(;;){
//txbuffer[0] = 12; txbuffer[1] = 13; txbuffer[2] = 14; txbuffer[3] = 15;
}
}
ADIO.h
uint16_t ReadChannel(uint8_t mux)
{
uint8_t i;
uint16_t result;
ADCSRA = (1<<ADEN) | (1<<ADPS1) | (1<<ADPS0); // Frequenzvorteiler
// setzen auf 8 (1) und ADC aktivieren (1)
ADMUX = mux; // Kanal waehlen
ADMUX |= (1<<REFS1) | (1<<REFS0); // interne Referenzspannung nutzen
/* nach Aktivieren des ADC wird ein "Dummy-Readout" empfohlen, man liest
also einen Wert und verwirft diesen, um den ADC "warmlaufen zu lassen" */
ADCSRA |= (1<<ADSC); // eine ADC-Wandlung
while ( ADCSRA & (1<<ADSC) ) {
; // auf Abschluss der Konvertierung warten
}
result = ADCW; // ADCW muss einmal gelesen werden,
// sonst wird Ergebnis der nächsten Wandlung
// nicht übernommen.
/* Eigentliche Messung - Mittelwert aus 4 aufeinanderfolgenden Wandlungen */
result = 0;
for( i=0; i<4; i++ )
{
ADCSRA |= (1<<ADSC); // eine Wandlung "single conversion"
while ( ADCSRA & (1<<ADSC) ) {
; // auf Abschluss der Konvertierung warten
}
result += ADCW; // Wandlungsergebnisse aufaddieren
}
ADCSRA &= ~(1<<ADEN); // ADC deaktivieren (2)
result /= 4; // Summe durch vier teilen = arithm. Mittelwert
return result;
}
L6229.c
/*L6229.c
Motortreiber Funktionen
*/
#include "L6229.h"
#include <avr/io.h>
void pwm_init (void) { // Initialisierung:
TCCR1A = (1<<WGM10) | (1<<COM1A1) | (1<<COM1A0) |(1<<COM1B1) | (1<<COM1B0);
TCCR1B = (1<<CS11) | (1<<WGM12);
OCR1B = 100;
OCR1A = 100;
}
void calc(void){
#define BIT_Bremse_1 4;
#define BIT_Enable_1 0;
#define BIT_FWD_REV_1 1;
#define BIT_Bremse_2 3;
#define BIT_Enable_2 6;
#define BIT_FWD_REV_2 2;
/*
Bremse2 = PD4
Enable2 = PD1
Fwd_Rev2 = PB5
*/
if(Motor[0].bremse==0) PORTB |= (1 << PB3);
if(Motor[0].bremse==1) PORTB &= ~(1 << PB3);
if(Motor[0].enable==1) PORTD |= (1 << PD0);
if(Motor[0].enable==0) PORTD &= ~(1 << PD0);
if(Motor[0].fwd_rev==1) PORTB |= (1 << PB0);
if(Motor[0].fwd_rev==0) PORTB &= ~(1 << PB0);
if(Motor[1].bremse==0) PORTB |= (1 << PB4);
if(Motor[1].bremse==1) PORTB &= ~(1 << PB4);
if(Motor[1].enable==1) PORTD |= (1 << PD1);
if(Motor[1].enable==0) PORTD &= ~(1 << PD1);
if(Motor[1].fwd_rev==1) PORTB |= (1 << PB5);
if(Motor[1].fwd_rev==0) PORTB &= ~(1 << PB5);
}
void l6229_init(){
Motor[0].bremse = 0;
Motor[0].fwd_rev= 0;
Motor[0].enable = 0;
Motor[1].bremse = 0;
Motor[1].fwd_rev= 0;
Motor[1].enable = 0;
pwm_init();
calc();
}
void enable(int mot_nr,int value){ //EN bit
if(value==1)Motor[mot_nr].enable=1;
if(value==0)Motor[mot_nr].enable=0;
calc(); //Bits setzten
}
void fwd_rev(int mot_nr,int value){ //FWD REV Bit
Motor[mot_nr].enable=0;
if(value==1)Motor[mot_nr].fwd_rev=1;
if(value==0)Motor[mot_nr].fwd_rev=0;
calc();
}
void bremse(int mot_nr,int value){ //brake bit
if(value==0)Motor[mot_nr].bremse=1;
if(value==1)Motor[mot_nr].bremse=0;
calc();
}
void LED(int nr, int value){
if (value>=1){
switch (nr){
case 1: PORTD |= (1 << PD5); break;
case 2: PORTD |= (1 << PD6); break;
case 3: PORTD |= (1 << PD7); break;
}
}
else{
switch (nr){
case 1: PORTD &= ~(1 << PD5); break;
case 2: PORTD &= ~(1 << PD6); break;
case 3: PORTD &= ~(1 << PD7); break;
}
}
}
beschreibung:
Es werden 2 PWM Timer gestartet OR1A und OCR1B diese PWM werden Tiefpassgefiltert und als Analgospannung dem Motorentreiber l6229 von 0-5volt gegeben.
5 Volt ist volle Geschwindigkeit (ca. OCR1A/B=80), 0 Volt = minimalgeschwindigkeit (OCR1A/B=255;)
in der ADIO.h werden Analogwandlungen gemacht, um den Strom an den Motoren zu messen.
Ich habe die Abtastrate der PID-Regler auf ca. 20ms eingestellt.
Das AVR Studio meldet 99.5% voll in region Text.
Kann mir jemand sagen wie ich die Routinen etwas Optimeiren könnte?
[/code]