File content as of revision 1:0cb065f9d55a:

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