DCC Controller for model railways - Bluetooth version

// v4 Jan 2025 CV1 write sequence extended
// v3 Aug 2 2023 Cs = Ccv +30 for read and write : was + 60 for write and +100 for read
// v2 30 Oct 2022 changed version to isolate text 'IBT' for App max current
// 11 Oct 2022

#include <AltSoftSerial.h>

AltSoftSerial bluetooth(8,9);  // RX TX

String Version = "ver: hcv ccv locomotive_dcc_L6203 IBT 2.5amp_v4";


int C;

float Cinst;
long bt_fail;
int inst_value;

int sensorValue;

String inString;

int preamable_type = 0;
int Address;
int MsbAddr;
int LsbAddr;
float CV_VAL;
float cv_val;
float cv_val0;
float cv_val1;
float cv_val2;
float cv_val3;
float cv_val4;
float cv_val5;
float cv_val6;
float cv_val7;
int test_num;
int cv_write_val;
int Cs;
int Ccv;
boolean cv_logic;
boolean ok;

boolean power_off =false;
boolean e_stop = false;

int num;
int a[8];
int ops_cv_num1;
int ops_cv_num2;
int ops_cv_val;

// use digital pins 6 and 5 for DCC out

//Timer frequency is 2MHz for ( /8 prescale from 16MHz )
#define TIMER_SHORT 0x8D  // 58usec pulse length 
#define TIMER_LONG  0x1B  // 116usec pulse length 

unsigned char last_timer=TIMER_SHORT;  // store last timer value
   
unsigned char flag=0;  // used for short or long pulse
unsigned char every_second_isr = 0;  // pulse up or down

// definitions for state machine 
#define PREAMBLE 0    
#define SEPERATOR 1
#define SENDBYTE  2

unsigned char state= PREAMBLE;
unsigned char preamble_count = 16;
unsigned char index = 0;  // **
unsigned char outbyte = 0;
unsigned char cbit = 0x80;

// variables

unsigned char  xdata = 0, data = 0, data_f = 0,data_f1 = 0,data_f2 = 0;

int locoAdr = 9;   // this is the default address of the loco
int data_reg = 0;
int page_reg = 1;

// buffer for command
struct Message {
   unsigned char data[7];
   unsigned char len;
} ;

#define MAXMSG 3
// for the time being, use only 3 messages - the idle msg, the loco Speed msg, function msg

struct Message msg[MAXMSG] = { 
    { { 0xFF,0xFF, 0xFF, 0, 0, 0, 0}, 3},   // idle msg
    {  { locoAdr, 0x3f, 0,  0x36, 0, 0, 0}, 6},   // locoMsg with 128 speed steps 0x3f
    { { 16, 0, 0, 0, 0, 0, 0}, 3}  // ** added this message to handle preamble 
                                   // and idle message changes as required
  };               // loco msg must be filled later with speed and XOR data byte
                                
int msgIndex=0;  
int byteIndex=0;

//Setup Timer2.
//Configures the 8-Bit Timer2 to generate an interrupt at the specified frequency.
//Returns the time load value which must be loaded into TCNT2 inside your ISR routine.

