Diff: mtrAccess.cpp

Revision:

0:b3376afd10d8

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