Revision 1:0cb065f9d55a, committed 2020-05-18

Comitter:

Picmon

Date:

Mon May 18 19:04:41 2020 +0000

Parent:

0:333434a8611b

Commit message:

ring buffer26;

Changed in this revision

diff -r 333434a8611b -r 0cb065f9d55a MotorControl.cpp
--- a/MotorControl.cpp	Fri Mar 20 10:54:58 2020 +0000
+++ b/MotorControl.cpp	Mon May 18 19:04:41 2020 +0000
@@ -1,149 +1,465 @@
-#include "mbed.h"
-#include "MotorControl.h"
+/*
+
+EOL SEQUENCE = NONE ON YAT, PROTOCOL WILL FAIL IF THIS IS NOT DONE
 
-RawSerial mc_usart(PA_2,PA_15,115200);// this will be USART6 for motor control 
+*/
+
+
+
 
-const string rxMsgTable[MESSAGE] = {
+#include <stdio.h>
+#include "mbed.h"
+#include "main.h"
+#include "MotorControl.h"
+#include<string>
 
-    "SET DRIVE WORKING SETINGS",
-    "SET FEEDBACK BOOST CURRENT",    
+class motorControl{
+    
+    private:
+    
+    public:
+    
+    protected:
     
 };
 
-struct drvWorkingSettings{
- 
- bool par0;
- bool par1;
- bool par2;
- bool par3;
- bool feedBackMotChk;
- bool par5;
- bool auomaticMotCurRed;
- bool disableDigOutputsFWH;
- bool par8; 
- bool par9;
- bool motRotDir;
- bool par11;
- bool par12; 
- bool fbErrMotAct;
- bool par14;
- bool par15;  
-  
-};
+char *ptr;
+
+volatile uint8_t count = 0;
+
+bool msgValid = false;
+bool msgRdy = false;
+
+uint8_t i=0;
+
+DigitalIn d3_button(D3_BUTTON);
+DigitalIn d4_button(D4_BUTTON);
+DigitalIn d5_button(D5_BUTTON);
+DigitalIn d6_button(D6_BUTTON);
+DigitalIn d7_button(D7_BUTTON);
+DigitalIn d8_button(D8_BUTTON);
+DigitalIn d9_button(D9_BUTTON);
+DigitalIn d10_button(D10_BUTTON);
+DigitalIn d11_button(D11_BUTTON);
+DigitalIn d12_button(D12_BUTTON);
+
+DigitalOut a7_out1(A7_OUT1);//mot enable
+DigitalOut a6_out2(A6_OUT2);
+DigitalOut a5_out3(A5_OUT3);
 
-char rxMsgStore[BUFFER_SIZE];//received local message store
+DigitalIn a4_in1(A4_IN1);
+DigitalIn a3_in2(A3_IN2);
+DigitalIn a2_in3(A2_IN3);
+
+DigitalOut a1_out(A1_OUT);
+DigitalOut a0_out(A0_OUT);
+
+
+char rxMsgStore[BUFFER_SIZE];//received message store
 char mc_Tx_Buffer[BUFFER_SIZE];//transmitted message buffer 
-volatile char mc_Rx_Buffer[BUFFER_SIZE];//received message buffer
-volatile unsigned char mc_Rx_Rd_Ptr = 0;//circulare buffer read pointer
-volatile unsigned char mc_Rx_Wr_Ptr = 0;//circulare buffer write pointer
-unsigned int rx_int = 0;//received integer / character
+char mc_Rx_Buffer[BUFFER_SIZE];//received message buffer
+volatile unsigned char mc_Rx_Rd_Ptr = 0;//circular buffer read pointer
+volatile unsigned char mc_Rx_Wr_Ptr = 0;//circular buffer write pointer
+volatile bool rxMsgValid = false;//message received flag 
+volatile bool rxMsgInvalid = false;//message invalid flag
 
-//unsigned char i; 
-bool msgRecFlag = false;//message received flag 
+char motData[DATA_ITEMS][DATA_BUFFER_SIZE];//data only
+
+////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+// MOTOR ISR FUNCTIONS
+////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
 
 void motMsg_RX_ISR(void){
     
-    //led1=1;
-    //save character in buffer
-    mc_Rx_Buffer[mc_Rx_Wr_Ptr] = mc_usart.getc();
-    //increment pointer
-    mc_Rx_Wr_Ptr++;
-    //test for wrap around
-    if(mc_Rx_Wr_Ptr == (BUFFER_SIZE - 1))
-    {
-        mc_Rx_Wr_Ptr = 0;
-    }
-    //wait(0.0001);
-    //led1=0; 
-}
-  
-bool getMotMsg(char * msgArray){
-    
-    //declare variables
-    static unsigned char i;
-    unsigned int uiMsg= 0xFFFF;
+    mc_Rx_Buffer[mc_Rx_Wr_Ptr] = mc_debug.getc();
 
-    //test for any characters in the receiver buffer
-    if (mc_Rx_Wr_Ptr != mc_Rx_Rd_Ptr)
-    {
-        //character in the buffer, return the value
-        uiMsg = mc_Rx_Buffer[mc_Rx_Rd_Ptr];
-        //increment the Rd pointer
-        mc_Rx_Rd_Ptr++;
-        //test for wrap around
-        if(mc_Rx_Rd_Ptr == (BUFFER_SIZE - 1))
-        {
-            mc_Rx_Rd_Ptr = 0;
-        }
-    }
-                
-    if(uiMsg != 0xFFFF){//check that a character is in the buffer        
-        msgArray[i++] = uiMsg;//store characters in local message store array
+    mc_Rx_Wr_Ptr++;
+    count++;
+
+    if(mc_Rx_Wr_Ptr == (BUFFER_SIZE - 1))
+        mc_Rx_Wr_Ptr = 0;
         
-        mc_usart.printf("%c\r\n", uiMsg); 
-               
-        if(uiMsg == '\r'){//look for <CR> then print out the message in the RX buffer
-            
-            msgRecFlag = true;//set the message received flag
-            
-            mc_usart.printf("Stored Message\r\n");
-                            
-            for(i=0 ; i <BUFFER_SIZE ; i++)                    
-                mc_usart.printf("%c\r\n",msgArray[i]);
-                
-        }
-    }
-    else
-        i=0;    
-            
-             
-    return(msgRecFlag);
-} 
-
-void motMsgDecode(unsigned int rxMsgTable[], unsigned int rxMsg[]){
-    
-    if(memcmp(rxMsgTable, rxMsg, 10)){
-        
-    }
-    
 }
 
-void clrRxMsg(char array[]){
+////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+// MOTOR READ FUNCTIONS
+////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+
+uint8_t getMotMsg(char *txArray, char *rxArray){
+////////////////////////////////////////////////////////////////////////////////
+// get the motor message
+//
+// funtion receives command messages eched from the motor controller, the 
+// message eched is checked and verified, if correct the message is parsed
+// to expose the numerical data, this data is stored in the relevant memory 
+// location for later use  
+// 
+//
+////////////////////////////////////////////////////////////////////////////////       
+    uint8_t msgStatus = NULL;
+    static uint8_t msgReTry = 0;
+      
+    while(mc_Rx_Wr_Ptr != mc_Rx_Rd_Ptr){// if not true we have a message to collect
+        
+        rxArray[i] = mc_Rx_Buffer[mc_Rx_Rd_Ptr];//write the charcter into the array element indexed by i
 
-    for(unsigned char i=0; i<BUFFER_SIZE; i++)
-        array[i]=0;
+        if(rxArray[i] == ACK)//read the character from the array elecment indexed by i and if it is <ACK> set message valid flag (msgValid) 
+            msgValid = true;
+        
+        #ifdef DEBUG    
+            //mc_debug.printf("%c", rxArray[i]);//print out chahacters from array indexed by i
+            //mc_usart.printf("%c", rxArray[i]);//print out chahacters from array indexed by i            
+        #endif    
+        
+        i++;//increment the array index
+        
+        mc_Rx_Rd_Ptr++;//increment the read pointer
+        
+        count--;
+        
+        if(mc_Rx_Rd_Ptr == (BUFFER_SIZE - 1))
+            mc_Rx_Rd_Ptr = 0;         
+    }
+        
+    if((mc_Rx_Wr_Ptr == mc_Rx_Rd_Ptr)&&(msgValid == true)){//full message received
+ 
+         msgValid = false;
+         
+         if(strncmp(txArray, rxArray, strlen(txArray))==0){
+            #ifdef DEBUG  
+                //mc_debug.printf("\r\nEcho compare is Successful, txArray = %s, rxArray = %s\r\n", txArray, rxArray);     
+                mc_usart.printf("\r\nEcho compare is Successful, txArray = %s, rxArray = %s\r\n", txArray, rxArray);                                 
+            #endif   
+            
+            msgStatus = RX_ECHO_VALID; 
+            msgReTry=0; 
+            
+             //READ COMMAND TYPE 1 : <COMMAND><CR><VALUE><ACK>
+             if(strncmp(rxArray,"READ DIGITAL INPUTS\r", strlen(txArray))==0){  
+                led1=1;  
+                //msgStatus = RX_ECHO_VALID;                
+                //msgReTry=0;                
+                data2mem(READ_DIG_IN, motData, rxArray); 
+                #ifdef DEBUG                  
+                    //mc_debug.printf("\r\nArray Contents %s\r\n\r\n", motData[READ_DIG_IN]);  
+                    mc_usart.printf("\r\nArray Contents %s\r\n\r\n", motData[READ_DIG_IN]);  
+                #endif                        
+            }  
+            else   
+            if(strncmp(rxArray,"READ DIGITAL OUTPUTS\r", strlen(txArray))==0){    
+                led1=1;  
+                //msgStatus = RX_ECHO_VALID;                
+                //msgReTry=0;                
+                data2mem(READ_DIG_OUT, motData, rxArray); 
+                #ifdef DEBUG                 
+                    //mc_debug.printf("Array Contents %s\r\n\r\n", motData[READ_DIG_OUT]);  
+                    mc_usart.printf("Array Contents %s\r\n\r\n", motData[READ_DIG_OUT]);                      
+                #endif                        
+            }  
+            else 
+            if(strncmp(rxArray,"READ CURRENT ACTUAL\r", strlen(txArray))==0){         
+                led1=1;  
+                //msgStatus = RX_ECHO_VALID;                
+                //msgReTry=0;                 
+                data2mem(READ_CURRENT, motData, rxArray); 
+                #ifdef DEBUG  
+                    //mc_debug.printf("Array Contents %s\r\n\r\n", motData[READ_CURRENT]);  
+                    mc_usart.printf("Array Contents %s\r\n\r\n", motData[READ_CURRENT]);                     
+                #endif                        
+            }  
+            else
+            if(strncmp(rxArray,"READ DRIVE REGISTER\r", strlen(txArray))==0){     
+                led1=1;  
+                //msgStatus = RX_ECHO_VALID;                
+                //msgReTry=0;                 
+                data2mem(READ_DRV_REG, motData, rxArray); 
+                #ifdef DEBUG                   
+                    //mc_debug.printf("Array Contents %s\r\n\r\n", motData[READ_DRV_REG]);  
+                    mc_usart.printf("Array Contents %s\r\n\r\n", motData[READ_DRV_REG]);                     
+                #endif                        
+            }   
+            else
+            if(strncmp(rxArray,"READ DRIVE REGISTER EXTENDED\r", strlen(txArray))==0){ 
+                led1=1;
+                //msgStatus = RX_ECHO_VALID;                
+                //msgReTry=0;                 
+                data2mem(READ_DRV_REG_EXT, motData, rxArray); 
+                #ifdef DEBUG                 
+                    //mc_debug.printf("Array Contents %s\r\n\r\n", motData[READ_DRV_REG_EXT]);  
+                    mc_usart.printf("Array Contents %s\r\n\r\n", motData[READ_DRV_REG_EXT]);                      
+                #endif                        
+            }    
+            else  
+            if(strncmp(rxArray,"READ DRIVE TEMPERATURE\r", strlen(txArray))==0){  
+                led1=1; 
+                //msgStatus = RX_ECHO_VALID;                
+                //msgReTry=0;                 
+                data2mem(READ_DRV_TEMP, motData, rxArray); 
+                #ifdef DEBUG                  
+                    //mc_debug.printf("Array Contents %s\r\n\r\n", motData[READ_DRV_TEMP]); 
+                    mc_usart.printf("Array Contents %s\r\n\r\n", motData[READ_DRV_TEMP]);                      
+                #endif                        
+            }  
+            else
+            if(strncmp(rxArray,"READ DRIVE VOLTAGE\r", strlen(txArray))==0){ 
+                led1=1;  
+                //msgStatus = RX_ECHO_VALID;                
+                //msgReTry=0;                   
+                data2mem(READ_DRV_VOLTAGE, motData, rxArray); 
+                #ifdef DEBUG                   
+                    //mc_debug.printf("Array Contents %s\r\n\r\n", motData[READ_DRV_VOLTAGE]);  
+                    mc_usart.printf("Array Contents %s\r\n\r\n", motData[READ_DRV_VOLTAGE]);                      
+                #endif                        
+            }      
+            else
+            if(strncmp(rxArray,"READ DRIVE WORKING SETTINGS\r", strlen(txArray))==0){
+                led1=1;  
+                //msgStatus = RX_ECHO_VALID;                
+                //msgReTry=0;                 
+                data2mem(READ_DRV_WORKING_SET, motData, rxArray); 
+                #ifdef DEBUG              
+                    //mc_debug.printf("Array Contents %s\r\n\r\n", motData[READ_DRV_WORKING_SET]);  
+                    mc_usart.printf("Array Contents %s\r\n\r\n", motData[READ_DRV_WORKING_SET]);                      
+                #endif                        
+            } 
+            else
+            if(strncmp(rxArray,"READ DRIVE WORKING SETTINGS EXTENDED\r", strlen(txArray))==0){        
+                led1=1;  
+                //msgStatus = RX_ECHO_VALID;                
+                //msgReTry=0;                 
+                data2mem(READ_DRV_WORKING_SET_EXT, motData, rxArray); 
+                #ifdef DEBUG                    
+                    //mc_debug.printf("Array Contents %s\r\n\r\n", motData[READ_DRV_WORKING_SET_EXT]);  
+                    mc_usart.printf("Array Contents %s\r\n\r\n", motData[READ_DRV_WORKING_SET_EXT]);                    
+                #endif                        
+            }      
+            else
+            if(strncmp(rxArray,"READ ERROR REGISTER\r", strlen(txArray))==0){    
+                led1=1;
+                //msgStatus = RX_ECHO_VALID;                
+                //msgReTry=0;                 
+                data2mem(READ_ERROR_REG, motData, rxArray); 
+                #ifdef DEBUG                    
+                    //mc_debug.printf("Array Contents %s\r\n\r\n", motData[READ_ERROR_REG]);  
+                    mc_usart.printf("Array Contents %s\r\n\r\n", motData[READ_ERROR_REG]);                     
+                #endif                        
+            }         
+            else
+            if(strncmp(rxArray,"READ FEEDBACK BOOST CURRENT\r", strlen(txArray))==0){      
+                led1=1;
+                //msgStatus = RX_ECHO_VALID;                
+                //msgReTry=0;                 
+                data2mem(READ_FB_BOOST_CUR, motData, rxArray); 
+                #ifdef DEBUG          
+                    //mc_debug.printf("Array Contents %s\r\n\r\n", motData[READ_FB_BOOST_CUR]);  
+                    mc_usart.printf("Array Contents %s\r\n\r\n", motData[READ_FB_BOOST_CUR]);                     
+                #endif                        
+            }               
+            else
+            if(strncmp(rxArray,"READ FEEDBACK STATUS\r", strlen(txArray))==0){  
+                led1=1;  
+                //msgStatus = RX_ECHO_VALID;                
+                //msgReTry=0;                 
+                data2mem(READ_FB_STATUS, motData, rxArray); 
+                #ifdef DEBUG                   
+                    //mc_debug.printf("Array Contents %s\r\n\r\n", motData[READ_FB_STATUS]);  
+                    mc_usart.printf("Array Contents %s\r\n\r\n", motData[READ_FB_STATUS]);                    
+                #endif                        
+            }              
+            else    
+            if(strncmp(rxArray,"READ FIRMWARE VERSION\r", strlen(txArray))==0){   
+                led1=1; 
+                //msgStatus = RX_ECHO_VALID;                
+                //msgReTry=0;                
+                data2mem(READ_FW_VERSION, motData, rxArray);
+                #ifdef DEBUG                 
+                    //mc_debug.printf("Array Contents %s\r\n\r\n", motData[READ_FW_VERSION]); 
+                    mc_usart.printf("Array Contents %s\r\n\r\n", motData[READ_FW_VERSION]);                     
+                #endif                        
+            }   
+            else                            
+            if(strncmp(rxArray,"READ FIRMWARE CHECKSUM\r", strlen(txArray))==0){    
+                led1=1;  
+                //msgStatus = RX_ECHO_VALID;                
+                //msgReTry=0;                
+                data2mem(READ_FW_CHECKSUM, motData, rxArray); 
+                #ifdef DEBUG                  
+                    //mc_debug.printf("Array Contents %s\r\n\r\n", motData[READ_FW_CHECKSUM]);  
+                    mc_usart.printf("Array Contents %s\r\n\r\n", motData[READ_FW_CHECKSUM]);                     
+                #endif                        
+            }                
+            else
+            if(strncmp(rxArray,"READ MASTER REGISTER\r", strlen(txArray))==0){    
+                led1=1;  
+                //msgStatus = RX_ECHO_VALID;                
+                //msgReTry=0;                
+                data2mem(READ_MASTER_REG, motData, rxArray); 
+                #ifdef DEBUG                    
+                    //mc_debug.printf("Array Contents %s\r\n\r\n", motData[READ_MASTER_REG]);  
+                    mc_usart.printf("Array Contents %s\r\n\r\n", motData[READ_MASTER_REG]);                    
+                #endif                        
+            }   
+            else
+            if(strncmp(rxArray,"READ MIN CURRENT\r", strlen(txArray))==0){
+                led1=1;  
+                //msgStatus = RX_ECHO_VALID;                
+                //msgReTry=0;                
+                data2mem(READ_MIN_CURRENT, motData, rxArray); 
+                #ifdef DEBUG                 
+                    //mc_debug.printf("Array Contents %s\r\n\r\n", motData[READ_MIN_CURRENT]);  
+                    mc_usart.printf("Array Contents %s\r\n\r\n", motData[READ_MIN_CURRENT]);                    
+                #endif                        
+            }       
+            else
+            if(strncmp(rxArray,"READ MAX CURRENT\r", strlen(txArray))==0){    
+                led1=1;  
+                //msgStatus = RX_ECHO_VALID;                
+                //msgReTry=0;                
+                data2mem(READ_MAX_CURRENT, motData, rxArray); 
+                #ifdef DEBUG                  
+                    //mc_debug.printf("Array Contents %s\r\n\r\n", motData[READ_MAX_CURRENT]);  
+                    mc_usart.printf("Array Contents %s\r\n\r\n", motData[READ_MAX_CURRENT]);                     
+                #endif                        
+            }   
+            else
+            if(strncmp(rxArray,"READ BOOST CURRENT\r", strlen(txArray))==0){      
+                led1=1;  
+                //msgStatus = RX_ECHO_VALID;                
+                //msgReTry=0;                
+                data2mem(READ_BOOST_CURRENT, motData, rxArray); 
+                #ifdef DEBUG                   
+                    //mc_debug.printf("Array Contents %s\r\n\r\n", motData[READ_BOOST_CURRENT]); 
+                    mc_usart.printf("Array Contents %s\r\n\r\n", motData[READ_BOOST_CURRENT]);                     
+                #endif                        
+            }         
+            else
+            if(strncmp(rxArray,"READ NOMINAL CURRENT\r", strlen(txArray))==0){      
+                led1=1;  
+                //msgStatus = RX_ECHO_VALID;                
+                //msgReTry=0;                
+                data2mem(READ_NOM_CURRENT, motData, rxArray); 
+                #ifdef DEBUG                   
+                    //mc_debug.printf("Array Contents %s\r\n\r\n", motData[READ_NOM_CURRENT]);  
+                    mc_usart.printf("Array Contents %s\r\n\r\n", motData[READ_NOM_CURRENT]);                    
+                #endif                        
+            }    
+         }   
+         else{
     
-    #ifdef DEBUG    
-        mc_usart.printf("Clear the RX local message store\r\n"); 
-    #endif
+            msgStatus = RX_ECHO_FAIL;             
+
+            if(msgReTry < MSG_RETRY_COUNT){//limit message transmit re-tries
+                mc_debug.printf("%s",txArray);//transmit the last message again 
+                msgReTry++;//increment the re-try counter
                 
-    for(unsigned char i=0; i<BUFFER_SIZE; i++){
-    
-        #ifdef DEBUG
-            mc_usart.printf("%c\r\n",array[i]);  
-        #endif
+                #ifdef DEBUG 
+                    //mc_usart.printf("\r\nEcho compare FAILED, txArray = %s, rxArray = %s\r\n", txArray, rxArray); 
+                    mc_usart.printf("\r\nTransmit re-try = %d, txArray = %s, rxArray = %s, txArray Length = %d\r\n", msgReTry , txArray, rxArray, strlen(txArray));                 
+                #endif  
+            }
+            else{
+                #ifdef DEBUG  
+                    //mc_debug.printf("\r\nEcho compare has FAILED, txArray = %s, rxArray = %s, txArray Length = %d\r\n", txArray, rxArray, strlen(txArray));   
+                    mc_usart.printf("\r\nEcho compare has FAILED, re-try = %d, txArray = %s, rxArray = %s, txArray Length = %d\r\n",  msgReTry, txArray, rxArray, strlen(txArray));   
+                #endif    
+                msgStatus = REBOOT;   
+                msgReTry=0;                     
+            }   
+        }
+   
+        i=0;//reset the array indexer for next message                        
+        memset(rxArray, NULL, BUFFER_SIZE);//clear the entire buffer for next message
     }
+    return(msgStatus);
 } 
 
 
-bool moveMot(uint8_t par1, int32_t par2){
+////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+// mc_debug FUNCTIONS
+////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+
+
+void data2mem(uint8_t dataLoc, char destMemArray[][DATA_BUFFER_SIZE], char sourceMemArray[BUFFER_SIZE]){  
+////////////////////////////////////////////////////////////////////////////////
+// data to memory for command type 1
+//
+// 
+//
+////////////////////////////////////////////////////////////////////////////////  
+
+    //parse the string to isolate the VALUE
+    //<COMMAND><CR><VALUE><ACK>
+    
+    ptr = strrchr(sourceMemArray, ACK);//first set a pointer the last <ACK> address in the string,<ACK> is not required and we want this location for null terminator 
     
-    static uint8_t newPar1 = 0;
-    static int32_t newPar2 = 0;    
-    bool fault = false;
+    *ptr = '\0';//overwrite <ACK> character with null terminator character, this will serve as end of string terminataor now            
+    
+    ptr = strchr(sourceMemArray, '\r');//now set the pointer first <CR> address in the string
+            
+    *ptr++;//point to first character of VALUE (MSB) in the string, <CR> + 1 = <VALUE>
+    
+    strcpy(destMemArray[dataLoc], ptr);//copy the <VALUE> to the destination memory location
+}
+
+void Init(void){
+  
+    mc_debug.attach(&motMsg_RX_ISR);//start the serial receiver interrupt
+    //mc_usart.attach(&motMsg_RX_ISR);//start the serial receiver interrupt    
+    led1=0;
+    
+    
+    mc_usart.printf("\r\n ED Motor Controler API");  
+    mc_usart.printf("\r\n\r\n HARDWARE \t\t=  %s", HARDWARE.c_str());
+    mc_usart.printf("\r\n\r\n SOFTWARE \t\t=  %s",SOFTWARE.c_str());
+    mc_usart.printf("\r\n\r\n AUTHOR \t\t=  %s",AUTHOR.c_str());
+    mc_usart.printf("\r\n\r\n DATE \t\t\t=  %s",__DATE__);        
+    mc_usart.printf("\r\n\r\n TIME \t\t\t=  %s\r\n\r\n",__TIME__); 
     
-    if((par1 != newPar1) || (par2 != newPar2)){
-        
-        newPar1 = par1;
-        newPar2 = par2;        
-        
-        if(((par1 >=0) && (par1 <=4))&&((par2 >= REV_MAX_STEPS) && (par2 <= FWD_MAX_STEPS))){  
-            sprintf(mc_Tx_Buffer,"MOVEMOT %d %u\r",par1, par2);      
-            mc_usart.printf("%s\r",mc_Tx_Buffer);    
-        }
-        else
-            fault = true;
-                            
-    }
-    return(fault);    
+          
+/*    
+    d3_button.mode(PullUp);
+    d4_button.mode(PullUp);
+    d5_button.mode(PullUp);        
+    d6_button.mode(PullUp);
+    d7_button.mode(PullUp);
+    d8_button.mode(PullUp);        
+    d9_button.mode(PullUp);
+    d10_button.mode(PullUp);
+    d11_button.mode(PullUp);        
+    d12_button.mode(PullUp);
+*/      
+    
+    a7_out1 = 0;//motor enable
+    a6_out2 = 0;
+    a5_out3 = 0;
+
 }
+
+long mapI(long x, long in_min, long in_max, long out_min, long out_max){
+  return (x - in_min) * (out_max - out_min) / (in_max - in_min) + out_min;
+}
+
+float mapF(float in, float inMin, float inMax, float outMin, float outMax){
+  // check it's within the range
+  if (inMin<inMax) { 
+    if (in <= inMin) 
+      return outMin;
+    if (in >= inMax)
+      return outMax;
+  } else {  // cope with input range being backwards.
+    if (in >= inMin) 
+      return outMin;
+    if (in <= inMax)
+      return outMax;
+  }
+  // calculate how far into the range we are
+  float scale = (in-inMin)/(inMax-inMin);
+  // calculate the output.
+  return outMin + scale*(outMax-outMin);
+}
\ No newline at end of file

diff -r 333434a8611b -r 0cb065f9d55a MotorControl.h
--- a/MotorControl.h	Fri Mar 20 10:54:58 2020 +0000
+++ b/MotorControl.h	Mon May 18 19:04:41 2020 +0000
@@ -4,33 +4,197 @@
 #include "mbed.h"
 #include <string> 
 
-#define DEBUG_MOTOR
+#define DEBUG
+
+
+#define ED_MIN_SPEED            1000
+#define ED_MAX_SPEED            3000000
+#define N_MIN_SPEED             0.001
+#define N_MAX_SPEED             125.000//125.0RPM
+
+#define REV_MAX_STEPS           -2147483648
+#define FWD_MAX_STEPS           2147483648
+
+#define MIN_CURRENT             0
+#define MAX_CURRENT             6000//6000mA
+
+#define MIN_TORQUE              0
+#define MAX_TORQUE              8500//N.m
+
+
+#define SPACE                   0x20 
+#define ACK                     0x06
+#define NAK                     0x15
+
+#define VALID_MSG_LEN           0x03
+
+#define ML_ROTATION             5.6
+#define STEPS_PER_ROTATION      1024
+
+#define CW                      1
+#define CCW                     0
+
+#define RX_ECHO_VALID           0 
+#define RX_ECHO_FAIL            1
+#define RX_REGITSER_FAIL        2
+#define RX_REGITSER_VALID       3
+#define MSG_RESEND              4
 
-#define BUFFER_SIZE  32
-#define MESSAGE 20
+#define REBOOT                  5
+
+#define MSG_RETRY_COUNT         3
+ 
+#define NOT_USED                0 
+#define STOP_NO_RAMP            0
+#define STOP_WITH_RAMP          1 
+#define STOP_WITH_STEPS         2
+
+#define BUFFER_SIZE             50//command message + data
+#define DATA_BUFFER_SIZE        20//characters
+#define DATA_ITEMS              19
+
+////////////////////////////////////////////
+// MOTOR COMMAND RESPONSE DATA STORAGE 
+/////////////////////////////////////////// 
+#define READ_DIG_IN                 0
+#define READ_DIG_OUT                1
+#define READ_CURRENT                2
+#define READ_DRV_REG                3
+#define READ_DRV_REG_EXT            4
+#define READ_DRV_TEMP               5
+#define READ_DRV_VOLTAGE            6
+#define READ_DRV_WORKING_SET        7
+#define READ_DRV_WORKING_SET_EXT    8
+#define READ_ERROR_REG              9 
+#define READ_FB_BOOST_CUR           10
+#define READ_FB_STATUS              11
+#define READ_FW_VERSION             12
+#define READ_FW_CHECKSUM            13
+#define READ_MASTER_REG             14
+#define READ_MIN_CURRENT            15
+#define READ_MAX_CURRENT            16
+#define READ_BOOST_CURRENT          17
+#define READ_NOM_CURRENT            18
+
+#define COMMANDS_TO_TEST            10
 
-#define REV_MAX_STEPS -2147483648
-#define FWD_MAX_STEPS 2147483648
-#define DEBUG
+
+////////////////////////////////////////////
+// BAUD RATES
+///////////////////////////////////////////
+#define SLOW_BAUD               9600
+#define MED_BAUD                115200
+#define FAST_BAUD               921600
+ 
+#define MOT_AL_PDT              0x0001//Protection Drive Thermal
+#define MOT_AL_PDV              0x0002//Protection Drive Voltage
+#define MOT_AL_PDC              0x0004//Protection Drive Current
+#define MOT_AL_POP              0x0008//Protection Open Phase
+#define MOT_AL_UNUSED1          0x0010//NOT USED
+#define MOT_AL_FE               0x0020//Feedback Error    
+#define MOT_AL_PCR              0x0040//Protection Current Regulation
+#define MOT_AL_POT              0x0080//Protection Open Transistor
+#define MOT_AL_UNUSED2          0x0100//NOT USED
+#define MOT_AL_SWER             0x0200//Protection Software Error
+#define MOT_AL_UNUSED3          0x0400//NOT USED
+#define MOT_AL_MC               0x0800//Missing Calibration
+#define MOT_AL_WDG              0x1000//Watchdog
+#define MOT_AL_EEF              0x2000//EEPROM Fail
+#define MOT_AL_I2T              0x4000//I2T Potection
+#define MOT_AL_UNUSED4          0x8000//NOT USED
+
  
+#define D3_BUTTON               D3
+#define D4_BUTTON               D4
+#define D5_BUTTON               D5
+#define D6_BUTTON               D6
+#define D7_BUTTON               D7
+#define D8_BUTTON               D8
+#define D9_BUTTON               D9
+#define D10_BUTTON              D10
+#define D11_BUTTON              D11
+#define D12_BUTTON              D12
 
-extern RawSerial mc_usart;// this will be USART6 for motor control 
+#define A7_OUT1                 A7
+#define A6_OUT2                 A6
+#define A5_OUT3                 A5
+
+#define A4_IN1                  A4
+#define A3_IN2                  A3
+#define A2_IN3                  A2
+
+#define A1_OUT                  A1
+#define A0_OUT                  A0
+
+extern DigitalIn d3_button;
+extern DigitalIn d4_button;
+extern DigitalIn d5_button;
+extern DigitalIn d6_button;
+extern DigitalIn d7_button;
+extern DigitalIn d8_button;
+extern DigitalIn d9_button;
+extern DigitalIn d10_button;
+extern DigitalIn d11_button;
+extern DigitalIn d12_button;
+
+extern DigitalOut a7_out1;
+extern DigitalOut a6_out2;
+extern DigitalOut a5_out3;
+extern DigitalIn a4_in1;
+extern DigitalIn a3_in2;
+extern DigitalIn a2_in3;
+
+extern DigitalOut a1_out;
+extern DigitalOut a0_out;
+
+extern char motData[][DATA_BUFFER_SIZE];//data only
+
+extern char binBuffer[];
+extern uint8_t cnt;
 extern const string rxMsgTable[];
 extern char rxMsgStore[];//received local message store
 extern char mc_Tx_Buffer[];//transmitted message buffer 
-extern volatile char mc_Rx_Buffer[];//received message buffer
+extern char mc_Rx_Buffer[];//received message buffer
 extern volatile unsigned char mc_Rx_Rd_Ptr;//circulare buffer read pointer
 extern volatile unsigned char mc_Rx_Wr_Ptr;//circulare buffer write pointer
 extern unsigned int rx_int;//received integer / character
+extern volatile bool rxMsgValid;//message received flag
+extern bool msgRdy;
 
-//unsigned char i; 
-extern bool msgRecFlag;//message received flag 
+void motEncoderISR(void);
+
+void int2bin(int value, char* buffer, int bufferSize);
 
+void rstPointers(void);
+
+void readCurrent(char *array);
+bool setDigOutput(uint8_t par1, char *array);
+void readDigInput(char *array);
+void clrRxMsg(char *array);
 void motMsg_RX_ISR(void);
-bool getMotMsg(char * msgArray);
-void motMsgDecode(unsigned int rxMsgTable[], unsigned int rxMsg[]);void clrRxMsg(char array[]);
-bool moveMot(uint8_t par1, int32_t par2);
+uint8_t getMotMsg(char *txReadCmd, char *rxRegArray);
+
 
 
 
+bool set_mL(bool direction, uint32_t mL, char *array);
+bool setRotMotor(bool direction, char *array);
+
+void initBuffer(string s, char *array, char character, uint8_t size);
+
+void dispBuffer(string s, uint8_t size, char array[]);
+void dispMotReg(string s, uint8_t regSel, char regArray[9][BUFFER_SIZE]);
+
+
+uint8_t procMotMsg(char *txMsg, char *rxMsg, char motReg[9][BUFFER_SIZE]);
+void dispRegDec(string s, uint8_t regSel, char regArray[9][BUFFER_SIZE]);
+void dispRegHex(string s, uint8_t regSel, char regArray[9][BUFFER_SIZE]);
+
+
+void data2mem(uint8_t dataLoc, char destMemArray[][DATA_BUFFER_SIZE], char sourceMemArray[BUFFER_SIZE]);//for command with <CR><VALUE>  
+void Init(void);
+
+long mapI(long x, long in_min, long in_max, long out_min, long out_max);
+float mapF(float in, float inMin, float inMax, float outMin, float outMax);
+
 #endif
\ No newline at end of file

diff -r 333434a8611b -r 0cb065f9d55a main.cpp
--- a/main.cpp	Fri Mar 20 10:54:58 2020 +0000
+++ b/main.cpp	Mon May 18 19:04:41 2020 +0000
@@ -1,3 +1,44 @@
+/*
+
+EOL SEQUENCE = NONE ON YAT, PROTOCOL WILL FAIL IF THIS IS NOT DONE
+
+*/
+
+
+#include "mbed.h"
+#include <string> 
+#include "MotorControl.h"
+#include "readCommands.h"
+#include "writeCommands.h"
+#include "main.h"
+
+//================================================================================================================================
+// COMPANY                :  Verderflex
+//
+// PROJECT                :  ED Motor Controler API
+// DATE                   :  11th May 2020   
+
+string HARDWARE =           "Hardware Version : X1.00";                                                
+string SOFTWARE =           "Software Version : X1.00";
+string AUTHOR   =           "Author : Jim Lowe";
+
+/*
+ *   Firmware Numbering Scheme, V Major.Minor.Bugfix
+ *
+ *   Major  : Major Changes to function
+ *   Minor  : Minor Changes to function
+ *   Bugfix : Firmware fixes to function
+ *
+ */
+
+// HARDWARE              :   NUCLEO-F746ZG + Nextion 4.3" Display
+//
+// COMPILER              :   Mbed Studio
+
+//================================================================================================================================
+//************************************************  Code Status / Changes ********************************************************
+//================================================================================================================================
+
 
 /*
 
@@ -10,46 +51,192 @@
     In order to compare what was sent to what is echoed back two buffers are required, a TX buffer and an RX buffer, they can then be compared 
     using memcmp() function
     
-    19/03/20 Compare of tx and rx buffers can be done
-    20/03/20 Migrate code to mbed studio
-
-
+    19/03/20    Compare of tx and rx buffers can be done
+    20/03/20    Migrate code to mbed studio
+    20/03/20    Code cannot be migrated to mbed studio as the target does not use mbed OS 5
+    23/03/20    memcmp() and memcpy() implemented and working, comparision of tx and rx buffers can be done
+    26/03/20    setSpeed(),setTorque(),setRotMotor(),set_mL() write commands tested and working with echo compare
+    26/03/20_D  ACK and NAK added, rx message checked for <CR><ACK> or <NAK>
+    27/03/20_D  Sorting of received massages into register type, echo type and NAK seems to be working
+    30/03/20_B  Process Motor Message function created that combines all message types that can be transmitted and received
+    31/03/20_D  LED1=1 indication validates data automatically, if data is invalid LED1=0 
+    03/04/20_A  Setup another serial port required for testing API, one for Debug and the other to communicate with Sagittarius drive
+    07/04/20_A  Simplified so that RX ISR collects the data and stores in the required memory location
+    08/04/20_B  Software manual IMD11 V00R02 changes need to be added to software, this is a lot of work as we now have to collect a lot of bytes returnd from the motor controller
+    11/05/20_A  Back to work,
+*/
+//What is sent is received exactly, no problems
 
-*/
+RawSerial mc_debug(USBTX, USBRX, MED_BAUD);//debug port PA2_TX, PA15_RX
+RawSerial mc_usart(PA_9,PA_10,MED_BAUD);//motor controller communication port
+//RawSerial mc_debug(PA_2,PB_4,MED_BAUD);//motor controller communication port
 
-#include "mbed.h"
-#include <string> 
-#include "MotorControl.h"
+//RawSerial mc_debug(PA_9,PA_10,MED_BAUD);//motor controller communication port
+//RawSerial mc_usart(USBTX, USBRX, MED_BAUD);//debug port PA2_TX, PA15_RX
+
+Timer tBtn;
 
 DigitalOut led1(LED1);
 
+Timer t1;
 
+uint8_t msgResend = false;
+
+float n_speed = 0.001;
+
+uint8_t cmdCnt=0;
+bool rxMsgPending = false;
+
+uint32_t speed=0;
 
-int main(){
-    
-    led1=0;
+int main(){//1
+
+    Init();
+       
+    //setup motor control
+    a7_out1 = 1;//motor hardware enable
+    setTorque(3456, mc_Tx_Buffer);//set torque 3.456N.m  
+    setRotMotor(CCW, mc_Tx_Buffer);//set motor CCW rotation
+    n_speed = 123.456;
+    setSpeed(&n_speed, mc_Tx_Buffer);
     
-    mc_usart.attach(&motMsg_RX_ISR);//start the serial receiver interrupt
-    mc_usart.printf("ED Motor Controller Commands\r\n");
-       
-    clrRxMsg(rxMsgStore);
-
-    moveMot(1, 1000);
+    tBtn.start();
     
-    mc_usart.printf("TX Buffer Contents\r\n");
+    t1.start();
+    
+    while(1){//2
     
-    for(unsigned char i=0 ; i <BUFFER_SIZE ; i++)                     
-        mc_usart.printf("%c\r\n",mc_Tx_Buffer[i]);
-  
-    while(1){
+        /*identify the last message sent    
+        
+          the mc_Tx_Buffer contain the last message sent out which has failed
+          this message need to be resent a number of times (3 tries)
         
-        if(getMotMsg(rxMsgStore)){//get the message from the receive buffer and store it in local message store
+        */
+
+        if(tBtn.read() > 0.100){//every x read the buttons
+            
+            tBtn.reset();
+            
+            if(d3_button){//speed inc
+                if(t1.read() > 1.0){
+                    t1.reset();
+                    n_speed+=10;
+
+                }
+                n_speed+=0.001;
+                setSpeed(&n_speed, mc_Tx_Buffer);
+            }
+            else
+            if(d4_button){//speed dec
+                if(t1.read() > 1.0){
+                    t1.reset();
+                    n_speed-=10;
+                }
+                n_speed-=0.001;
+                setSpeed(&n_speed, mc_Tx_Buffer);
+            }
+            else
+            if(d5_button){//stop
+               stopMot(STOP_NO_RAMP, NOT_USED,  mc_Tx_Buffer); 
+            }
+            else
+            if(d6_button){
+                
+            
+            }
+            else
+            if(d7_button){
+                
+            
+            }
+            else
+            if(d3_button){
+                
+            
+            }
+            else
+            if(d8_button){
+                
+            
+            }
+            else
+            if(d9_button){
+                
+            
+            }
+            else
+            if(d10_button){
+                
+            
+            }
+            else
+            if(d11_button){
+                
             
-            msgRecFlag = false;//clear the message received flag
+            }
+            else   
+            if(d12_button){
+                
             
-            if(memcmp(rxMsgStore,mc_Tx_Buffer,strlen(rxMsgStore)) == 0){
-                led1=1;
-            }            
-        }                      
-    }
-}
\ No newline at end of file
+            }
+            else
+                t1.reset();
+        }
+
+        //mc_debug.printf("cmdCnt = %d", cmdCnt);
+/*           
+        switch(cmdCnt){    
+            case 0: setRotMotor(CCW, mc_Tx_Buffer);break;//free run the motor CCW 
+            case 1: setSpeed(123456, mc_Tx_Buffer);break;//123.456 RPM  
+            case 2: setRotMotor(CW, mc_Tx_Buffer);break;//free run the motor CW
+            case 3: setTorque(3456, mc_Tx_Buffer);break;//3.456N.m 
+            case 4: setTorque(7000, mc_Tx_Buffer);break;//7N.m    
+            case 5: set_mL(CW, 1000, mc_Tx_Buffer);break;//dispence 1000ml running CW
+            case 6: set_mL(CCW, 1000, mc_Tx_Buffer);break;//dispence 1000ml running CW
+        }
+*/        
+/*           
+        switch(cmdCnt){
+            case READ_DIG_IN:               readDigInput(mc_Tx_Buffer);break;
+            case READ_DIG_OUT:              readDigOutput(mc_Tx_Buffer);break;
+            case READ_CURRENT:              readCurrent(mc_Tx_Buffer);break;
+            case READ_DRV_REG:              readDrvReg(mc_Tx_Buffer);break;
+            case READ_DRV_REG_EXT:          readDrvRegExt(mc_Tx_Buffer);break;
+            case READ_DRV_TEMP:             readTemp(mc_Tx_Buffer);break;
+            case READ_DRV_VOLTAGE:          readVoltage(mc_Tx_Buffer);break;
+            case READ_DRV_WORKING_SET:      readDrvWorkSet(mc_Tx_Buffer);break;
+            case READ_DRV_WORKING_SET_EXT:  readDrvWorkSetExt(mc_Tx_Buffer);break; 
+            case READ_ERROR_REG:            readErrReg(mc_Tx_Buffer);break;
+            case READ_FB_BOOST_CUR:         readFBboostCur(mc_Tx_Buffer);break;
+            case READ_FB_STATUS:            readFBstat(mc_Tx_Buffer);break;
+            case READ_FW_VERSION:           readFWVwersion(mc_Tx_Buffer);break;
+            case READ_FW_CHECKSUM:          readFWChecksum(mc_Tx_Buffer);break;
+            case READ_MASTER_REG:           readMastReg(mc_Tx_Buffer);break;   
+            case READ_MIN_CURRENT:          readMinCur(mc_Tx_Buffer);break;
+            case READ_MAX_CURRENT:          readMaxCur(mc_Tx_Buffer);break;
+            case READ_BOOST_CURRENT:        readBoostCur(mc_Tx_Buffer);break;  
+            case READ_NOM_CURRENT:          readNomCur(mc_Tx_Buffer);break;                
+        }
+*/        
+ /*
+         if(rxMsgPending == false){   
+            rxMsgPending = true;
+            
+             if(cmdCnt < COMMANDS_TO_TEST) 
+                cmdCnt++;
+            else
+                cmdCnt=0;     
+        }
+*/        
+
+        //while(getMotMsg(mc_Tx_Buffer, rxMsgStore) == RX_ECHO_FAIL);//message has failed so resent the last message again and loop a nuber of re-tries if required.
+    
+        
+        if(getMotMsg(mc_Tx_Buffer, rxMsgStore) == REBOOT){
+            mc_usart.printf("\r\n\r\nRebooting the system\r\n\r\n");  
+            NVIC_SystemReset(); 
+        }
+        
+        
+    }//2     
+}//1
\ No newline at end of file

diff -r 333434a8611b -r 0cb065f9d55a main.h
--- a/main.h	Fri Mar 20 10:54:58 2020 +0000
+++ b/main.h	Mon May 18 19:04:41 2020 +0000
@@ -2,6 +2,16 @@
 #define MAIN_H
 
 #include "mbed.h"
+#include <string> 
 
+extern RawSerial mc_debug;
+extern RawSerial mc_usart;// this will be USART6 for motor control 
 
+extern string HARDWARE;                                                
+extern string SOFTWARE;
+extern string AUTHOR;
+
+extern uint8_t cmdCnt;
+extern DigitalOut led1;
+extern bool rxMsgPending;
 #endif 
\ No newline at end of file

diff -r 333434a8611b -r 0cb065f9d55a readCommands.cpp
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/readCommands.cpp	Mon May 18 19:04:41 2020 +0000
@@ -0,0 +1,374 @@
+#include <stdio.h>
+#include "mbed.h"
+#include "main.h"
+#include "MotorControl.h"
+#include<string>
+
+
+////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+// MOTOR COMMAND READ FUNCTIONS
+////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+
+void readDigInput(char *array){
+////////////////////////////////////////////////////////////////////////////////
+// Read digital inputs
+//
+//////////////////////////////////////////////////////////////////////////////// 
+    if(rxMsgPending == false){
+        sprintf(array,"READ DIGITAL INPUTS\r");      
+        mc_debug.printf("%s",array);
+        //mc_usart.printf("%s",array);        
+        rxMsgPending = true;
+        led1=0;  
+        
+        if(cmdCnt < COMMANDS_TO_TEST) 
+            cmdCnt++;
+        else
+            cmdCnt=0;                   
+    }   
+} 
+
+void readDigOutput(char *array){
+////////////////////////////////////////////////////////////////////////////////
+// Read digital inputs
+//
+//////////////////////////////////////////////////////////////////////////////// 
+    if(rxMsgPending == false){
+        sprintf(array,"READ DIGITAL OUTPUTS\r");      
+        mc_debug.printf("%s",array);
+        //mc_usart.printf("%s",array);        
+        rxMsgPending = true;
+        led1=0;  
+        
+        if(cmdCnt < COMMANDS_TO_TEST) 
+            cmdCnt++;
+        else
+            cmdCnt=0;                   
+    }   
+} 
+
+void readCurrent(char *array){
+////////////////////////////////////////////////////////////////////////////////
+// Read the motor current command
+//
+//////////////////////////////////////////////////////////////////////////////// 
+    if(rxMsgPending == false){
+        sprintf(array,"READ CURRENT ACTUAL\r");      
+        mc_debug.printf("%s",array);
+        //mc_usart.printf("%s",array);        
+        rxMsgPending = true;
+        led1=0;  
+
+        if(cmdCnt < COMMANDS_TO_TEST) 
+            cmdCnt++;
+        else
+            cmdCnt=0;             
+    }       
+}  
+
+void readDrvReg(char *array){
+////////////////////////////////////////////////////////////////////////////////
+// Read drive register command
+//
+//////////////////////////////////////////////////////////////////////////////// 
+    if(rxMsgPending == false){
+        sprintf(array,"READ DRIVE REGISTER\r");      
+        mc_debug.printf("%s",array);
+        //mc_usart.printf("%s",array);        
+        rxMsgPending = true;
+        led1=0;  
+        
+        if(cmdCnt < COMMANDS_TO_TEST) 
+            cmdCnt++;
+        else
+            cmdCnt=0;         
+    }   
+} 
+
+void readDrvRegExt(char *array){
+////////////////////////////////////////////////////////////////////////////////
+// Read drive register extended command
+//
+//////////////////////////////////////////////////////////////////////////////// 
+    if(rxMsgPending == false){
+        sprintf(array,"READ DRIVE REGISTER EXTENDED\r");      
+        mc_debug.printf("%s",array); 
+        //mc_usart.printf("%s",array);         
+        rxMsgPending = true;
+        led1=0;   
+        
+        if(cmdCnt < COMMANDS_TO_TEST) 
+            cmdCnt++;
+        else
+            cmdCnt=0;          
+    }
+} 
+
+void readTemp(char *array){
+////////////////////////////////////////////////////////////////////////////////
+// Read motor temperature command
+//
+//////////////////////////////////////////////////////////////////////////////// 
+    if(rxMsgPending == false){
+        sprintf(array,"READ DRIVE TEMPERATURE\r");      
+        mc_debug.printf("%s",array);
+        //mc_usart.printf("%s",array);        
+        rxMsgPending = true;
+        led1=0;    
+        
+        if(cmdCnt < COMMANDS_TO_TEST) 
+            cmdCnt++;
+        else
+            cmdCnt=0;                   
+    } 
+} 
+
+void readVoltage(char *array){
+////////////////////////////////////////////////////////////////////////////////
+// Read motor voltage command
+//
+//////////////////////////////////////////////////////////////////////////////// 
+    if(rxMsgPending == false){
+        sprintf(array,"READ DRIVE VOLTAGE\r");      
+        mc_debug.printf("%s",array);
+        //mc_usart.printf("%s",array);        
+        rxMsgPending = true;
+        led1=0;   
+        
+        if(cmdCnt < COMMANDS_TO_TEST) 
+            cmdCnt++;
+        else
+            cmdCnt=0;         
+    }      
+}  
+
+void readDrvWorkSet(char *array){
+////////////////////////////////////////////////////////////////////////////////
+// Read drive working settings command
+//
+//////////////////////////////////////////////////////////////////////////////// 
+    if(rxMsgPending == false){
+        sprintf(array,"READ DRIVE WORKING SETTINGS\r");//<CR>      
+        mc_debug.printf("%s",array);
+        //mc_usart.printf("%s",array);        
+        rxMsgPending = true; 
+        led1=0;  
+        
+        if(cmdCnt < COMMANDS_TO_TEST) 
+            cmdCnt++;
+        else
+            cmdCnt=0;           
+    } 
+} 
+
+void readDrvWorkSetExt(char *array){
+////////////////////////////////////////////////////////////////////////////////
+// Read
+//
+//////////////////////////////////////////////////////////////////////////////// 
+    if(rxMsgPending == false){
+        sprintf(array,"READ DRIVE WORKING SETTINGS EXTENDED\r");//<CR>       
+        mc_debug.printf("%s",array);
+        //mc_usart.printf("%s",array);        
+        rxMsgPending = true;
+        led1=0;  
+        
+        if(cmdCnt < COMMANDS_TO_TEST) 
+            cmdCnt++;
+        else
+            cmdCnt=0;           
+    }  
+} 
+
+void readErrReg(char *array){
+////////////////////////////////////////////////////////////////////////////////
+// Read error register command
+//
+//////////////////////////////////////////////////////////////////////////////// 
+    if(rxMsgPending == false){
+        sprintf(array,"READ ERROR REGISTER\r");      
+        mc_debug.printf("%s",array);
+        //mc_usart.printf("%s",array);        
+        rxMsgPending = true; 
+        led1=0; 
+        
+        if(cmdCnt < COMMANDS_TO_TEST) 
+            cmdCnt++;
+        else
+            cmdCnt=0;         
+    } 
+} 
+
+void readFBboostCur(char *array){
+////////////////////////////////////////////////////////////////////////////////
+// Read
+//
+//////////////////////////////////////////////////////////////////////////////// 
+    if(rxMsgPending == false){
+        sprintf(array,"READ FEEDBACK BOOST CURRENT\r");//<CR>        
+        mc_debug.printf("%s",array);
+        //mc_usart.printf("%s",array);        
+        rxMsgPending = true;
+        led1=0;
+        
+        if(cmdCnt < COMMANDS_TO_TEST) 
+            cmdCnt++;
+        else
+            cmdCnt=0;             
+    }  
+}
+
+void readFBstat(char *array){
+////////////////////////////////////////////////////////////////////////////////
+// Read
+//
+//////////////////////////////////////////////////////////////////////////////// 
+    if(rxMsgPending == false){
+        sprintf(array,"READ FEEDBACK STATUS\r");//<CR>        
+        mc_debug.printf("%s",array);
+        //mc_usart.printf("%s",array);        
+        rxMsgPending = true;
+        led1=0;  
+        
+        if(cmdCnt < COMMANDS_TO_TEST) 
+            cmdCnt++;
+        else
+            cmdCnt=0;           
+    }  
+}
+
+void readFWVwersion(char *array){
+////////////////////////////////////////////////////////////////////////////////
+// Read the firmware version command
+//
+//////////////////////////////////////////////////////////////////////////////// 
+    if(rxMsgPending == false){
+        sprintf(array,"READ FIRMWARE VERSION\r");      
+        mc_debug.printf("%s",array);  
+        //mc_usart.printf("%s",array);        
+        rxMsgPending = true;  
+        led1=0;
+        
+        if(cmdCnt < COMMANDS_TO_TEST) 
+            cmdCnt++;
+        else
+            cmdCnt=0;         
+    }  
+    
+}
+
+void readFWChecksum(char *array){
+////////////////////////////////////////////////////////////////////////////////
+// Read the firmware checksum command
+//
+//////////////////////////////////////////////////////////////////////////////// 
+    if(rxMsgPending == false){
+        sprintf(array,"READ FIRMWARE CHECKSUM\r");      
+        mc_debug.printf("%s",array);   
+        //mc_usart.printf("%s",array);           
+        rxMsgPending = true; 
+        led1=0;  
+        
+        if(cmdCnt < COMMANDS_TO_TEST) 
+            cmdCnt++;
+        else
+            cmdCnt=0;         
+    }  
+}
+
+void readMastReg(char *array){
+////////////////////////////////////////////////////////////////////////////////
+// Read
+//
+//////////////////////////////////////////////////////////////////////////////// 
+    if(rxMsgPending == false){
+        sprintf(array,"READ MASTER REGISTER\r");//<CR>        
+        mc_debug.printf("%s",array);
+        //mc_usart.printf("%s",array);        
+        rxMsgPending = true;
+        led1=0;   
+        
+        if(cmdCnt < COMMANDS_TO_TEST) 
+            cmdCnt++;
+        else
+            cmdCnt=0;         
+    }  
+}
+
+void readMinCur(char *array){
+////////////////////////////////////////////////////////////////////////////////
+// Read
+//
+//////////////////////////////////////////////////////////////////////////////// 
+    if(rxMsgPending == false){
+        sprintf(array,"READ MIN CURRENT\r");//<CR>        
+        mc_debug.printf("%s",array);
+        //mc_usart.printf("%s",array);        
+        rxMsgPending = true;
+        led1=0;   
+        
+        if(cmdCnt < COMMANDS_TO_TEST) 
+            cmdCnt++;
+        else
+            cmdCnt=0;         
+    }  
+}
+
+void readMaxCur(char *array){
+////////////////////////////////////////////////////////////////////////////////
+// Read
+//
+//////////////////////////////////////////////////////////////////////////////// 
+    if(rxMsgPending == false){
+        sprintf(array,"READ MAX CURRENT\r");//<CR>        
+        mc_debug.printf("%s",array);
+        //mc_usart.printf("%s",array);        
+        rxMsgPending = true;
+        led1=0;   
+        
+        if(cmdCnt < COMMANDS_TO_TEST) 
+            cmdCnt++;
+        else
+            cmdCnt=0;         
+    }  
+}
+
+void readBoostCur(char *array){
+////////////////////////////////////////////////////////////////////////////////
+// Read
+//
+//////////////////////////////////////////////////////////////////////////////// 
+    if(rxMsgPending == false){
+        sprintf(array,"READ BOOST CURRENT\r");//<CR>        
+        mc_debug.printf("%s",array);
+        //mc_usart.printf("%s",array);        
+        rxMsgPending = true;
+        led1=0;   
+        
+        if(cmdCnt < COMMANDS_TO_TEST) 
+            cmdCnt++;
+        else
+            cmdCnt=0;         
+    }  
+}
+
+void readNomCur(char *array){
+////////////////////////////////////////////////////////////////////////////////
+// Read
+//
+//////////////////////////////////////////////////////////////////////////////// 
+    if(rxMsgPending == false){
+        sprintf(array,"READ NOMINAL CURRENT\r");//<CR>        
+        mc_debug.printf("%s",array);
+        //mc_usart.printf("%s",array);        
+        rxMsgPending = true;
+        led1=0;   
+        
+        if(cmdCnt < COMMANDS_TO_TEST) 
+            cmdCnt++;
+        else
+            cmdCnt=0;         
+    }  
+}

diff -r 333434a8611b -r 0cb065f9d55a readCommands.h
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/readCommands.h	Mon May 18 19:04:41 2020 +0000
@@ -0,0 +1,27 @@
+#ifndef READCOMMANDS_H
+#define READCOMMANDS_H
+
+#include "mbed.h"
+#include <string> 
+
+void readDigOutput(char *array);
+void readErrReg(char *array);
+void readTemp(char *array);
+void readVoltage(char *array);
+void readDrvReg(char *array);
+void readDrvRegExt(char *array);
+void readDrvWorkSet(char *array);
+void readDrvWorkSetExt(char *array);
+void readMastReg(char *array);
+void readFWVwersion(char *array);
+void readFWChecksum(char *array);
+void readFBboostCur(char *array);
+void readFBstat(char *array);
+void readMinCur(char *array);
+void readMaxCur(char *array);
+void readBoostCur(char *array);
+void readMasterReg(char *array);
+void readNomCur(char *array);
+
+
+#endif
\ No newline at end of file

diff -r 333434a8611b -r 0cb065f9d55a writeCommands.cpp
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/writeCommands.cpp	Mon May 18 19:04:41 2020 +0000
@@ -0,0 +1,374 @@
+#include <stdio.h>
+#include "mbed.h"
+#include "main.h"
+#include "MotorControl.h"
+#include "writeCommands.h"
+#include<string>
+
+////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+// PRODUCT COMMAND FUNCTIONS
+////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+
+bool setTorque(uint16_t newtonMilliMetres, char *array){
+////////////////////////////////////////////////////////////////////////////////
+// Set Torque
+//
+// newtonMilliMetres : (0 - 8500) : 0 - 8.5 N.m
+//
+// return : invalid param = 1, ok = 0
+//
+////////////////////////////////////////////////////////////////////////////////  
+
+    bool invParm = false;
+
+    if(rxMsgPending == false){  
+        if(newtonMilliMetres <= MAX_TORQUE){  
+            sprintf(array,"SET FEEDBACK BOOST CURRENT %u\r",newtonMilliMetres);      
+            mc_debug.printf("%s",array);   
+            //mc_usart.printf("%s",array);   
+                    
+            #ifdef DEBUG    
+                //mc_debug.printf("Set Torque %u\r\n", newtonMilliMetres);   
+                mc_usart.printf("Set Torque %u\r\n", newtonMilliMetres);                             
+            #endif 
+        }
+        else{
+            invParm = true;
+            #ifdef DEBUG    
+                //mc_debug.printf("set Torque parameter fail\r\n");    
+                mc_usart.printf("set Torque parameter fail\r\n");                          
+            #endif
+        }  
+/*    
+        if(cmdCnt < COMMANDS_TO_TEST) 
+            cmdCnt++;
+        else
+            cmdCnt=0; 
+*/            
+        //rxMsgPending = true;
+        led1=0;              
+    }
+    return(invParm);   
+}
+
+
+bool set_mL(bool direction, uint32_t mL, char *array){
+////////////////////////////////////////////////////////////////////////////////
+// Set milli litres
+//
+// direction : CW = 1, CCW = 0
+//
+// mL : 0 - 2147483648, -2147483648 - 0 : 2.1 billion
+//
+// return : invalid param = 1, ok = 0
+//
+////////////////////////////////////////////////////////////////////////////////      
+
+    bool invParm = false;    
+    uint32_t steps = 0;
+    
+    if(rxMsgPending == false){       
+        steps = ((mL/1000)/(ML_ROTATION/1000))*STEPS_PER_ROTATION;
+        
+        if(direction == CW)
+            invParm = moveMot(2, steps, array);
+        else    
+            if(direction == CCW)   
+                invParm = moveMot(3, steps, array);
+      
+        #ifdef DEBUG   
+            if(direction == CW){ 
+                //mc_debug.printf("Dispense CW %d steps\r\n", steps);
+                mc_usart.printf("Dispense CW %d steps\r\n", steps);                
+            }      
+            else             
+            if(direction == CCW){ 
+                //mc_debug.printf("Dispense CCW %d steps\r\n", steps); 
+                mc_usart.printf("Dispense CCW %d steps\r\n", steps); 
+            }                 
+                            
+        #endif 
+/*        
+        if(cmdCnt < COMMANDS_TO_TEST) 
+            cmdCnt++;
+        else
+            cmdCnt=0; 
+*/            
+        //rxMsgPending = true;
+        led1=0;                   
+    }      
+    return(invParm);             
+}
+
+bool setRotMotor(bool direction, char *array){
+////////////////////////////////////////////////////////////////////////////////
+// Rotate motor
+//
+// direction : CW = 1, CCW = 0
+//
+// return : invalid param = 1, ok = 0
+//
+////////////////////////////////////////////////////////////////////////////////
+   
+    bool invParm = false;
+    
+    if(rxMsgPending == false){
+        if(direction == CW)
+            invParm = moveMot(0, 0, array);
+        else    
+            if(direction == CCW)   
+                invParm = moveMot(1, 0, array); 
+      
+        #ifdef DEBUG   
+            if(direction == CW){ 
+                //mc_debug.printf("MOVEMOT free run CW\r\n"); 
+                mc_usart.printf("MOVEMOT free run CW\r\n"); 
+            }                 
+            else   
+            if(direction == CCW){ 
+                //mc_debug.printf("MOVEMOT free run CCW\r\n");   
+                mc_usart.printf("MOVEMOT free run CCW\r\n");                
+            }              
+                            
+        #endif 
+/*              
+        if(cmdCnt < COMMANDS_TO_TEST) 
+            cmdCnt++;
+        else
+            cmdCnt=0;
+*/            
+        //rxMsgPending = true;
+        led1=0;                   
+    }      
+    return(invParm);                   
+}
+
+bool stopMot(uint8_t par1, int32_t par2, char *array){
+////////////////////////////////////////////////////////////////////////////////
+// Stop Motor
+//
+// par1 = 0 : stop with no ramp  | par2 : not used
+// par1 = 1 : stop with ramp     | par2 : not used
+// par1 = 2 : stop with steps    | par2 : number of stop steps
+//
+// return : invalid param = 1
+//
+////////////////////////////////////////////////////////////////////////////////    
+
+    static uint8_t newPar1 = 0xff;
+    static int32_t newPar2 = 0xff;    
+    bool invParm = false;
+    
+    //if((par1 != newPar1) || (par2 != newPar2)){
+        
+        newPar1 = par1;
+        newPar2 = par2;        
+        
+        if(((par1 == STOP_NO_RAMP)||(par1 == STOP_WITH_RAMP))&&(par2 == NOT_USED)){
+            sprintf(array,"STOPMOT %u %u\r",par1, par2);      
+            mc_debug.printf("%s",array); 
+            //mc_usart.printf("%s",array);  
+            
+            #ifdef DEBUG    
+                //mc_debug.printf("STOPMOT %u %u\r",par1, par2);   
+                mc_usart.printf("STOPMOT %u %u\r",par1, par2);                                  
+            #endif              
+        }
+        else    
+        if((par1 == STOP_WITH_STEPS)&&((par2 >= REV_MAX_STEPS)&&(par2 <= FWD_MAX_STEPS))){  
+            sprintf(array,"STOPMOT %u %u\r",par1, par2);      
+            mc_debug.printf("%s",array); 
+            //mc_usart.printf("%s",array);   
+            
+            #ifdef DEBUG    
+                //mc_debug.printf("STOPMOT %u %u\r",par1, par2);   
+                mc_usart.printf("STOPMOT %u %u\r",par1, par2);                                  
+            #endif                  
+        }
+        else{
+            invParm = true;
+            #ifdef DEBUG    
+                //mc_debug.printf("STOPMOT parameter fail\r\n"); 
+                mc_usart.printf("STOPMOT parameter fail\r\n");                     
+            #endif
+        }                            
+    //}
+    led1=0;             
+        
+    return(invParm);   
+}
+
+bool setSpeed(float *n_speed, char *array){
+////////////////////////////////////////////////////////////////////////////////
+// Set Speed
+//
+// 1000 = 0.001 RPM
+// 3000000 = 125.0 RPM
+//
+// range 1000 - 3000000
+//
+// speed :(1 - 125000) : 0.001 - 125.000 RPM
+//
+// return : invalid param = 1, ok = 0
+//
+////////////////////////////////////////////////////////////////////////////////   
+
+    bool invParm = false;
+    uint32_t ed_speed;//motor controller speed, 1000 - 3000000
+    
+    if((*n_speed >= N_MIN_SPEED)&&(*n_speed <= N_MAX_SPEED)){  
+        
+        ed_speed = mapF(*n_speed, N_MIN_SPEED, N_MAX_SPEED, ED_MIN_SPEED, ED_MAX_SPEED);
+         
+        sprintf(array,"SET PROFILE VELOCITY %u\r",ed_speed);      
+        mc_debug.printf("%s",array);      
+                            
+        #ifdef DEBUG    
+            //mc_debug.printf("ED_SetSpeed %u, Neptune_SetSpeed %f\r\n", ed_speed, n_speed);  
+            mc_usart.printf("ED_SetSpeed %u, Neptune_SetSpeed %0.3f\r\n", ed_speed, *n_speed);                            
+        #endif              
+    
+        led1=0;             
+    }
+    else{
+        
+        if(*n_speed > N_MAX_SPEED)
+            *n_speed = N_MAX_SPEED;
+        else    
+        if(*n_speed < N_MIN_SPEED)
+            *n_speed = N_MIN_SPEED;
+        
+
+/*        
+        #ifdef DEBUG  
+            ed_speed = mapF(*n_speed, N_MIN_SPEED, N_MAX_SPEED, ED_MIN_SPEED, ED_MAX_SPEED);          
+            //mc_debug.printf("Auto Limit applied ED_SetSpeed %u, Neptune_SetSpeed %0.3f\r\n", ed_speed, *n_speed); 
+            mc_usart.printf("Auto Limit applied ED_SetSpeed %u, Neptune_SetSpeed %0.3f\r\n", ed_speed, *n_speed);                             
+        #endif  
+*/        
+    }
+    return(invParm);  
+}
+
+bool setWorkSetExt(uint16_t par1, char *array){
+////////////////////////////////////////////////////////////////////////////////
+// Set Working Settings Extended
+//
+// par1 : bit 5 = disable auto motor enable
+// par1 : bit 6 = encoder rotation direction
+// par1 : bit 9 = RL motor detection
+// par1 : bit 10 = enable I2T protection
+//
+// return : invalid param = 1, ok = 0
+//
+////////////////////////////////////////////////////////////////////////////////     
+
+    static uint8_t newPar1 = 0xff;
+    bool invParm = false;
+    
+    if(rxMsgPending == false){      
+        if(par1 != newPar1){
+            
+            newPar1 = par1;    
+            
+            if(par1 <= 15){  
+                sprintf(array,"SET DRIVE WORKING SETTINGS EXTENDED %u\r",par1);      
+                mc_debug.printf("%s",array);    
+                //mc_usart.printf("%s",array);                 
+                
+                #ifdef DEBUG    
+                    //mc_debug.printf("Drive working setting extended %u\r\n", par1);   
+                    mc_usart.printf("Drive working setting extended %u\r\n", par1);                              
+                #endif                 
+            }
+            else{
+                invParm = true;
+                #ifdef DEBUG    
+                    //mc_debug.printf("setWorkSet parameter fail\r\n");     
+                    mc_usart.printf("setWorkSet parameter fail\r\n");                          
+                #endif
+            }                            
+        }
+/*    
+        if(cmdCnt < COMMANDS_TO_TEST) 
+            cmdCnt++;
+        else
+            cmdCnt=0; 
+*/    
+        //rxMsgPending = true;
+        led1=0;         
+    }
+    return(invParm);
+}   
+
+bool moveMot(uint8_t par1, int32_t par2, char *array){
+////////////////////////////////////////////////////////////////////////////////
+// Move Motor
+//
+// par1 = 0 : move free running forward    | par2 : not used
+// par1 = 1 : move free running backwards  | par2 : not used
+// par1 = 2 : move step forward            | par2 : number of steps
+// par1 = 3 : move step backwards          | par2 : number of steps
+// par1 - 4 : move to target posistion     | par2 : posistion to reach
+//
+// return : invalid param = 1, ok = 0
+//
+////////////////////////////////////////////////////////////////////////////////
+
+    static uint8_t newPar1 = 0xff;
+    static int32_t newPar2 = 0xff;    
+    bool invParm = false;
+
+    if(rxMsgPending == false){      
+        if((par1 != newPar1) || (par2 != newPar2)){
+            
+            newPar1 = par1;
+            newPar2 = par2;        
+            
+            if(((par1 >=2)&&(par1 <=4))&&((par2 >= REV_MAX_STEPS)&&(par2 <= FWD_MAX_STEPS))){  
+                sprintf(array,"MOVEMOT %u %u\r",par1, par2);      
+                mc_debug.printf("%s",array);  
+                //mc_usart.printf("%s",array);                 
+                
+                #ifdef DEBUG    
+                    //mc_debug.printf("MOVEMOT %u, %u\r\n", par1, par2);   
+                    mc_usart.printf("MOVEMOT %u, %u\r\n", par1, par2);                               
+                #endif                   
+            }
+            else
+            if((par1 == 0)||(par1 ==1)){  
+                sprintf(array,"MOVEMOT %u %u\r",par1, 0);      
+                mc_debug.printf("%s\r",array);   
+                mc_usart.printf("%s",array);                  
+                
+                #ifdef DEBUG    
+                    //mc_debug.printf("MOVEMOT %u, %u\r\n", par1, par2);  
+                    mc_usart.printf("MOVEMOT %u, %u\r\n", par1, par2);                                  
+                #endif                 
+            }   
+            else{            
+                invParm = true;
+                #ifdef DEBUG    
+                    mc_debug.printf("movMot parameter fail\r\n");     
+                    mc_usart.printf("movMot parameter fail\r\n");                                
+                #endif
+            }                            
+        }
+/*    
+        if(cmdCnt < COMMANDS_TO_TEST) 
+            cmdCnt++;
+        else
+            cmdCnt=0; 
+*/            
+        //rxMsgPending = true;
+        led1=0;             
+    }           
+    return(invParm);   
+}
+
+
+
+
+

diff -r 333434a8611b -r 0cb065f9d55a writeCommands.h
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/writeCommands.h	Mon May 18 19:04:41 2020 +0000
@@ -0,0 +1,15 @@
+#ifndef WRITECOMMANDS_H
+#define WRITECOMMANDS_H
+
+#include "mbed.h"
+#include <string> 
+
+
+bool moveMot(uint8_t par1, int32_t par2, char *array);
+bool setSpeed(float *n_speed, char *array);
+bool stopMot(uint8_t par1, int32_t par2, char *array);
+bool setTorque(uint16_t newtonMilliMetres, char *array);
+bool setWorkSet(uint16_t par1, char *array);
+bool setWorkSetExt(uint16_t par1, char *array);
+
+#endif
\ No newline at end of file