void SetupTimer2(){
  //Timer2 Settings: Timer Prescaler /8, mode 0
  //Timmer clock = 16MHz/8 = 2MHz period 0,5usec
  TCCR2A = 0;
  TCCR2B = 0<<CS22 | 1<<CS21 | 0<<CS20; 

  //Timer2 Overflow Interrupt Enable   
  TIMSK2 = 1<<TOIE2;

  //load the timer for its first cycle
  TCNT2=TIMER_SHORT; 
}
//Timer2 overflow interrupt vector handler
ISR(TIMER2_OVF_vect) {
  //Capture the current timer value TCTN2. This is how much error we have
  //due to interrupt latency and the work in this function
  //Reload the timer and correct for latency.  
  unsigned char latency;
  // for every second interupt just toggle signal
  if (every_second_isr)  {
    if (power_off == false) {PORTD = B01100000;PORTD = B00100000;// pin5 to IN1 pin6 to IN2 to avoid h-bridge shoot thro
    }
    if (power_off == true) {PORTD = B01100000; // for supply off
     }  
     every_second_isr = 0;        
     // set timer to last value
     latency=TCNT2;
     TCNT2=latency+last_timer;  
  }  else  {  // != every second interrupt, advance bit or state
     if (power_off == false) {PORTD = B01100000;PORTD = B01000000;// pin5 to IN1 pin6 to IN2 to avoid h-bridge shoot thro
     }
     if (power_off == true) {PORTD = B01100000; // for supply off
     }
     every_second_isr = 1; 
     switch(state)  {
       case PREAMBLE:
           flag=1; // short pulse
           preamble_count--;
           if (preamble_count == 0)  {  // advance to next state
              state = SEPERATOR;
              // get next message
              msgIndex++;
              if (msgIndex >= MAXMSG-1)  {  msgIndex = index;}  // **
              byteIndex = 0; //start msg with byte 0
           }
           break;
        case SEPERATOR:
           flag=0; // long pulse
           // then advance to next state
           state = SENDBYTE;
           // goto next byte ...
           cbit = 0x80;  // send this bit next time first         
           outbyte = msg[msgIndex].data[byteIndex];
           break;
        case SENDBYTE:
           if (outbyte & cbit)  { 
              flag = 1;  // send short pulse
           }  else  {
              flag = 0;  // send long pulse
           }
           cbit = cbit >> 1;
           if (cbit == 0)  {  // last bit sent, is there a next byte?
              byteIndex++;
              if (byteIndex >= msg[msgIndex].len)  {
                 // this was already the XOR byte then advance to preamble
                 state = PREAMBLE;
                 preamble_count = msg[2].data[0];  // **
                 index = msg[2].data[1];           // **
              }  else  {
                 // send separtor and advance to next byte
                 state = SEPERATOR ;
              }
           }
           break;
     }   
     if (flag)  {  // if data==1 then short pulse
        latency=TCNT2;
        TCNT2=latency+TIMER_SHORT;
        last_timer=TIMER_SHORT;
     }  else  {   // long pulse
        latency=TCNT2;
        TCNT2=latency+TIMER_LONG; 
        last_timer=TIMER_LONG;
     }  
  }
}

void setup(){
Serial.begin(115200);
analogReference(INTERNAL);
pinMode(13, OUTPUT);
digitalWrite(13, HIGH);
Ccv = 60;
bt_fail = 1;
e_stop = false;

 bluetooth.begin(9600);
 bluetooth.setTimeout(500);
    delay(500); // wait for bluetooth module to start
    Serial.println("Bluetooth Started");  
    bluetooth.println("Bluetooth Started");
    
bluetooth.println(Version);

DDRD = B01100000;   //  register D5 for digital pin 5, D6 for digital pin 6 

 //Start the timer 
  SetupTimer2();

power_off = true;  // power off
int get_cv_val; 
}

void current(){
int i;
  int value = 0;
  int numReadings = 5;
for (i = 0; i < numReadings; i++){
    // Read sensor data.
    value = value + analogRead(A0);
    // 1ms pause adds more stability between reads.
    delay(1);
  }

  sensorValue = value/numReadings;
  // Convert the analog reading (which goes from 0 - 1023) to a voltage (0 - 1.1V) = 1.08mv per division
  // 0.1 ohm resistor on current sense gives 200mv at 2 Amps or, 100mv per Amp
  // 1.08 mv per div for Internal Ref of 1.1v  : 100/1.08 = 92.6 divisions per 1000mA or 1 div = 10.8mA
  C = 10.8 * sensorValue ;  // mA
  
  if(C >2500){  // 2.5 amps
   Serial.println("Over Current");
   power_off = true;  // power off
   if(bluetooth.available() == true);{
   bluetooth.println("Short!");
   }
   delay(5);
   if(bluetooth.available() == true);{
   bluetooth.println("Short!");
   } 
  }
}

