Optimizing code on arduino due

Official Hardware Mega Due
2

CODE from previous message

#include <SPI.h>
#include <Ethernet.h>
#include <PubSubClient.h>
#include "HX711.h"

byte mac[] = {0xDE, 0xAD, 0xBE, 0xEF, 0xFE, 0xED};
IPAddress ip(192, 168, 5, 2);
byte MQTT_SERVER[] = {192,168,5,1};

EthernetClient ethClient;
PubSubClient client;

char send_data_char[128];
char send_data_char_vs[128];
char send_data_char_vd[128];
char send_data_char_gs[128];
char send_data_char_gd[128];
char send_data_char_fs[128];
char send_data_char_fd[128];

volatile int rpmcounts = 0;
volatile int rpmcountd = 0;

float times0, times01, times2=0, rpms=0;
float timed0, timed01, timed2=0, rpmd=0;



unsigned long timehx = 100;
int read_hx = 0;
float gs1, gs2, gs3, gs4, gd1, gd2, gd3, gd4, fs, fd, ps1, ps2, GS, GD;
String send_data;
String send_data_vs; 
String send_data_vd; 
String send_data_gs; 
String send_data_gd; 
String send_data_fs; 
String send_data_fd; 

HX711 s01(22, 23);  // gs1 // (dt, clk)
HX711 s02(24, 25);  // gs2 // (dt, clk)
HX711 s03(26, 27);  // gs3 // (dt, clk)
HX711 s04(28, 29);  // gs4 // (dt, clk)

HX711 s05(30, 31);  // gd1 // (dt, clk)
HX711 s06(32, 33);  // gd2 // (dt, clk)
HX711 s07(34, 35);  // gd3 // (dt, clk)
HX711 s08(36, 37);  // gd4 // (dt, clk)

HX711 s09(38, 39);  // fs // (dt, clk)
HX711 s10(40, 41);  // fd // (dt, clk)

//HX711 s11(42, 43);  // ps1 // (dt, clk)
//HX711 s12(44, 45);  // ps2 // (dt, clk)

int R1 = 46;
int R2 = 47;
int R3 = 48;
int R4 = 49;
int R5 = 5;

const int prezs_pin = 14;
const int prezd_pin = 15;

int prezs = 0, prezd = 0;

void setup() {
 // Serial.begin(9600);
 // Serial.println("START");

  pinMode(R1, OUTPUT);
  digitalWrite(R1, HIGH); // R1 OFF
  pinMode(R2, OUTPUT);
  digitalWrite(R2, HIGH); // R2 OFF
  pinMode(R3, OUTPUT);
  digitalWrite(R3, HIGH); // R3 OFF
  pinMode(R4, OUTPUT);
  digitalWrite(R4, HIGH); // R4 OFF
  pinMode(R5, OUTPUT);
  digitalWrite(R5, HIGH); // R5 OFF
  
  s01.set_scale(2500);       //calib gs2
  s01.tare();             
  s02.set_scale(2500);       //calib gs2
  s02.tare();
  s03.set_scale(2500);       //calib gs3
  s03.tare();
  s04.set_scale(2500);       //calib gs4
  s04.tare();

  s05.set_scale(2500);       //calib gd1
  s05.tare();
  s06.set_scale(2500);       //calib gd2
  s06.tare();
  s07.set_scale(2500);       //calib gd3
  s07.tare();
  s08.set_scale(2500);       //calib gd4
  s08.tare();

  s09.set_scale(2500);       //calib fs
  s09.tare();
  s10.set_scale(2500);       //calib fd
  s10.tare();

//  s11.set_scale(2500);       //calib ps1
//  s11.tare();
//  s12.set_scale(2500);       //calib ps2
//  s12.tare();

  Ethernet.begin(mac, ip);
  client  = PubSubClient(MQTT_SERVER, 1883, callback, ethClient);


//  Serial.print("Network ip : ");
//  Serial.println(Ethernet.localIP());

pinMode(prezs_pin, INPUT);
pinMode(prezd_pin, INPUT);

attachInterrupt(2, hallSensors, RISING);//interrupt 0 is on pin 2.
attachInterrupt(3, hallSensord, RISING);//interrupt 1 is on pin 3.

}

