File content as of revision 0:b3376afd10d8:

#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
    
    flg_timerint_motor = false;
    flg_ival_motor_cw = false;
    flg_ival_motor_ccw = false;
}

// ======================================================================
//  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_m1,       // real speed x 10 for motor1
    int     speed_m2        // real speed x 10 for motor2
){  
    // Winch
    if ( !strncmp( cmd, "XX_DRAM", sizeofcmd ) ) {
        sndCmd2MC( MCTR_CANID_PTORWCH, MOTOR_NO1, speed_m1 );
    }
    else if ( !strncmp(cmd, "XX_CABL", sizeofcmd) ) {
        sndCmd2MC( MCTR_CANID_PTORWCH, MOTOR_NO2, speed_m2 );
    }
    else if ( !strncmp( cmd,"XX_BOTH", sizeofcmd) )
    {
        if(( speed_m1 == 0 )||(speed_m2 == 0))
        {
            if( speed_m1 == 0 )
            {
               sndCmd2MC( MCTR_CANID_PTORWCH, MOTOR_NO1, 0 );  
            }
            if( speed_m2 == 0 )
            {
               sndCmd2MC( MCTR_CANID_PTORWCH, MOTOR_NO2, 0 );  
            }
        }
        else{
            if( flg_ival_motor_cw == true )
            {
                if( move_interval_cw >=0 )
                {
                    sndCmd2MC( MCTR_CANID_PTORWCH, MOTOR_NO1, speed_m1 );        
                    if( flg_timerint_motor == true )
                        sndCmd2MC( MCTR_CANID_PTORWCH, MOTOR_NO2, speed_m2 ); 
                }
                else
                {
                    sndCmd2MC( MCTR_CANID_PTORWCH, MOTOR_NO2, speed_m2 );        
                    if( flg_timerint_motor == true )
                        sndCmd2MC( MCTR_CANID_PTORWCH, MOTOR_NO1, speed_m1 );                     
                }       
            }
            else if ( flg_ival_motor_ccw == true )
            {
                if( move_interval_ccw >=0 )
                {
                    sndCmd2MC( MCTR_CANID_PTORWCH, MOTOR_NO1, speed_m1 );        
                     if( flg_timerint_motor == true )
                        sndCmd2MC( MCTR_CANID_PTORWCH, MOTOR_NO2, speed_m2 ); 
                }
                else
                {
                    sndCmd2MC( MCTR_CANID_PTORWCH, MOTOR_NO2, speed_m2 );        
                    if( flg_timerint_motor == true )
                       sndCmd2MC( MCTR_CANID_PTORWCH, MOTOR_NO1, speed_m1 );                     
                }       
            }
        }
    }

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

  
    return mc_abs_pct;
}