Diff: mtrAccess.cpp

Revision:

0:19075177391c

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/mtrAccess.cpp	Thu Jul 26 00:18:49 2018 +0000
@@ -0,0 +1,142 @@
+#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;
+}
\ No newline at end of file