Here is my code. Normally, my servos are being positioned between 0-180 degrees. I took position values from HMI according to my adjust. But sometimes some servo positions comes abnormal values like 2300, 1800 etc without my intervention by me. Problem is this.
#include <SoftwareSerial.h>
#include <EEPROM.h>
#include <Servo.h>
#include <Wire.h>
Servo servobir;
Servo servoiki;
unsigned long btnPressTime = 0;
unsigned long oldtime1 =0; //for data reading from dwin
unsigned long oldtime2 =0; // for data saving
unsigned long oldtime3 =0; // for gear counting
unsigned long oldtime4 = 0;// for servo control
unsigned long oldtime5 = 0; // for gear counting a int reset
unsigned long oldtime6 = 0; // for RPM writing to Dwin
unsigned long oldtime7 = 0; // for Speed writing to Dwin
//Write Buttons Variables
int servoyazdir = 0;
int sensoryazdir =0;
int tarlayazdir = 0;
int otomatikyazdir = 0;
//Dwin Datas Saving Addresses
int servobirnotr;
int servoikinotr;
int vitesbir;
int vitesiki;
int vitesuc;
int vitesdort;
int devirsensor;
int hizsensor;
int hidroliksensor;
int vitesartir;
int vitesazalt;
// Memory Addresses
int servobirnotrhafiza;
int servoikinotrhafiza;
int vitesbirhafiza;
int vitesikihafiza;
int vitesuchafiza;
int vitesdorthafiza;
int devirsensorhafiza;
int hizsensorhafiza;
int hidroliksensorhafiza;
int vitesartirhafiza;
int vitesazalthafiza;
//Data Write To Dwin Addresses
unsigned char Buffer[9];
unsigned char servobirnotrdeger[8] = {0x5a, 0xa5, 0x05, 0x82, 0x70, 0x00, 0x00, 0x00};
unsigned char vitesbirdeger[8] = {0x5a, 0xa5, 0x05, 0x82, 0x71, 0x00, 0x00, 0x00};
unsigned char vitesikideger[8] = {0x5a, 0xa5, 0x05, 0x82, 0x72, 0x00, 0x00, 0x00};
unsigned char servoikinotrdeger[8] = {0x5a, 0xa5, 0x05, 0x82, 0x73, 0x00, 0x00, 0x00};
unsigned char vitesucdeger[8] = {0x5a, 0xa5, 0x05, 0x82, 0x74, 0x00, 0x00, 0x00};
unsigned char vitesdortdeger[8] = {0x5a, 0xa5, 0x05, 0x82, 0x75, 0x00, 0x00, 0x00};
unsigned char devirsensordeger[8] = {0x5a, 0xa5, 0x05, 0x82, 0x56, 0x00, 0x00, 0x00};
unsigned char hizsensordeger[8] = {0x5a, 0xa5, 0x05, 0x82, 0x57, 0x00, 0x00, 0x00};
unsigned char hidroliksensordeger[8] = {0x5a, 0xa5, 0x05, 0x82, 0x58, 0x00, 0x00, 0x00};
unsigned char vitesartirdeger[8] = {0x5a, 0xa5, 0x05, 0x82, 0x90, 0x00, 0x00, 0x00};
unsigned char vitesazaltdeger[8] = {0x5a, 0xa5, 0x05, 0x82, 0x91, 0x00, 0x00, 0x00};
unsigned char vitesdeger[8] = {0x5a, 0xa5, 0x05, 0x82, 0x50 , 0x00, 0x00, 0x00};
unsigned char rpmdeger[8] = {0x5a, 0xa5, 0x05, 0x82, 0x52 , 0x00, 0x00, 0x00};
unsigned char hizdeger[8] = {0x5a, 0xa5, 0x05, 0x82, 0x53 , 0x00, 0x00, 0x00};
unsigned char incomingdata[9];
unsigned int incomingdwin;
// Shift value + additional variables
int vites = 0;
int count1;
int count2;
boolean a = LOW;
boolean b = LOW;
boolean c = LOW;
boolean debounceDone;
// ********************** Engine RPM Measurement Variables *************************
const int IR_PIN = 3; // IR sensor out pin is connected to arduino pin 2 which is interrupt pin
volatile unsigned int counter = 0; // Counter variable for revolutions
unsigned long previousMillis = 0; // Variable to store previous time
unsigned int rpm = 0; // Variable to store RPM value
void setup()
{
pinMode(15,INPUT); //shift + button
pinMode(7,INPUT); // shift - button
pinMode(4,INPUT); // shift notr button
pinMode(9,INPUT); // Hand clutch input (D9)
pinMode(12,OUTPUT); // Hand clutch relay (D12)
vites = EEPROM.read(15);
servobirnotrhafiza = EEPROM.read(0);
vitesbirhafiza = EEPROM.read(1);
vitesikihafiza = EEPROM.read(2);
servoikinotrhafiza = EEPROM.read(3);
vitesuchafiza = EEPROM.read(4);
vitesdorthafiza = EEPROM.read(5),
devirsensorhafiza = EEPROM.read(6);
hizsensorhafiza = EEPROM.read(7);
hidroliksensorhafiza = EEPROM.read(8);
vitesartirhafiza = EEPROM.read(9);
vitesazalthafiza = EEPROM.read(10);
servobir.attach(5);
servoiki.attach(6);
Serial.begin(9600);
delay(500);
servobirnotrdeger[6] = highByte(servobirnotrhafiza);
servobirnotrdeger[7] = lowByte(servobirnotrhafiza);
Serial.write(servobirnotrdeger, 8);
vitesbirdeger[6] = highByte(vitesbirhafiza);
vitesbirdeger[7] = lowByte(vitesbirhafiza);
Serial.write(vitesbirdeger, 8);
vitesikideger[6] = highByte(vitesikihafiza);
vitesikideger[7] = lowByte(vitesikihafiza);
Serial.write(vitesikideger, 8);
servoikinotrdeger[6] = highByte(servoikinotrhafiza);
servoikinotrdeger[7] = lowByte(servoikinotrhafiza);
Serial.write(servoikinotrdeger, 8);
vitesucdeger[6] = highByte(vitesuchafiza);
vitesucdeger[7] = lowByte(vitesuchafiza);
Serial.write(vitesucdeger, 8);
vitesdortdeger[6] = highByte(vitesdorthafiza);
vitesdortdeger[7] = lowByte(vitesdorthafiza);
Serial.write(vitesdortdeger, 8);
devirsensordeger[6] = highByte(devirsensorhafiza);
devirsensordeger[7] = lowByte(devirsensorhafiza);
Serial.write(devirsensordeger, 8);
hizsensordeger[6] = highByte(hizsensorhafiza);
hizsensordeger[7] = lowByte(hizsensorhafiza);
Serial.write(hizsensordeger, 8);
hidroliksensordeger[6] = highByte(hidroliksensorhafiza);
hidroliksensordeger[7] = lowByte(hidroliksensorhafiza);
Serial.write(hidroliksensordeger, 8);
vitesartirdeger[6] = highByte(vitesartirhafiza);
vitesartirdeger[7] = lowByte(vitesartirhafiza);
Serial.write(vitesartirdeger, 8);
vitesazaltdeger[6] = highByte(vitesazalthafiza);
vitesazaltdeger[7] = lowByte(vitesazalthafiza);
Serial.write(vitesazaltdeger, 8);
vitesdeger[6] = highByte(vites);
vitesdeger[7] = lowByte(vites);
Serial.write(vitesdeger, 8);
//----- Engine RPM Parameter set up -------
pinMode(IR_PIN, INPUT); // using internal pull up resistors of arduino as pin 2 interrupt is triggered on logic low (falling edge)
attachInterrupt(digitalPinToInterrupt(IR_PIN), IRinterrupt, FALLING);
}
void loop()
{
// ************** Shift Counter Area *******
if(millis() - btnPressTime > 250) debounceDone = true;
if(digitalRead(7) == HIGH && vites<=3 && debounceDone == true && a == 0)
{
vites++;
vitesdeger[6] = highByte(vites);
vitesdeger[7] = lowByte(vites);
Serial.write(vitesdeger, 8);
btnPressTime = millis();
debounceDone = false;
a = 1;
Serial.print("vites:");
Serial.println(vites);
}
if(millis() - btnPressTime > 250) debounceDone = true;
if(digitalRead(4) == HIGH && vites>0 && debounceDone == true && b == 0)
{
vites--;
vitesdeger[6] = highByte(vites);
vitesdeger[7] = lowByte(vites);
Serial.write(vitesdeger, 8);
btnPressTime = millis();
debounceDone = false;
b = 1;
Serial.print("vites:");
Serial.println(vites);
}
if(millis() - btnPressTime > 250) debounceDone = true;
if(digitalRead(15) == HIGH && vites>0 && debounceDone == true && c == 0)
{
vites = 0;
vitesdeger[6] = highByte(vites);
vitesdeger[7] = lowByte(vites);
Serial.write(vitesdeger, 8);
btnPressTime = millis();
debounceDone = false;
c = 1;
Serial.print("vites:");
Serial.println(vites);
}
// ********************************** Servo Positioning ****************************
if( vites == 0)
{
servobir.write(servobirnotrhafiza);
servoiki.write(servoikinotrhafiza);
if (digitalRead(7) == HIGH || digitalRead(4) == HIGH || digitalRead(15) == HIGH)
{
EEPROM.write(15,vites);
vitesdeger[6] = highByte(vites);
vitesdeger[7] = lowByte(vites);
Serial.write(vitesdeger, 8);
Serial.print("Servobirpozisyon:");
Serial.println(servobirnotrhafiza);
Serial.print("Servoikipozisyon:");
Serial.println(servoikinotrhafiza);
}
}
if( vites == 1)
{
servoiki.write(servoikinotrhafiza);
servobir.write(vitesbirhafiza);
if (digitalRead(7) == HIGH || digitalRead(4) == HIGH || digitalRead(15) == HIGH)
{
EEPROM.write(15,vites);
vitesdeger[6] = highByte(vites);
vitesdeger[7] = lowByte(vites);
Serial.write(vitesdeger, 8);
Serial.print("Servobirpozisyon:");
Serial.println(vitesbirhafiza);
Serial.print("Servoikipozisyon:");
Serial.println(servoikinotrhafiza);
}
}
if( vites == 2)
{
servoiki.write(servoikinotrhafiza);
delay(400);
servobir.write(vitesikihafiza);
if (digitalRead(7) == HIGH || digitalRead(4) == HIGH || digitalRead(15) == HIGH)
{
EEPROM.write(15,vites);
vitesdeger[6] = highByte(vites);
vitesdeger[7] = lowByte(vites);
Serial.write(vitesdeger, 8);
Serial.print("Servobirpozisyon:");
Serial.println(vitesikihafiza);
Serial.print("Servoikipozisyon:");
Serial.println(servoikinotrhafiza);
}
}
if( vites == 3)
{
servobir.write(servobirnotrhafiza);
delay(400);
servoiki.write(vitesuchafiza);
if (digitalRead(7) == HIGH || digitalRead(4) == HIGH || digitalRead(15) == HIGH)
{
EEPROM.write(15,vites);
vitesdeger[6] = highByte(vites);
vitesdeger[7] = lowByte(vites);
Serial.write(vitesdeger, 8);
Serial.print("Servobirpozisyon:");
Serial.println(servobirnotrhafiza);
Serial.print("Servoikipozisyon:");
Serial.println(vitesuchafiza);
}
}
if( vites == 4)
{
servobir.write(servobirnotrhafiza);
servoiki.write(vitesdorthafiza);
if (digitalRead(7) == HIGH || digitalRead(4) == HIGH || digitalRead(15) == HIGH)
{
EEPROM.write(15,vites);
vitesdeger[6] = highByte(vites);
vitesdeger[7] = lowByte(vites);
Serial.write(vitesdeger, 8);
Serial.print("Servobirpozisyon:");
Serial.println(servobirnotrhafiza);
Serial.print("Servoikipozisyon:");
Serial.println(vitesdorthafiza);
}
}
if ( millis () - oldtime5 > 90 )
{
if ( a == 1 && digitalRead(7) == LOW)
{
a = 0;
}
if ( b == 1 && digitalRead(4) == LOW)
{
b = 0;
}
if ( c == 1 && digitalRead(15) == LOW)
{
c = 0;
}
oldtime5 = millis();
}
//********************* Engine RPM Calculation ************************************
unsigned long currentMillis = millis(); // only update rpm values after 1 sec interval
if (currentMillis - previousMillis >= 1000)
{
detachInterrupt(digitalPinToInterrupt(IR_PIN)); // disable interrupt on pin 2 during this calculation because any comming interrupt on this moment will increment the counter and calculation result will change
rpm = (counter / 1) * 60; // Calculate RPM . use the value of pulses you get per revolution in your rotating object
counter = 0; // reset counter value after calculating rmp
attachInterrupt(digitalPinToInterrupt(IR_PIN), IRinterrupt, FALLING); // enable the interrupt again to start revolutions counting again
previousMillis = currentMillis;
rpmdeger[6] = highByte(rpm);
rpmdeger[7] = lowByte(rpm);
Serial.write(rpmdeger, 8);
Serial.print("RPM degeri:");
Serial.println(rpm);
}
//*************************** Speed Calculation (will be added later)***************************
// *********** Hand Clutch Button Circuit (will be added later)***********************
Dwin_to_Arduino();
delay(10);
Arduino_to_Dwin();
delay(10);
}
void IRinterrupt()
{ // this inerrupt function is called whenever a falling edge is triggered on arduino pin 3
counter++; // incriment the revolutions counter on each interrupt
}
//********Data Reading From Dwin***********
void Dwin_to_Arduino()
{
if(millis() - oldtime1 >200)
{
if(Serial.available()>0)
{
int i=0;
while (Serial.available() > 0)
{
char comingByte = Serial.read();
delay(2);
incomingdata[i] = comingByte;
i++;
}
if (incomingdata[3] == (byte)0x83)
{
incomingdwin = (incomingdata[7]<<8) | incomingdata[8];
Serial.println(incomingdwin);
}
if (incomingdata[0] ==0x5A)
{
switch(incomingdata[4])
{
case 0x20: //servo yazdir butonu
if(incomingdata[8]==1)
{
Serial.println("Servo ekranı yazdir butonu ON");
servoyazdir = 1;
}
else
{
Serial.println("Servo ekranı yazdir butonu OFF");
servoyazdir = 0;
}
break;
case 0x21: //sensör ekranı yazdir butonu
if(incomingdata[8]==1)
{
Serial.println("Sensör ekranı yazdir butonu ON");
sensoryazdir = 1;
}
else
{
Serial.println("Sensör ekranı yazdir butonu OFF");
sensoryazdir=0;
}
break;
case 0x22: //tarla sayfası yazdir butonu
if(incomingdata[8]==1)
{
Serial.println("Tarla ekranı yazdir butonu ON");
tarlayazdir = 1;
}
else
{
Serial.println("Tarla ekranı yazdir butonu OFF");
tarlayazdir=0;
}
break;
case 0x23: //otomatik vites on/off butonu
if(incomingdata[8]==1)
{
Serial.println("otomatik vites butonu ON");
otomatikyazdir = 1;
}
else
{
Serial.println("otomatik vites butonu OFF");
otomatikyazdir=0;
}
break;
case 0x60: //Birinci servo nötr pozisyon ekrandan gelen parametre
if(incomingdata[8]>=1)
{
servobirnotr = incomingdwin;
}
break;
case 0x61: //Vites bir pozisyon ekrandan gelen parametre
if(incomingdata[8]>=1)
{
vitesbir = incomingdwin;
}
break;
case 0x62: //Vites iki pozisyon ekrandan gelen parametre
if(incomingdata[8]>=1)
{
vitesiki = incomingdwin;
}
break;
case 0x63: //İkinci Servo nötr pozisyon ekrandan gelen parametre
if(incomingdata[8]>=1)
{
servoikinotr = incomingdwin;
}
break;
case 0x64: //Vites üç pozisyon değeri
if(incomingdata[8]>=1)
{
vitesuc = incomingdwin;
}
break;
case 0x65: //Vites dört pozisyon değeri
if(incomingdata[8]>=1)
{
vitesdort = incomingdwin;
}
break;
case 0x80: //Devir sensor çarpan parametre değeri
if(incomingdata[8]>=1)
{
devirsensor = incomingdwin;
}
break;
case 0x81: //Hiz sensor çarpan parametre değeri
if(incomingdata[8]>=1)
{
hizsensor = incomingdwin;
}
break;
case 0x82: //hidrolik sensor parametre değeri
if(incomingdata[8]>=1)
{
hidroliksensor = incomingdwin;
}
break;
case 0x92: //Vites artırma devri
if(incomingdata[8]>=1)
{
vitesartir = incomingdwin;
}
break;
case 0x93: //Vites artırma devri
if(incomingdata[8]>=1)
{
vitesazalt = incomingdwin;
}
break;
}
}
memset(incomingdata,0,sizeof(incomingdata));
}
oldtime1 = millis();
}
}
// ********Data write to Dwin from Arduino**************
void Arduino_to_Dwin()
{
if(millis() - oldtime2 >200 && servoyazdir ==1)
{
switch(servoyazdir)
{
case 1:
if(servoyazdir >0)
{
if(servobirnotr >0 && servobirnotr < 180)
{
servobirnotrhafiza = servobirnotr;
servobirnotrdeger[6] = highByte(servobirnotrhafiza);
servobirnotrdeger[7] = lowByte(servobirnotrhafiza);
Serial.write(servobirnotrdeger, 8);
EEPROM.write(0,servobirnotrhafiza);
Serial.print("Servobirnotr hafizaya alınan deger:");
Serial.println(servobirnotrhafiza);
servobirnotr = 0;
}
if(vitesbir >0 && vitesbir < 180)
{
vitesbirhafiza = vitesbir;
vitesbirdeger[6] = highByte(vitesbirhafiza);
vitesbirdeger[7] = lowByte(vitesbirhafiza);
Serial.write(vitesbirdeger, 8);
EEPROM.write(1,vitesbirhafiza);
Serial.print("Vitesbir hafizaya alınan deger:");
Serial.println(vitesbirhafiza);
vitesbir =0;
}
if(vitesiki >0 && vitesiki < 180 )
{
vitesikihafiza = vitesiki;
vitesikideger[6] = highByte(vitesikihafiza);
vitesikideger[7] = lowByte(vitesikihafiza);
Serial.write(vitesikideger, 8);
EEPROM.write(2,vitesikihafiza);
Serial.print("Vitesiki hafizaya alınan deger:");
Serial.println(vitesikihafiza);
vitesiki = 0;
}
if(servoikinotr >0 && servoikinotr < 180)
{
servoikinotrhafiza = servoikinotr;
servoikinotrdeger[6] = highByte(servoikinotrhafiza);
servoikinotrdeger[7] = lowByte(servoikinotrhafiza);
Serial.write(servoikinotrdeger, 8);
EEPROM.write(3,servoikinotrhafiza);
Serial.print("Servoikinotr hafizaya alınan deger:");
Serial.println(servoikinotrhafiza);
servoikinotr = 0;
}
if(vitesuc >0 && vitesuc < 180)
{
vitesuchafiza = vitesuc;
vitesucdeger[6] = highByte(vitesuchafiza);
vitesucdeger[7] = lowByte(vitesuchafiza);
Serial.write(vitesucdeger, 8);
EEPROM.write(4,vitesuchafiza);
Serial.print("Vitesuc hafizaya alınan deger:");
Serial.println(vitesuchafiza);
vitesuc = 0;
}
if(vitesdort >0 && vitesdort <180)
{
vitesdorthafiza = vitesdort;
vitesdortdeger[6] = highByte(vitesdorthafiza);
vitesdortdeger[7] = lowByte(vitesdorthafiza);
Serial.write(vitesdortdeger, 8);
EEPROM.write(5,vitesdorthafiza);
Serial.print("Vitesdort hafizaya alınan deger:");
Serial.println(vitesdorthafiza);
vitesdort = 0;
}
}
servoyazdir = 0;
break;
}
if(sensoryazdir >0)
{
if(devirsensor >0 && devirsensor < 255)
{
devirsensorhafiza = devirsensor;
devirsensordeger[6] = highByte(devirsensorhafiza);
devirsensordeger[7] = lowByte(devirsensorhafiza);
Serial.write(devirsensordeger, 8);
EEPROM.write(6,devirsensorhafiza);
Serial.print("Devirsensor hafizaya alınan deger:");
Serial.println(devirsensorhafiza);
devirsensor = 0;
}
if(hizsensor >0 && hizsensor < 255)
{
hizsensorhafiza = hizsensor;
hizsensordeger[6] = highByte(hizsensorhafiza);
hizsensordeger[7] = lowByte(hizsensorhafiza);
Serial.write(hizsensordeger, 8);
EEPROM.write(7,hizsensorhafiza);
Serial.print("hizsensor hafizaya alınan deger:");
Serial.println(hizsensorhafiza);
hizsensor = 0;
}
if(hidroliksensor >0 && hidroliksensor < 255)
{
hidroliksensorhafiza = hidroliksensor;
hidroliksensordeger[6] = highByte(hidroliksensorhafiza);
hidroliksensordeger[7] = lowByte(hidroliksensorhafiza);
Serial.write(hidroliksensordeger, 8);
EEPROM.write(8,hidroliksensorhafiza);
Serial.print("hidroliksensor hafizaya alınan deger:");
Serial.println(hidroliksensorhafiza);
hidroliksensor = 0;
}
sensoryazdir = 0;
}
if(otomatikyazdir >0)
{
if( vitesartir > 0 && vitesartir < 30)
{
vitesartirhafiza = vitesartir;
vitesartirdeger[6] = highByte(vitesartirhafiza);
vitesartirdeger[7] = lowByte(vitesartirhafiza);
Serial.write(vitesartirdeger, 8);
EEPROM.write(9,vitesartirhafiza);
Serial.print("vitesartir hafizaya alınan deger:");
Serial.println(vitesartirhafiza);
vitesartir = 0;
}
if( vitesazalt > 0 && vitesazalt < 30)
{
if(vitesazalt < vitesartirhafiza)
{
vitesazalthafiza = vitesazalt;
vitesazaltdeger[6] = highByte(vitesazalthafiza);
vitesazaltdeger[7] = lowByte(vitesazalthafiza);
Serial.write(vitesazaltdeger, 8);
EEPROM.write(10,vitesazalthafiza);
Serial.print("vitesazalt hafizaya alınan deger:");
Serial.println(vitesazalthafiza);
vitesazalt = 0;
}
}
otomatikyazdir = 0;
}
oldtime2 = millis();
}
}