Svenska ElektronikForumet

Kod: Markera allt

/* PROGRAMVARA FÖR MOTORSTYRNIGNGSKRETS v3
   Oscar Leon
   CHALMERS TENISKA HÖGSKOLA
   2010-05-11
*/

/* Variablerdeklaration */
int Broms=0; int Set=0; int Resume=0; int Pwm=0; int Puls=0;
int Gas=0; int CC=0; int Hastighet=0; int Timer_ms=0;

/* Funktionsdeklaration */
void interrupt(void);                   // Interruptfunktioner
void InitMain(void);                    // Initieringsrutin
void LasKnappar(void);                  // Funktion som läser av Broms, Set, Resume.
void AnalogStyrning(void);              // Funktion AnalogStyrning
void CruiseControl(void);               // Funktion Cruise Control
void LogikBox(void);                    // Logikfunktion som dutycycle
void MotorStyrning(void);               // Sätter PWM till dutycycle

void InitMain(){
     ANSEL=  0xFF;                      // Sätt AN0 till AN7 som analoga I/O
     ANSELH= 0;                         // Sätt AN7-xx pinnar som digitala I/O
     PORTA=  0xFF;
     TRISA=  0x01;                      // Sätt Analog0 till input
     PORTB=  0xFF;
     TRISB=  0x0F;                      // Sätt RB0-RB3 till input
     PORTC=  0x00;                      // Sätt PORTC till 0
     TRISC=  0x00;                      // Sätt PORTC till output
     ADCON1= 0x80;                      // Config analog input och Vref
     PWM1_INIT(500);                    // Initierar PWM frekvens till 500Hz Vårt optimala är kring 5000-6000Hz

     /* Interrupt settings */
     OPTION_REG.INTEDG=0;               // Interrupt Edge Select bit, negativ flank
     OPTION_REG.T0CS=0;                 // Internal instruction cycle clock (Fosc/4)
     OPTION_REG.PSA=0;                  // Prescaler is assigned to the Timer0 module
     OPTION_REG.PS2=0;
     OPTION_REG.PS1=0;                  // Prescalern sätts till 4 (8MHz/4*256*4=976Hz)
     OPTION_REG.PS0=1;
     INTCON.GIE=1;                      // Enabla globalt interrupt
     INTCON.T0IE=1;                     // Timer0 Overflow Interrupt Enable bit
     INTCON.INTE=1;                     // Enabla INT/RB0 extern interrupt
     INTCON.INTF=0;                     // Nollställ flaggan
     INTCON.T0IF=0;                     // Timer0 Overflow Interrupt Flag bit
}

void interrupt (void){
     if(PORTB.B0==0 && INTCON.T0IF==0){ // Kolla om RB0 är 0, i så fall har vi negativ flank
         Hastighet=1500/Timer_ms;       // Ex: 100 ms mellan 2 flanker, ger en Hastighet på cykeln till 15km/h.
         if(Hastighet<2)
             Hastighet=0;
         Timer_ms=0;
     }
     if(INTCON.T0IF==1){                // Om Timer0 interrupt inträffar (varje ms)
         Timer_ms++;                    // Öka variabeln Timer_ms med 1
         INTCON.T0IF=0;                 // Nollställ Timer0 overflow Interrupt Flag bit
     }
     INTCON.INTF=0;                     // Klarsignalering; Nollställ interruptflagga
}

void main(){
     InitMain();                        // Initieringsvillkor
     while(1){
         LasKnappar();
         AnalogStyrning();
         CruiseControl();
         LogikBox();
         MotorStyrning();
     }
}

/* PIN CONFIG: RB0: Hastighetsmätning, RB1: Set RB2: Resume RB3: Broms */
void LasKnappar(void){
     if(PORTB.B1==1)                    // Setknapp aktiv? (Slutande funktion)
         Set=1;
     if(PORTB.B1==0)
         Set=0;
     if(PORTB.B2==1)                    // Resumeknapp aktiv? (Slutande funktion)
         Resume=1;
     if(PORTB.B2==0)
         Resume=0;
     if(PORTB.B3==0)                    // Broms aktiv? (Brytande funktion)
         Broms=1;
     if(PORTB.B3==1)
         Broms=0;
}

void AnalogStyrning(void){              // Interpolering är kvar att göra
     Gas=ADC_READ(0)>>2;                // Skiftar bort 2 LSB. ad0 har nu ett värde mellan 0 och 255.
     if(Gas<52)                         // Om gasreglaget är nollställd, skicka ut 0.
         Gas=0;
     if(Gas>220)                        // Gasreglage på maximal styrka, skicka full PWM
         Gas=255;
}

void CruiseControl(void){               // Tillfälligt
     CC=0;
//     Hastighet=15;
}

void LogikBox(void){
     if(Broms==0 && Hastighet<25 && Timer_ms<10000 //temporärt){
         if(CC>Gas)                     // Högst av CC eller Gas anger PWM
             PWM=CC;
         else
             PWM=Gas;
     }
     else
         PWM=0;
}

void MotorStyrning(void){
     PWM1_Start();
     PWM1_SET_DUTY(PWM);
}

Kod: Markera allt

     if(INTCON.T0IF==1){                // Om Timer0 interrupt inträffar (varje ms)
         Timer_ms++;                    // Öka variabeln Timer_ms med 1
         INTCON.T0IF=0;                 // Nollställ Timer0 overflow Interrupt Flag bit
     }
     INTCON.INTF=0;                     // Klarsignalering; Nollställ interruptflagga

Kod: Markera allt

      flow_1=ADC_Read(0);
      pid=pid+k_p*flow_0-k_p*flow_1+k_i*set_point-k_i*flow_1;
      flow_0=flow_1;
      if(pid>0x00FFFFFF) pid=0x00FFFFFF;
      else if (pid<0) pid=0;

Kod: Markera allt

flow_1=ADC_Read(0);
      pid=pid+k_p*flow_0-k_p*flow_1+k_i*set_point-k_i*flow_1;
      flow_0=flow_1;
      if(pid>0x00FFFFFF) pid=0x00FFFFFF;
      else if (pid<0) pid=0;

Kod: Markera allt

error=setpoint-process_value;
sumerror+= error;

p_term = KP*error;
i_term = KI*sumerror;

UTny = UTgammal + p_term + i_term;

Kod: Markera allt

error=setpoint-process_value;
sumerror+= error;

p_term = KP*error;
i_term = KI*sumerror;

UTny = UTgammal + p_term + i_term;
UTgammal=UTny

Kod: Markera allt

if(UTny>255) 
   UTny=255; 
if(UTny<0) 
   UTny=0;

Kod: Markera allt

if(sumerror>255)
   sumerror=255;
if(sumerror<-255)
   sumerror=-255;