void loop() { 
      if( power_off || e_stop){
    Serial.println("Over Current here");
     if(bluetooth.available() == true);{
      inString = bluetooth.readStringUntil('\n');
        Serial.println(inString);
     }
      if (inString.substring(0,1) == "C"){
      digitalWrite(13, HIGH); 
      power_off = false;
      e_stop = false;
      bt_fail = 0;
      bluetooth.println(Version);}
            // reset after over current, logic control of DCC signal on,brake of
    return;}

       inst_value = analogRead(A0);
    Cinst = (10.8 * inst_value)  ;  // mA
    //Serial.println(Cinst);
    //Serial.print("current inst = ");Serial.println((int)Cinst);
       if(Cinst >2500){  // 2.5 amps 
   //Serial.println("Over Current");
   digitalWrite(13, LOW); // output off 
   power_off = true; 
    if(bluetooth.available() == true);{
   bluetooth.println("Short!");
   }
   delay(5);
   if(bluetooth.available() == true);{
   bluetooth.println("Short!");
   } 
  }
    
  if(bluetooth.available() == true);
    {
        inString = bluetooth.readStringUntil('\n');
        Serial.println(inString);
       
    }
    if (inString.substring(0,1) == "F"){
    bt_fail = 1;
    stringFF(); }
            // incoming DCC packet DCC_2

            
     if (inString.substring(0,1) == "T"){
    bt_fail = 1;
    stringTT(); }
            // incoming DCC packet DCC_2 ops mode      
                    
      if (inString.substring(0,1) == "D"){
    bt_fail = 1; 
    string(); }
            // incoming DCC packet DCC
                  
    if (inString.substring(0,1) == "C"){
      bt_fail = 1;
      power_off = false;  digitalWrite(13, HIGH); 
      bluetooth.println(Version);} //power on
            // reset after over current, logic control of DCC signal on,brake off

 if (inString.substring(0,1) == "S"){
        bt_fail = 0;
        get_Ccv();
        bluetooth.println(Version);
       }
        // adjust CV sensitivity
            
    if (inString.substring(0,1) == "G"){
        bt_fail = 1;
        current();
          bluetooth.print(C);
          bluetooth.println(" mA");
          Serial.print(C);
          bluetooth.println(" mA");
          }  
          // request for load current value 
          
      if (inString.substring(0,1) == "X"){
        bt_fail = 1;
          }  
          // keep alive for CV Set App
            
    if (inString.substring(0,1) == "A"){
      digitalWrite(13, HIGH);
       power_off = false;
       bt_fail = 0;
       get_cv_data();
       cv1_prog(); 
       digitalWrite(13, LOW);
       }  
       // set CV1 address
    if (inString.substring(0,3) == "get"){
      digitalWrite(13, HIGH);
       bt_fail = 0;
       power_off = false;
       cv_val = 0;
       get_cv_num();
       cv_read();
       digitalWrite(13, LOW);
       }  
       // CV read
       
    if (inString.substring(0,3) == "add"){
      digitalWrite(13, HIGH);
       bt_fail = 0;
       power_off = false;
       cv_write_val = 3;
       get_cv_new_val();
       repeat_cv_write(); 
       digitalWrite(13, LOW);
       }  
       // CV write
          
     
       
    if (inString.substring(0,1) == "V"){
     for (int i = 0; i<=10; i++){
      delay(20);
      bluetooth.println("CV1 updated"); // added 3x message
      };
      Serial.println(Address);}
            // cv1 write confirm
             
   if (inString.substring(0,1) == "E"){
       e_stop = true; // e stop
       emergency_stop(); 
       delay(20);
       return;
            }   
            // e-stop
            
        //Serial.print("bt_fail = " );Serial.println(bt_fail);    
      if (inString == "" && bt_fail >= 1){
        bt_fail = bt_fail + 1;
        if (bt_fail >= 5){
       power_off = true; // power off
       Serial.println("Lost BT connection");
            } 
      }     // bt fail safe 
           
    inString = "";

}

void get_Ccv(){
    Serial.println(inString);
    String temp ="";
    int x = inString.indexOf(",") + 1; 
    temp = inString.substring(x,8);
    Serial.println(temp);
    Ccv = temp.toInt();
    Serial.print("Ccv: ");Serial.println(Ccv); 
   }


