2018.07.26
Dependencies: EthernetInterface TextLCD USBDevice USBHost mbed
mtrAccess.cpp
- Committer:
- sayzyas
- Date:
- 2018-07-26
- Revision:
- 1:fdf87a1a724b
- Parent:
- 0:19075177391c
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; }