void loop() {

  if (!client.connected())  {
    client.connect("std_vs");
    client.connect("std_vd");
    client.connect("std_gs");
    client.connect("std_gd");
    client.connect("std_fs");
    client.connect("std_fd");
    client.subscribe("stdin");
      }
  
  read_hx++;
  if (read_hx == 1){
      timehx = millis();
  }
  if (millis() - timehx >= 100){
   
    send_data_vs = String(rpms);
    send_data_vs.toCharArray(send_data_char_vs, send_data_vs.length() + 1);
    client.publish("std_vs", send_data_char_vs);
    
    send_data_vd = String(rpmd);
    send_data_vd.toCharArray(send_data_char_vd, send_data_vd.length() + 1);
    client.publish("std_vd", send_data_char_vd);

    send_data_gs = String(GS);
    send_data_gs.toCharArray(send_data_char_gs, send_data_gs.length() + 1);
    client.publish("std_gs", send_data_char_gs);

    send_data_gd = String(GD);
    send_data_gd.toCharArray(send_data_char_gd, send_data_gd.length() + 1);
    client.publish("std_gd", send_data_char_gd);

    send_data_fs = String(fs);
    send_data_fs.toCharArray(send_data_char_fs, send_data_fs.length() + 1);
    client.publish("std_fs", send_data_char_fs);

    send_data_fd = String(fd);
    send_data_fd.toCharArray(send_data_char_fd, send_data_fd.length() + 1);
    client.publish("std_fd", send_data_char_fd);
  
    read_hx = 0;
  }

  readhx();
  
  read_prezs();
  read_prezd();
  
  client.loop();
}

void readhx(){

  gs1 = s01.get_units(3);
  gs2 = s02.get_units(3);
  gs3 = s03.get_units(3);
  gs4 = s04.get_units(3);
  GS = gs1 + gs2 + gs3 + gs4;
  
  
  gd1 = s05.get_units(3);
  gd2 = s06.get_units(3);
  gd3 = s07.get_units(3);
  gd4 = s08.get_units(3);
  GD = gd1 + gd2 + gd3 + gd4;


  fs = s09.get_units(3);
  fd = s10.get_units(3);

//    ps1 = s11.get_units();
//    ps2 = s12.get_units();

}

void startR1() {
  digitalWrite(R1, LOW);
}

void startR2() {
  digitalWrite(R2, LOW);
}

void startR3() {
  digitalWrite(R3, LOW);
}

void startR4() {
  digitalWrite(R4, LOW);
}

void startR5() {
  digitalWrite(R5, LOW);
}

void stopR1() {
  digitalWrite(R1, HIGH);
}

void stopR2() {
  digitalWrite(R2, HIGH);
}

void stopR3() {
  digitalWrite(R3, HIGH);
}

void stopR4() {
  digitalWrite(R4, HIGH);
}

void stopR5() {
  digitalWrite(R5, HIGH);
}

void read_prezs(){
  prezs = digitalRead(prezs_pin);
  if (prezs == HIGH){
    prezs = 1;
  }else{
    prezs = 0;
  }
}

void read_prezd(){
  prezd = digitalRead(prezd_pin);
  if (prezd == HIGH){
    prezd = 1;
  }else{
    prezd = 0;
  }
}

void hallSensors(){

 rpmcounts++;
 if (rpmcounts == 1){
   times01 = millis();
 }
 if (rpmcounts == 2){
   times2 = millis() - times01;
   rpms = (1 / (times2 / 1000) * 60);
   
   rpmcounts=0;  
 }
}

void hallSensord(){
  rpmcountd++;
  if (rpmcountd == 1){
    timed01 = millis();
  }
  if (rpmcountd == 2){
    timed2 = millis() - timed01;
    rpmd = (1 / (timed2 / 1000) * 60);

    rpmcountd=0;  
  }
}

void callback(char* topic, byte* payload, unsigned int length) {
    
  if (String((char)payload[0]) == "R") {
      if ((char)payload[1] == '1') {
          if ((char)payload[2] == '1') {
              startR1();
          }
          else if ((char)payload[2] == '0') { 
              stopR1();
          }
    }
    else if ((char)payload[1] == '2') {
          if ((char)payload[2] == '1') {
              startR2();
          }
          else if ((char)payload[2] == '0') { 
              stopR2();
          }
    }
    else if ((char)payload[1] == '3') {
          if ((char)payload[2] == '1') {
              startR3();
          }
          else if ((char)payload[2] == '0') { 
              stopR3();
          }
    }
    else if ((char)payload[1] == '4') {
          if ((char)payload[2] == '1') {
              startR4();
          }
          else if ((char)payload[2] == '0') { 
              stopR4();
          }
    }
    else if ((char)payload[1] == '5') {
          if ((char)payload[2] == '1') {
              startR5();
          }
          else if ((char)payload[2] == '0') { 
              stopR5();
          }
  
   }
  else if (String((char)payload[0]) == "T") {
      if ((char)payload[1] == '1') {  // tare st
            s01.tare();             
            s02.tare();
            s03.tare();
            s04.tare();
      }
      else if ((char)payload[1] == '2') {  //tare dr 
            s05.tare();
            s06.tare();
            s07.tare();
            s08.tare();
      }
      else if ((char)payload[1] == '3') {  //tare FS / FD 
            s09.tare();
            s10.tare();
      }
//      else if ((char)payload[1] == '4') {  //tare Pres1 
//            s11.tare();
//      }
//      else if ((char)payload[1] == '5') {  //tare Pres2 
//            s12.tare();
//      }
        
  }
  
  }
}