void get_cv_new_val(){
    Serial.println(inString);
    String cv_w ="";
    int x = inString.indexOf("new,") + 4; 
    cv_w = inString.substring(x,x+3);
    Serial.println(cv_w);
    cv_write_val = cv_w.toInt();
    Serial.print("cv_write_val: ");Serial.println(cv_write_val); 

    String addr ="";
    int z = inString.indexOf("addr,") + 5; 
    addr = inString.substring(z,z+3);
    Serial.println(addr);
    Address = addr.toInt();
    Serial.print("Address: ");Serial.println(Address);
    Address = Address -1;
    calc_address(); 
   }
  
 void get_cv_data(){
    unsigned long z = inString.length();
    int y = 0;
    int count = 0;    
    String inChar;
    String temp ="";
   for (int i = 0; i<=z; i++){
    inChar = inString.substring(i,i+1);
    if (inChar == ",") {count++;}
    if (inChar != "," && inChar != "A") {temp += inChar;}
    if (inChar == ",") {Address = (temp.toInt());y = y +1;temp = "";}
       }
    amend_len3(msg[1]); 
    assemble_3_byteDD(); 
    Serial.println(inString);
   }


  void stringTT(){    // incoming DCC packet DCC_2 ops mode 
    unsigned long z = inString.length();
    int y = 0;
    for (int i = 0; i<=6; i++){
      a[i] = 0;
    }
int count = 0;    
String inChar;
String temp ="";
   for (int i = 0; i<=z; i++){
    inChar = inString.substring(i,i+1);
    if (inChar == ",") {
      count++;
    }
    if (inChar != "," && inChar != "T") {
      temp += inChar;
    }
    if (inChar == ",") {
      a[y] = (temp.toInt());
      y = y +1;
      temp = "";
    }
   }
    a[3] = a[3] -1;
    if (count == 5 && a[2] <=100){  
     Serial.println("tt");
     //print_data();
    ops_cv_num1 = a[3] % 256;
    ops_cv_num2 = int(a[3] / 256);
    ops_cv_val = a[4]; 
    amend_len5(msg[1]);
    assemble_5_byteTT(); 
    //print_data();
    } 

    if (count == 5 && a[2] >= 101){  
     Serial.println(inString);
     //print_data();
     a[5] = a[2] % 256;
     a[2] = 192 + int(a[2] / 256);
    ops_cv_num1 = a[3] % 256;
    ops_cv_num2 = int(a[3] / 256);
    ops_cv_val = a[4]; 
    amend_len6(msg[1]);
    assemble_6long_byteTT(); 
    //print_data();
    } 
    
   Serial.println(inString);
   Serial.println(a[1]);
   Serial.println(a[2]);
   Serial.println(a[3]);
   Serial.println(a[4]);
   Serial.println(a[5]);
   Serial.println(a[6]);
    }


 void stringFF(){      // incoming DCC packet DCC_2
    unsigned long z = inString.length();
    int y = 0;
    for (int i = 0; i<=6; i++){
      a[i] = 0;
    }
int count = 0;    
String inChar;
String temp ="";
   for (int i = 0; i<=z; i++){
    inChar = inString.substring(i,i+1);
    if (inChar == ",") {
      count++;
    }
    if (inChar != "," && inChar != "F") {
      temp += inChar;
    }
    if (inChar == ",") {
      a[y] = (temp.toInt());
      y = y +1;
      temp = "";
    }
   }
   
    //Serial.println(count);
    if (count == 3 ){  
      //Serial.println(inString);
      //print_data();
    amend_len3(msg[1]); 
    assemble_3_byteFF(); 
    }
    if (count == 4 && a[2] <=100){  
      //Serial.println(inString);
      //print_data();
    amend_len3(msg[1]); 
    assemble_3_byteFF(); 
    }
    if (count == 5 && a[2] <=100){  
     //Serial.println(inString);
     //print_data();
    amend_len4(msg[1]);
    assemble_4_byteFF(); 
    //print_data();
    } 

if (count == 4 && a[2] >100){ 
      //Serial.println(inString);
      //print_data();
    a[5] = a[2] % 256;
    a[2] = 192 + int(a[2] / 256);
    amend_len4(msg[1]); 
    assemble_4long_byteFF(); 
    }
    if (count == 5 && a[2] >100){  
     //Serial.println(inString);
     //print_data();
     a[5] = a[2] % 256;
     a[2] = 192 + int(a[2] / 256);
    amend_len5(msg[1]);
    assemble_5long_byteFF(); 
    //print_data();
    } 
    }

 void string(){
    unsigned long z = inString.length();
    int y = 0;
    for (int i = 0; i<=5; i++){
      a[i] = 0;
    }
int count = 0;    
String inChar;
String temp ="";
   for (int i = 0; i<=z; i++){
    inChar = inString.substring(i,i+1);
    if (inChar == ",") {
      count++;
    }
    if (inChar != "," && inChar != "D") {
      temp += inChar;
    }
    if (inChar == ",") {
      a[y] = (temp.toInt());
      y = y +1;
      temp = "";
    }
   }
    
    if (count == 3){  
      //Serial.println(inString);
      //print_data();
    amend_len3(msg[1]); 
    assemble_3_byteDD(); 
    }
    if (count == 4){  
     //Serial.println(inString);
     //print_data();
    amend_len4(msg[1]);
    assemble_4_byteDD(); 
    } 
    //Serial.println(inString);
    //Serial.println(a[1]);
    // Serial.println(a[2]);
    //Serial.println(a[3]);
    //Serial.println(a[4]);
    //print_data();  
    }
