File content as of revision 1:fdf87a1a724b:

#include "mbed.h"
#include "rtos.h"
#include "stdio.h"
#include "common.h"
#include "com_func.h"
#include "mtrAccess.h"

Serial sdc2130(p28, p27);     // Communicate with RpboteQ Driver by tx, rx

Mutex stdio_mutex; 


mtrAccess::mtrAccess()
{
    MCTR_CANID_PTORWCH      = 3;    // PAN,TILT(B1) or Winch(B2) motor ID
    MCTR_CANID_TRANSFORM    = 2;    // TRANSFORM motor ID
    MCTR_CANID_CRAWLER      = 1;    // CRAWLER motor ID
}


// ======================================================================
//  Send Motor command to motor controller
// ======================================================================
bool mtrAccess::sndCmd2MC( 
    int rcan_id,    // RoboCAN Motor controller id 
    int no,         // Motor number (1 or 2)
    int speed       // Motor Speed <-- this is 10x data
){    
    sdc2130.printf( "@%02d!G %d %d\r", rcan_id, no, speed );
    return true;
}


bool mtrAccess::setBaudRate( int baudrate )
{
    sdc2130.baud(baudrate);
    return true;
}

// ======================================================================
//  Get command from client and send to motor controller
// ======================================================================
bool mtrAccess::cmdControl(
    char*   cmd,        // Operationg Command
    int     sizeofcmd,  // Command size
    int     speed       // real speed x 10 : this is from Rst Handy
){  
    // Winch
    if ( !strncmp( cmd,"XX_WICH", sizeofcmd) ) {
        sndCmd2MC( MCTR_CANID_PTORWCH, MOTOR_NO1, speed );
    }
    // Crawler
    else if ( !strncmp( cmd, "XX_CLRF", sizeofcmd ) ) {
        sndCmd2MC( MCTR_CANID_CRAWLER, MOTOR_NO1, speed );
    }
    else if ( !strncmp(cmd, "XX_CLLB", sizeofcmd) ) {
        sndCmd2MC( MCTR_CANID_CRAWLER, MOTOR_NO2, speed );
    }
    // Crawler
    else if ( !strncmp( cmd, "XX_TFRF", sizeofcmd ) ) {
        sndCmd2MC( MCTR_CANID_TRANSFORM, MOTOR_NO1, speed );
    }
    else if ( !strncmp(cmd, "XX_TFLB", sizeofcmd) ) {
        sndCmd2MC( MCTR_CANID_TRANSFORM, MOTOR_NO2, speed );
    }
    // Camera PAN
    else if ( !strncmp(cmd, "XX_CPAN", sizeofcmd) ) {
        sndCmd2MC( MCTR_CANID_PTORWCH, MOTOR_NO2, speed );
    }
    else if ( !strncmp(cmd, "XX_CPAN", sizeofcmd) ) {
        sndCmd2MC( MCTR_CANID_PTORWCH, MOTOR_NO2, speed );
    }
    // Camera PAN
    else if ( !strncmp(cmd, "XX_CTLT", sizeofcmd) ) {
        sndCmd2MC( MCTR_CANID_PTORWCH, MOTOR_NO1, speed );
    }
    else if ( !strncmp(cmd, "XX_CTLT", sizeofcmd) ) {
        sndCmd2MC( MCTR_CANID_PTORWCH, MOTOR_NO1, speed );
    }
    return true;
}

// ============================================================
// Read motor current
// ============================================================
int mtrAccess::readMotorCurrent(
    int motor_id,   // Motor CAN ID 
    int motor_no,   // Motor Number ( 1 or 2 )
    int motor_dir   // Motor direction 
){

    float   motor_current;
    int     i;
    int     mc_abs_pct = 0;
    int     mc_Threshold = 0;

    if( motor_id == MCTR_CANID_PTORWCH )
    {
        if (motor_no == MOTOR_2 ){
            //motor_current = mcnt_panwdm.read()*100.0f;
            mc_abs_pct = abs((int)((motor_current - motor1_current_center_value)*100.0f));
        //    DEBUG_PRINT_L1("Bd0> M1:%lf/%lf= [%03d%%] th:%3.2f< <%3.2f\t", motor_current, motor1_current_center_value, mc_abs_pct, motor1_current_rvs_thd, motor1_current_fwd_thd );

            for( i = 0; i < (mc_abs_pct/10); i++){
            //    DEBUG_PRINT_L1(">");
            }
        //    DEBUG_PRINT_L4("\r\n");
        
            if( motor_dir == MOTOR_FWD ){
                mc_Threshold = (int)motor1_current_fwd_thd;
                //    DEBUG_PRINT_L1("Bd0> Upper threshold: %d\r\n", mc_Threshold);
            }
            else{
                mc_Threshold = (int)motor1_current_rvs_thd;
                DEBUG_PRINT_L1("Bd0> Lower threshold: %d\r\n", mc_Threshold);
            }
            if( mc_abs_pct > mc_Threshold ){
            //    DEBUG_PRINT_L1("Bd0> **** MC1 Over the Limit [%d] ****\r\n", motor1_lock_count );
                motor_pantiltwch_1_lock_count += 1;
                if( motor_pantiltwch_1_lock_count >= MC_LOCK_COUNT ){
                    stdio_mutex.lock();     // Mutex Lock
                    flg_motor_pantiltwch_1_lock = 1;
                    stdio_mutex.unlock();   // Mutex Release
                //    DEBUG_PRINT_L1("Bd0> #### MOTOR1 LOCK ! #### (%d)\r\n", flg_motor_pantiltwch_1_lock);
                }
            }
            else{
            //    DEBUG_PRINT_L1("Bd0> Pass\r\n");
                if( motor_pantiltwch_1_lock_count > 0 ) flg_motor_pantiltwch_1_lock -= 1;
                else motor_pantiltwch_1_lock_count = 0;
            }
        }
    }
    else if( motor_id == MCTR_CANID_TRANSFORM )
    {
    }
    else if( motor_id == MCTR_CANID_CRAWLER )
    {
    }
    
    return mc_abs_pct;
}