Arduino Uno - TimerOne - Servo

Witam,
Majsterkuję już od ponad pół roku przy zestawie Arduino Uno lecz ostatni problem spędza mi sen z oczu...

Tworzę regulator PID do własnych potrzeb i z ogólnej ciekawości wykorzystując wszelkie mi dostępne biblioteki. Zatrzymałem się na problemie związanym z "przerwaniem od timera" w którym wykonuje się odczyt wartości z termometru, wyliczenia regulatora, mapowanie wartości dla serwa i wysterowanie serwa. Gdy podłączę układ oraz włączę monitor szeregowy (kreślarkę) w Arduino IDE i uruchomię program - otrzymuję piękny obraz jak reaguje Regulator na zmianę wartości zadanych. Niestety fizycznie nie steruje mi serwem...

Proszę o sugestię...

P.S. Poniżej zamieszczam kod mojego programu... Proszę nie karcić mnie za moje rozwiązania... jestem laikiem który do wszystkiego dochodzi sam poprzez kursy:) Niektóre linijki są zapożyczone inne dopisałem sam - jest mały bałagan ale mam zamiar to uporządkować jak już skończę projekt:)

#include <PID_v1.h>
#include <Servo.h>
#include <Wire.h> 
#include <LiquidCrystal_I2C.h>
#include <TimerOne.h>

LiquidCrystal_I2C lcd(0x38,16,2);  // set the LCD address to 0x38 for a 16 chars and 2 line display

const byte ledPin = 13;
boolean ledState = false;
//Define Variables we'll be connecting to
double Setpoint = 860;
long SetpointUp = 0, SetpointDn = 0, Add = 0;
double  Input, Output;
double Kp=0.5, Ki=1, Kd=35;
int Btup = 3, Btdn = 6, VievscoreM = 0, Btent = 2, Btret = 5, Btupstate = 0, Btdnstate = 0, Btuplstate = 0, Btdnlstate=0;
bool menu = false, czysc = false;
int changemenuA = 0, changemenuB = 0;

unsigned long time, now, lasttime;

//DEKLARACJA PODPROGRAMÓW//
void controller();
void additionSetpoint();
void subtractionSetpoint();
void mainmenu();
void Btreturn();

//Specify the links and initial tuning parameters
PID myPID(&Input, &Output, &Setpoint,Kp,Ki,Kd, DIRECT);

Servo myservo;   // create servo object to control a servo
int val;    // variable to read the value from the analog pin

void setup()
{

 lcd.init();                      // initialize the lcd 
 // Print a message to the LCD.
 lcd.backlight();
 lcd.setCursor(0,0);
 lcd.print("PID CONTROLER");
 lcd.setCursor(0,1);
 lcd.print("VERSION 1.1");
 delay(1500);
 lcd.clear();
 //initialize the variables we're linked to
 Input = analogRead(0);
 myservo.attach(9);  // attaches the servo on pin 10 to the servo object
 myPID.SetMode(AUTOMATIC);//turn the PID on

 pinMode(Btup, INPUT_PULLUP);
 pinMode(Btdn, INPUT_PULLUP);
 pinMode(Btent, INPUT_PULLUP);
 pinMode(Btret, INPUT_PULLUP);

  pinMode(ledPin,OUTPUT);
 digitalWrite(ledPin,ledState);

 // start serial port at 9600 bps:
 Serial.begin(9600);
 while (!Serial) {
   ; // wait for serial port to connect. Needed for native USB port only
 }
Timer1.initialize(100000);

 Timer1.attachInterrupt(controller);


}

void loop(){
if (czysc == true){
lcd.clear();
czysc = false;
}
       lcd.setCursor(0, 0);
       lcd.print("Input=");
       lcd.setCursor(6, 0);
       lcd.print(Input);
       lcd.setCursor(0, 1);
       lcd.print("Setpoint=");  
       lcd.setCursor(9, 1);
       lcd.print(Setpoint);     

           // controller();
            additionSetpoint();
            subtractionSetpoint();
            mainmenu();

               if (Setpoint > 1023){
                 Setpoint = 1023;
                 }

               if (Setpoint < 0){
                 Setpoint = 0;
                 }

/*
 Serial.print(Output);
Serial.print("\t");
Serial.print(Setpoint);
 Serial.print("\t");
 Serial.print(Input);
Serial.println("");
 */

                     delay(5); 
                     SetpointUp = 0;
                     SetpointDn = 0;
                     VievscoreM = 0;
                     lasttime=0;
                     Add=0;
}