void assemble_4_byteDD() { 
   noInterrupts(); 
   msg[2].data[0] = 14; // 14 x '1's  // **
   msg[2].data[1] = 0;  // **
   msg[1].data[0] = a[1]; 
   msg[1].data[1] = a[2];
   msg[1].data[2] = a[3];
   msg[1].data[3] = ((a[1] ^ a[2])^ a[3]);
   interrupts();
}
void assemble_3_byteDD() { 
   noInterrupts(); 
   msg[2].data[0] = 14; // 14 x '1's  // **
   msg[2].data[1] = 0; // **
   msg[1].data[0] = a[1];
   msg[1].data[1] = a[2];
   msg[1].data[2] = (a[1] ^ a[2]);
   interrupts();
   
}


void emergency_stop(){
   amend_len3(msg[1]); 
   noInterrupts();  
   msg[2].data[0] = 14; // 14 x '1's
   msg[2].data[1] = 0;
   msg[1].data[0] = 0;
   msg[1].data[1] = B01000001;
   msg[1].data[2] = (msg[1].data[0] ^ msg[1].data[1]);  // e stop 
   interrupts(); 
   delay(5);
   power_off = true;
}

     
 void amend_len4 (struct Message & x) 
{ 
 x.len = 4;
   //Serial.println(x.len);
}

void amend_len5 (struct Message & x) 
{ 
 x.len = 5;
   //Serial.println(x.len);
}

void amend_len6 (struct Message & x) 
{ 
 x.len = 6;
   //Serial.println(x.len);
}

void assemble_4_byteFF() { 
   noInterrupts(); 
   msg[2].data[0] = 14; // 14 x '1's  // **
   msg[2].data[1] = 0;  // **
   msg[1].data[0] = a[2]; 
   msg[1].data[1] = a[3];
   msg[1].data[2] = a[4];
   msg[1].data[3] = ((a[2] ^ a[3])^ a[4]);
   interrupts();
}

void amend_len3 (struct Message & x) 
{ 
 x.len = 3;
  //Serial.println(x.len);
}


void assemble_3_byteFF() { 
   noInterrupts(); 
   msg[2].data[0] = 14; // 14 x '1's  // **
   msg[2].data[1] = 0; // **
   msg[1].data[0] = a[2];
   msg[1].data[1] = a[3];
   msg[1].data[2] = (a[2] ^ a[3]);
   interrupts();
   
}

void assemble_5_byteTT() { 
  Serial.print("ops_cv_num1 ");Serial.println(ops_cv_num1);
  Serial.print("ops_cv_num2 ");Serial.println(ops_cv_num2);
  Serial.print("loco num ");Serial.println(a[2]);
  Serial.print("consist addr ");Serial.println(a[4]);
   noInterrupts(); 
   msg[2].data[0] = 14; // 14 x '1's  // **
   msg[2].data[1] = 0;  // **
   msg[1].data[0] = a[2]; // loco short address
   msg[1].data[1] = B11101100 | ops_cv_num2; //  write to VV (msb of CV number) 1110CCVV
   msg[1].data[2] = ops_cv_num1; // VVVVVVVV  LSB of CV number
   msg[1].data[3] = a[4]; // DDDDDDDD  value to write
   msg[1].data[4] = msg[1].data[0] ^ msg[1].data[1]^ msg[1].data[2] ^ msg[1].data[3];
   interrupts();
   
}

void assemble_6long_byteTT() { 
   noInterrupts(); 
   msg[2].data[0] = 14; // 14 x '1's  // **
   msg[2].data[1] = 0;  // **
   msg[1].data[0] = a[2]; // loco long address
   msg[1].data[1] = a[5]; // loco long address
   msg[1].data[2] = B11101100 | ops_cv_num2; //  write to VV (msb of CV number) 1110CCVV
   msg[1].data[3] = ops_cv_num1; // VVVVVVVV  LSB of CV number
   msg[1].data[4] = a[4]; // DDDDDDDD  value to write
   msg[1].data[5] = ((((msg[1].data[0] ^ msg[1].data[1])^ msg[1].data[2]) ^ msg[1].data[3]) ^ msg[1].data[4]);
   interrupts();
   
}

