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
Zitieren
Lesezeichen