//**********************************************************************************************************
//                                      POD PROGRAMY
//**********************************************************************************************************

void controller(){
  Input = analogRead(0);
 myPID.Compute();
  val = map(Output, 0, 255, 23, 45);     // scale it to use it with the servo (value between 0 and 180)
 myservo.write(val);   // sets the servo position according to the scaled value
ledState ? ledState=false : ledState=true;
 digitalWrite(ledPin,ledState);
 delay(15);
}

void additionSetpoint(){

do{
 Btupstate = digitalRead(Btup);
  if(Btupstate != Btuplstate){
             czysc = true;
             if (Btupstate == LOW){
               Setpoint++;
             }
           }
           Btuplstate = Btupstate;



SetpointUp++;

 if (SetpointUp == 300){

   Setpoint++;
   Add++;

   lcd.setCursor(9, 1);
   lcd.print(Setpoint); 
   SetpointUp=0;
    }   
   if (Add > 10){

     Setpoint = Setpoint + 10;
     lcd.setCursor(9, 1);
     lcd.print(Setpoint); 
     delay(300);
   }
     if (Setpoint > 1023){
      Setpoint = 1023;
     }
delay(1);

}

 while (digitalRead(Btup) == LOW);

}



void subtractionSetpoint(){

do{
 Btdnstate = digitalRead(Btdn);

 if(Btdnstate != Btdnlstate){

             if (Btdnstate == LOW){
               Setpoint--;
               czysc = true;
             }
           }

           Btdnlstate = Btdnstate;

SetpointDn++;

 if (SetpointDn == 300){

   Setpoint--;
   Add++;

   lcd.setCursor(9, 1);
   lcd.print(Setpoint); 
   SetpointDn=0;
    }   
   if (Add > 10){

     Setpoint = Setpoint - 10;
     lcd.setCursor(9, 1);
     lcd.print(Setpoint); 
     delay(300);
   }
     if (Setpoint < 0){
      Setpoint = 0;
     }
delay(1);

}

 while (digitalRead(Btdn) == LOW);
 }