void assemble_5long_byteFF() { 
   noInterrupts(); 
   msg[2].data[0] = 14; // 14 x '1's  // **
   msg[2].data[1] = 0;  // **
   msg[1].data[0] = a[2]; 
   msg[1].data[1] = a[5];
   msg[1].data[2] = a[3];
   msg[1].data[3] = a[4];
   msg[1].data[4] = ((a[2] ^ a[5])^ a[3] ^ a[4]);
   interrupts();
   
}

void assemble_4long_byteFF() { 
   noInterrupts(); 
   msg[2].data[0] = 14; // 14 x '1's  // **
   msg[2].data[1] = 0;  // **
   msg[1].data[0] = a[2]; 
   msg[1].data[1] = a[5];
   msg[1].data[2] = a[3];
   msg[1].data[3] = ((a[2] ^ a[5])^ a[3]);
   interrupts();
   
}


void print_data(){
 Serial.print(msg[1].data[0], DEC);
 Serial.print(",");
 Serial.print(msg[1].data[1], DEC);
 Serial.print(",");
 Serial.print(msg[1].data[2], DEC);
 Serial.print(",");
 Serial.print(msg[1].data[3], DEC);
 Serial.println(",");
  }

//CV read

void cv_current(){
for (int i = 1 ; i<=10 ; i++){
    sensorValue = analogRead(A0);
    C = 10.8 * sensorValue ;  // mA
    if (C >= Cs){
      cv_logic = true;
      i = 11;
    }
    delayMicroseconds(500);
    Serial.print("C = ");Serial.println(C);
    if (C >= 500){
         if(bluetooth.available() == true);{
         bluetooth.println("Short!..");
  }
    }
}
}


void cv1_prog(){
 reset();
for (int i = 0 ; i<=8 ; i++){
  page_preset_packet();
  delay(5);
 }

 reset(); 
for (int i = 0 ; i<=6 ; i++){
  cv1_write_packet();
  delay(5);
 } 
 reset();
}


void reset_packet() {
   amend_len3(msg[1]);
   noInterrupts();
   msg[2].data[0] = 24; // 14 x '1's // **
   msg[2].data[1] = 1; // **
   msg[1].data[0] = B00000000;
   msg[1].data[1] = B00000000;
   msg[1].data[2] = B00000000; 
   interrupts();
}
void page_preset_packet() {
   amend_len3(msg[1]);
   noInterrupts();
   msg[2].data[0] = 24; // 24 x '1's // **
   msg[2].data[1] = 1; // **
   msg[1].data[0] = B01111101;
   msg[1].data[1] = B00000001;
   msg[1].data[2] = B01111100; 
   msg[1].data[3] = B00000000;
   interrupts();
}
void cv1_write_packet() { 
   amend_len3(msg[1]);
   noInterrupts();
   msg[2].data[0] = 24; // 24 x '1's // **
   msg[2].data[1] = 1; // **
   msg[1].data[0] = B01111000; // Address only mode
   msg[1].data[1] = B00000000 | Address;
   msg[1].data[2] = (msg[1].data[0] ^ msg[1].data[1]);
   interrupts();
}

void cv_verify1_packet() { 
   amend_len4(msg[1]);
   noInterrupts();
   msg[2].data[0] = 24; // 24 x '1's // **
   msg[2].data[1] = 1; // **
   msg[1].data[0] = B01111000 | MsbAddr ; // bit manipulation mode  + 2 most sig bits of Address
   msg[1].data[1] = B00000000 | LsbAddr;
   msg[1].data[2] = B11101000 | num;
   msg[1].data[3] = (msg[1].data[0] ^ msg[1].data[1]) ^ msg[1].data[2];
   interrupts(); 
}

void cv_verify0_packet() { 
   amend_len4(msg[1]);
   noInterrupts();
   msg[2].data[0] = 24; // 24 x '1's // **
   msg[2].data[1] = 1; // **
   msg[1].data[0] = B01111000 | MsbAddr ; // bit manipulation mode + 2 most sig bits of Address 
   msg[1].data[1] = B00000000 | LsbAddr; 
   msg[1].data[2] = B11100000 | num;
   msg[1].data[3] = (msg[1].data[0] ^ msg[1].data[1]) ^ msg[1].data[2];
   interrupts(); 
}

