Optymalizacja kodu - co poprawić

[Belstar]

Witam wszystkich bo jestem tu nowy.

Buduje sobie urządzenie które będzie zasilane silnikiem z taniego blendera. Ponieważ silnik ma dość wysokie obroty. Wykorzystałem oryginalny układ kontroli prędkości blendera lekko zmodyfikowany, wpoiłem dodatkowy rezystor szeregowo z potencjometrem sterującym obrotami. Postanowiłem też dodać jakiś czujnik który by je mierzył - padło na halotron i arduino uno. Gdy już było cacy, stwierdziłem że silnik pod obciążenie się trochę grzeje, a że układ będzie zamknięty ze względów bezpieczeństwa, dodałem też czujnik DS18B20 mierzący temperaturę. Całość w tej chwili przedstawia się następująco:

Po włączeniu zasilania na wyświetlaczu wyświetlają się aktualne obroty silnika, po wciśnięciu przycisku wyświetlacz przełącza się na okres trzech sekund i w tym czasie pokazuje temperaturę silnika. Dodałem też diodę dwu kolorową (red green ) której zadaniem jest pokazywanie jaki jest stan temperatury bez potrzeby naciskania przycisku (zielona dobrze, czerwona źle i w miedzy czasie pośrednie kolory). W układzie pozostała też dioda z przykładów kursowych na pinie 13 i włącza się ona po naciśnięciu przycisku na okres taki jak wyświetlacz. I teraz mój problem: Co byście zmienili w kodzie by uczynić go bardziej optymalnym. Kod ten jest mieszanką różnych kursowych przykładów i tym co znalazłem w sieci. Muszę przyznać że tworzenie kodu dla Arduino gdy się ma przyzwyczajenia z VBA jest bardzo wkurzające(średniki, klamerki, przypisania, porównania itd).

Zaznaczam że nie pytam o gotowce, ale wskazówki co usprawnić.

// Adres czujnika DS18B20
//0x28, 0xFF, 0x4F, 0xB4, 0x90, 0x15, 0x3, 0x88
//biblioteki czujnika temperatury
//halotron nie działa! Zmień biegunowość magnesu
#include <OneWire.h>
#include <DS18B20.h>
//biblioteki OLED
#include <SPI.h>
#include <Wire.h>
#include <Adafruit_GFX.h>
#include <Adafruit_SSD1306.h>

#define OLED_RESET 4
Adafruit_SSD1306 display(OLED_RESET);

// Numer pinu do którego podłaczasz czujnik
#define ONEWIRE_PIN 10
// Adres czujnika
byte address[8] = {0x28, 0xFF, 0x4F, 0xB4, 0x90, 0x15, 0x3, 0x88};
OneWire onewire(ONEWIRE_PIN);
DS18B20 sensors(&onewire);
//***********************obsługa przycisku i ledów************************************
const int buttonPin = 12;               
const int ledPin = 13;
//Definicja wyprowadzeń pierwszej diody
#define R 5
#define G 6
//#define B 9                 


int Stan = LOW;                  
int buttonState;                       
int lastButtonState = LOW; 
unsigned long lastDebounceTime = 0;     
unsigned long debounceDelay = 50;       
unsigned long lastTime = 0;
unsigned long Delay = 3000;
//************************************************************************************

//------code for RPM----------------
//int encoder_pin = 2;
// The pin the encoder is connected
unsigned int rpm;
// rpm reading
volatile byte pulses;
// number of pulses
unsigned long timeold;
// The number of pulses per revolution
// depends on your index disc!!
unsigned int pulsesperturn = 1;
void setup() 
 {
   //while(!Serial);
   //Serial.begin(9600);
   pinMode(R, OUTPUT);
   pinMode(G, OUTPUT);
  // pinMode(B, OUTPUT);
   sensors.begin(9);
   sensors.request(address);
   display.begin(SSD1306_SWITCHCAPVCC, 0x3C);
   pinMode(buttonPin, INPUT);
   pinMode(ledPin, OUTPUT);
   digitalWrite(ledPin, Stan);
   attachInterrupt(digitalPinToInterrupt(2), counter, FALLING);

   pulses = 0;
   rpm = 0;
   timeold = 0;
 }

int valLen = 8;
int displayWidth = 128;
int letterWidth = 12;

void loop() 
 {
   int reading = digitalRead(buttonPin);

   if (reading != lastButtonState) 
     {                                     
       lastDebounceTime = millis();
       lastTime = millis();
     }
   if ((millis() - lastDebounceTime) > debounceDelay) 
     {
       if (reading != buttonState) 
         {
           buttonState = reading;
           if (buttonState == HIGH) 
             {
               Stan = !Stan;
             }
         }
     }                                        
   digitalWrite(ledPin, Stan);
   lastButtonState = reading;
   if (Stan == HIGH)
     {
       if (sensors.available())
         {
           updateDisplay_LED(false, 0); 
         }
       if (millis() - lastTime > Delay)
       {
         digitalWrite(ledPin, Stan);
         Stan = !Stan;
       }
     }
   else
     {
     if (millis() - timeold >= 1000)
       {
                                     /*Uptade every one second, this will be equal to reading frecuency (Hz).*/
                                     //Don't process interrupts during calculations
         detachInterrupt(0);
                                     //Note that this would be 60*1000/(millis() - timeold)*pulses if the interrupt
                                     //happened once per revolution
         rpm = (60 * 1000 / pulsesperturn )/ (millis() - timeold)* pulses;
         timeold = millis();
         pulses = 0;
                                     //Write it out to serial port
                                     //Serial.print("RPM = ");
                                     //Serial.println(rpm,DEC);
         updateDisplay_LED(true, rpm);
         sensors.request(address);
                                     //Restart the interrupt processing
         attachInterrupt(digitalPinToInterrupt(2), counter, FALLING);
       }
     }
 }

void counter()
 {  //Update count
     pulses++;
 }

void updateDisplay_LED(boolean Co, int rpm)
 {
   if(rpm > 9999)
     valLen = 5 + 4;
   else if(rpm > 999)
     valLen = 4 + 4;
   else if(rpm > 99 ||Co == false)
     valLen = 3 + 4;
   else if(rpm > 9)
     valLen = 2 + 4;
   else
     valLen = 1 + 4;


   float temperature = sensors.readTemperature(address);

   if (temperature <= 22)
     {
       analogWrite(G, 255);
       analogWrite(R, 0); 
     }
   else if (temperature <= 25)
     {
       analogWrite(G, 255);
       analogWrite(R, 60);  
     }
   else if (temperature <= 30)
     {
       analogWrite(G, 255);
       analogWrite(R, 120);
     }
   else if (temperature <= 32)
     {
       analogWrite(G, 120);
       analogWrite(R, 255);
     }
   else if (temperature <= 35)
     {
       analogWrite(G, 60);
       analogWrite(R, 255);
     }   
   else if (temperature <= 40)
     {
       analogWrite(G, 0);
       analogWrite(R, 255);
     }

   //-------------update display------------
   display.clearDisplay();
   display.setTextSize(2);
   display.setTextColor(WHITE);
   display.setCursor(28,0);
   display.println("GPTOOL");
   display.setTextColor(WHITE);
   display.setCursor((displayWidth/2)-(letterWidth/2*valLen),17);
   display.setTextSize(2);

   if (Co == true)
     {
       display.print(rpm); 
       display.println(" RPM"); 
     }
   else
     {
       display.setCursor((displayWidth/2)-(letterWidth/2*valLen),17);
       display.print(temperature);
       display.println(F(" 'C"));
     }
   display.display();
   //----------------------
 }

Pozdrawiam