void mainmenu(){

 while (digitalRead(Btent) == LOW){
   menu = true;
   czysc = true;
   if (czysc == true){
   lcd.clear();
   czysc = false;
   }
 }
     changemenuA = 1;
     while (menu == true){

       Btupstate = digitalRead(Btup);
       Btdnstate = digitalRead(Btdn);

           if(Btupstate != Btuplstate){

             if (Btupstate == LOW){
               changemenuA--;
               czysc = true;
               if (czysc == true){
               lcd.clear();
               czysc = false;
                }
             }
           }
           Btuplstate = Btupstate;

           if(Btdnstate != Btdnlstate){

             if (Btdnstate == LOW){
               changemenuA++;
               czysc = true;
               if (czysc == true){
               lcd.clear();
               czysc = false;
                }
             }
           }

           Btdnlstate = Btdnstate;

           switch (changemenuA){
           case 1:

           lcd.setCursor(0, 0);
           lcd.print("*MAIN MENU*");
           delay(200);
           break;

           case 2:

           lcd.setCursor(0, 0);
           lcd.print("*CHANGE Kp*");
           lcd.setCursor(6, 1);
           lcd.print(Kp);
           if (digitalRead(Btent) ==LOW){
             changemenuB=changemenuA;
           }
           while (changemenuB ==2){
              lcd.setCursor(5, 1);
              lcd.print("*");
              lcd.setCursor(6, 1);
              lcd.print(Kp);
               czysc = true;
              if (digitalRead(Btup) == LOW){
               Add++;
               Kp = Kp + 0.01 ;
                lcd.setCursor(6, 1);
                lcd.print(Kp);
               if(Add > 10){
                 Kp = Kp + 0.1;
               }
                if(Add > 20){
                 Kp = Kp + 1;
               }
                }

              else if (digitalRead(Btdn) == LOW){
               Add++;
               Kp = Kp - 0.01 ;
                lcd.setCursor(6, 1);
                lcd.print(Kp);
               if(Add > 10){
                 Kp = Kp - 0.1;
               }
                if(Add > 20){
                 Kp = Kp - 1;
               }
               }

              else{
               Add=0;
              }


              Btreturn();
              delay(200);
           }

           delay(200);
           break;

           case 3:

           lcd.setCursor(0, 0);
           lcd.print("*CHANGE Ki*");
           lcd.setCursor(6, 1);
           lcd.print(Ki);
            if (digitalRead(Btent) ==LOW){
             changemenuB=changemenuA;
           }
           while (changemenuB ==3){
              lcd.setCursor(5, 1);
              lcd.print("*");
              lcd.setCursor(6, 1);
              lcd.print(Ki);
              czysc = true;
               if (digitalRead(Btup) == LOW){
               Add++;
               Ki = Ki + 0.01 ;
                lcd.setCursor(6, 1);
                lcd.print(Ki);
               if(Add > 10){
                 Ki = Ki + 0.1;
               }
                if(Add > 20){
                 Ki = Ki + 1;
               }
                }

               else if (digitalRead(Btdn) == LOW){
               Add++;
               Ki = Ki - 0.01 ;
                lcd.setCursor(6, 1);
                lcd.print(Ki);
               if(Add > 10){
                 Ki = Ki - 0.1;
               }
                if(Add > 20){
                 Ki = Ki - 1;
               }
               }

              else{
               Add=0;
              }


              Btreturn();
              delay(200);
           }
           delay(200);
           break;

           case 4:

           lcd.setCursor(0, 0);
           lcd.print("*CHANGE Kd*");
           lcd.setCursor(6, 1);
           lcd.print(Kd);
            if (digitalRead(Btent) ==LOW){
             changemenuB=changemenuA;
           }
           while (changemenuB ==4){
              lcd.setCursor(5, 1);
              lcd.print("*");
              lcd.setCursor(6, 1);
              lcd.print(Kd);
              czysc = true;
              if (digitalRead(Btup) == LOW){
               Add++;
               Kd = Kd + 0.01 ;
                lcd.setCursor(6, 1);
                lcd.print(Kd);
               if(Add > 10){
                 Kd = Kd + 0.1;
               }
                if(Add > 20){
                 Kd = Kd + 1;
               }
                }

               else if (digitalRead(Btdn) == LOW){
               Add++;
               Kd = Kd - 0.01 ;
                lcd.setCursor(6, 1);
                lcd.print(Kd);
               if(Add > 10){
                 Kd = Kd - 0.1;
               }
                if(Add > 20){
                 Kd = Kd - 1;
               }
               }

              else{
               Add=0;
              }

              Btreturn();
              delay(200);
           }

           delay(200);
           break;

            case 5:

           lcd.setCursor(0, 0);
           lcd.print("*SET PID TUNINGS*");
           lcd.setCursor(0, 1);
           lcd.print("*IF YES PLEASE RET");
           while (digitalRead(Btent) == LOW){
             myPID.SetTunings(Kp, Ki, Kd);
           }
           delay(200);
           break;

           case 6:

           lcd.setCursor(0, 0);
           lcd.print("*SAVE TO SD CARD*");
           lcd.setCursor(0, 1);
           lcd.print("*IF YES PLEASE RET");
           delay(200);
           break;


           }
           if (changemenuA > 6){
             changemenuA=6;
           }
            if (changemenuA < 1){
             changemenuA=1;
           }
            while (digitalRead(Btret) == LOW){
            menu = false;
            czysc = true;
            }

     }
}


void Btreturn(){
 if (digitalRead(Btret) == LOW){
   changemenuB = 0;
 }
}

Problemem jest interakcja dwóch mechanizmów: generacji sygnału do serwa i przerwań od timera 1. Podłączanie serwomechanizmów do pinów "za którymi" stoją sprzętowe timery/generatory PWM jest dobrym pomysłem, bo skoro już ten timer w środku siedzi to niech pracuje. No ale jeśli jednocześnie próbujesz wykorzystać ten sam timer do okresowego zgłaszania przerwań, system Arduino musiał coś wybrać. To razem nie zadziała.

Ponieważ biblioteka servo używa timera 1 (i chyba nie można tego zmienić), do zgłaszania swoich wewnętrznych przerwań "organizujących" generację PWM, musisz użyć pozostałego timera 2 i innej biblioteki z nim związanej.

O ile pamiętam do prostego zgłaszania zdarzeń okresowych są też rozwiązania bazujące na standardowo (zawłaszczanym przez Arduino i..) używanym do liczenia okresów 1ms timerze 0. Wtedy timer 2 wciąż zostaje wolny do kolejnych pomysłów - a jest cennym zasobem:)

marek1707 - dziękuję za wyczerpującą wypowiedź w danym temacie. Dzięki wskazówkom rozwiązałem problem i regulator działa w przerwaniu - tym razem od timer 2.

Dziękuję za pomoc!!!