File content as of revision 1:0cb065f9d55a:

/*

EOL SEQUENCE = NONE ON YAT, PROTOCOL WILL FAIL IF THIS IS NOT DONE

*/




#include <stdio.h>
#include "mbed.h"
#include "main.h"
#include "MotorControl.h"
#include<string>

class motorControl{
    
    private:
    
    public:
    
    protected:
    
};

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);

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 
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

char motData[DATA_ITEMS][DATA_BUFFER_SIZE];//data only

////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
// MOTOR ISR FUNCTIONS
////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////

void motMsg_RX_ISR(void){
    
    mc_Rx_Buffer[mc_Rx_Wr_Ptr] = mc_debug.getc();

    mc_Rx_Wr_Ptr++;
    count++;

    if(mc_Rx_Wr_Ptr == (BUFFER_SIZE - 1))
        mc_Rx_Wr_Ptr = 0;
        
}

////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
// 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

        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{
    
            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
                
                #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);
} 


////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
// 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 
    
    *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__); 
    
          
/*    
    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);
}