void cv_write_packet() { 
   amend_len4(msg[1]);
   noInterrupts();
   msg[2].data[0] = 24; // 24 x '1's // **
   msg[2].data[1] = 1; // **
   msg[1].data[0] = B01111100 | MsbAddr ; // write mode + 2 most sig bits of Address
   msg[1].data[1] = B00000000 | LsbAddr; 
   msg[1].data[2] = B00000000 | cv_write_val;
   msg[1].data[3] = (msg[1].data[0] ^ msg[1].data[1]) ^ msg[1].data[2];
   interrupts(); 
}

void valid_packet() { 
   amend_len4(msg[1]);
   noInterrupts();
   msg[2].data[0] = 14; // 16 x '1's // **
   msg[2].data[1] = 0;  // **
   msg[1].data[0] = B00000011; 
   msg[1].data[1] = B00111111;
   msg[1].data[2] = B00000001;
   msg[1].data[3] = (msg[1].data[0] ^ msg[1].data[1]) ^ msg[1].data[2];
   interrupts();
}

void idle(){
   amend_len3(msg[1]); 
   noInterrupts(); 
   msg[2].data[0] = 14; // 16 x '1's // **
   msg[2].data[1] = 0; // **
   msg[1].data[0] = B11111111; 
   msg[1].data[1] = B00000000;
   msg[1].data[2] = B11111111;
   interrupts();
}
//CV write
void repeat_cv_write() {
  current();
  Cs = Ccv + 30;  // Cs = C + Ccv -> Cs = Ccv
  Serial.print("C = ");Serial.println(C);
  Serial.print("Cs = ");Serial.println(Cs);
  
  ok = false;
  for (int f = 1 ; f<=10 ; f++){
  if (ok == false){
    cv_write();
    delay(10);
  }
  } 
}

void reset(){
   for (int j = 1 ; j<=12 ; j++){
  reset_packet();
  delay(5);
 }
}

void cv_write(){
   delay(100);
for (int i = 1 ; i<=20 ; i++){
  valid_packet();
  delay(5);
   }
   
reset();
   cv_logic = false;
for (int i = 1 ; i<=10 ; i++){
  digitalWrite(13, HIGH); // output on
  cv_write_packet();
  cv_current();
  Serial.print("C= ");Serial.println(C);
  if (cv_logic){
    ok = true;
    cv_logic = false;
    i = 11;
    delay(50);   // *
    power_off = false;  // *
    digitalWrite(13, HIGH); // * 
   reset();
 if(bluetooth.available() == true);{  
            bluetooth.print("write = ");
            bluetooth.println(cv_write_val);
            Serial.print("cv_write_val ");Serial.println(cv_write_val);
         }
  }
  }
 if(bluetooth.available() == true);{
         if (ok == false){
            bluetooth.println("write = error");  
            }
   }

}

void calc_address(){
  MsbAddr = 0;
     LsbAddr = Address;
    if (Address >= 255 && Address <= 511){
      MsbAddr = 1;
      LsbAddr = Address - 256;
    }
    if (Address >= 512 && Address <= 767){
      MsbAddr = 2;
      LsbAddr = Address - 512;
    }
     if (Address >= 768 && Address <= 1023){
      MsbAddr = 3;
      LsbAddr = Address - 768;
    }
    Serial.print("Address: ");Serial.println(Address);
    Serial.print("LsbAddr: ");Serial.println(LsbAddr);
    Serial.print("MsbAddr: ");Serial.println(MsbAddr);
}



void get_cv_num(){
    cv_write_val = 0;
    Serial.println(inString);
    String temp ="";
    int x = inString.indexOf(",") + 1; 
    temp = inString.substring(x,8);
    Serial.println(temp);
    Address = temp.toInt();
    Serial.print("Address: ");Serial.println(Address); 
    Address = Address -1;
    calc_address(); 
   }
//
//
//
//
//

void cv_read(){
  cv_val = 0;
  delay(200);
  current();

   delay(100);     // **1
for (int i = 1 ; i<=20 ; i++){
  valid_packet();
  delay(5);
   }
   reset();           
  
   Cs = Ccv + 30;  // Cs = C + Ccv -> Cs = Ccv
  Serial.print("Cs = ");Serial.println(Cs);
  CV_VAL = 99;
  for (int f = 1 ; f<=5 ; f++){
...

This file has been truncated, please download it to see its full contents.