z ysaito / Mbed 2 deprecated CPro_DemoB1B2_0_Can_Class

2018.07.26

Dependencies:   EthernetInterface TextLCD USBDevice USBHost mbed

mtrAccess.cpp@1:fdf87a1a724b, 2018-07-26 (annotated)

Committer:
sayzyas
Date:
Thu Jul 26 00:26:07 2018 +0000
Revision:
1:fdf87a1a724b
Parent:
0:19075177391c 
2018.07.26

Who changed what in which revision?

UserRevisionLine numberNew contents of line
sayzyas 0:19075177391c 1 #include "mbed.h"
sayzyas 0:19075177391c 2 #include "rtos.h"
sayzyas 0:19075177391c 3 #include "stdio.h"
sayzyas 0:19075177391c 4 #include "common.h"
sayzyas 0:19075177391c 5 #include "com_func.h"
sayzyas 0:19075177391c 6 #include "mtrAccess.h"
sayzyas 0:19075177391c 7
sayzyas 0:19075177391c 8 Serial sdc2130(p28, p27); // Communicate with RpboteQ Driver by tx, rx
sayzyas 0:19075177391c 9
sayzyas 0:19075177391c 10 Mutex stdio_mutex;
sayzyas 0:19075177391c 11
sayzyas 0:19075177391c 12
sayzyas 0:19075177391c 13 mtrAccess::mtrAccess()
sayzyas 0:19075177391c 14 {
sayzyas 0:19075177391c 15 MCTR_CANID_PTORWCH = 3; // PAN,TILT(B1) or Winch(B2) motor ID
sayzyas 0:19075177391c 16 MCTR_CANID_TRANSFORM = 2; // TRANSFORM motor ID
sayzyas 0:19075177391c 17 MCTR_CANID_CRAWLER = 1; // CRAWLER motor ID
sayzyas 0:19075177391c 18 }
sayzyas 0:19075177391c 19
sayzyas 0:19075177391c 20
sayzyas 0:19075177391c 21 // ======================================================================
sayzyas 0:19075177391c 22 // Send Motor command to motor controller
sayzyas 0:19075177391c 23 // ======================================================================
sayzyas 0:19075177391c 24 bool mtrAccess::sndCmd2MC(
sayzyas 0:19075177391c 25 int rcan_id, // RoboCAN Motor controller id
sayzyas 0:19075177391c 26 int no, // Motor number (1 or 2)
sayzyas 0:19075177391c 27 int speed // Motor Speed <-- this is 10x data
sayzyas 0:19075177391c 28 ){
sayzyas 0:19075177391c 29 sdc2130.printf( "@%02d!G %d %d\r", rcan_id, no, speed );
sayzyas 0:19075177391c 30 return true;
sayzyas 0:19075177391c 31 }
sayzyas 0:19075177391c 32
sayzyas 0:19075177391c 33
sayzyas 0:19075177391c 34 bool mtrAccess::setBaudRate( int baudrate )
sayzyas 0:19075177391c 35 {
sayzyas 0:19075177391c 36 sdc2130.baud(baudrate);
sayzyas 0:19075177391c 37 return true;
sayzyas 0:19075177391c 38 }
sayzyas 0:19075177391c 39
sayzyas 0:19075177391c 40 // ======================================================================
sayzyas 0:19075177391c 41 // Get command from client and send to motor controller
sayzyas 0:19075177391c 42 // ======================================================================
sayzyas 0:19075177391c 43 bool mtrAccess::cmdControl(
sayzyas 0:19075177391c 44 char* cmd, // Operationg Command
sayzyas 0:19075177391c 45 int sizeofcmd, // Command size
sayzyas 0:19075177391c 46 int speed // real speed x 10 : this is from Rst Handy
sayzyas 0:19075177391c 47 ){
sayzyas 0:19075177391c 48 // Winch
sayzyas 0:19075177391c 49 if ( !strncmp( cmd,"XX_WICH", sizeofcmd) ) {
sayzyas 0:19075177391c 50 sndCmd2MC( MCTR_CANID_PTORWCH, MOTOR_NO1, speed );
sayzyas 0:19075177391c 51 }
sayzyas 0:19075177391c 52 // Crawler
sayzyas 0:19075177391c 53 else if ( !strncmp( cmd, "XX_CLRF", sizeofcmd ) ) {
sayzyas 0:19075177391c 54 sndCmd2MC( MCTR_CANID_CRAWLER, MOTOR_NO1, speed );
sayzyas 0:19075177391c 55 }
sayzyas 0:19075177391c 56 else if ( !strncmp(cmd, "XX_CLLB", sizeofcmd) ) {
sayzyas 0:19075177391c 57 sndCmd2MC( MCTR_CANID_CRAWLER, MOTOR_NO2, speed );
sayzyas 0:19075177391c 58 }
sayzyas 0:19075177391c 59 // Crawler
sayzyas 0:19075177391c 60 else if ( !strncmp( cmd, "XX_TFRF", sizeofcmd ) ) {
sayzyas 0:19075177391c 61 sndCmd2MC( MCTR_CANID_TRANSFORM, MOTOR_NO1, speed );
sayzyas 0:19075177391c 62 }
sayzyas 0:19075177391c 63 else if ( !strncmp(cmd, "XX_TFLB", sizeofcmd) ) {
sayzyas 0:19075177391c 64 sndCmd2MC( MCTR_CANID_TRANSFORM, MOTOR_NO2, speed );
sayzyas 0:19075177391c 65 }
sayzyas 0:19075177391c 66 // Camera PAN
sayzyas 0:19075177391c 67 else if ( !strncmp(cmd, "XX_CPAN", sizeofcmd) ) {
sayzyas 0:19075177391c 68 sndCmd2MC( MCTR_CANID_PTORWCH, MOTOR_NO2, speed );
sayzyas 0:19075177391c 69 }
sayzyas 0:19075177391c 70 else if ( !strncmp(cmd, "XX_CPAN", sizeofcmd) ) {
sayzyas 0:19075177391c 71 sndCmd2MC( MCTR_CANID_PTORWCH, MOTOR_NO2, speed );
sayzyas 0:19075177391c 72 }
sayzyas 0:19075177391c 73 // Camera PAN
sayzyas 0:19075177391c 74 else if ( !strncmp(cmd, "XX_CTLT", sizeofcmd) ) {
sayzyas 0:19075177391c 75 sndCmd2MC( MCTR_CANID_PTORWCH, MOTOR_NO1, speed );
sayzyas 0:19075177391c 76 }
sayzyas 0:19075177391c 77 else if ( !strncmp(cmd, "XX_CTLT", sizeofcmd) ) {
sayzyas 0:19075177391c 78 sndCmd2MC( MCTR_CANID_PTORWCH, MOTOR_NO1, speed );
sayzyas 0:19075177391c 79 }
sayzyas 0:19075177391c 80 return true;
sayzyas 0:19075177391c 81 }
sayzyas 0:19075177391c 82
sayzyas 0:19075177391c 83 // ============================================================
sayzyas 0:19075177391c 84 // Read motor current
sayzyas 0:19075177391c 85 // ============================================================
sayzyas 0:19075177391c 86 int mtrAccess::readMotorCurrent(
sayzyas 0:19075177391c 87 int motor_id, // Motor CAN ID
sayzyas 0:19075177391c 88 int motor_no, // Motor Number ( 1 or 2 )
sayzyas 0:19075177391c 89 int motor_dir // Motor direction
sayzyas 0:19075177391c 90 ){
sayzyas 0:19075177391c 91
sayzyas 0:19075177391c 92 float motor_current;
sayzyas 0:19075177391c 93 int i;
sayzyas 0:19075177391c 94 int mc_abs_pct = 0;
sayzyas 0:19075177391c 95 int mc_Threshold = 0;
sayzyas 0:19075177391c 96
sayzyas 0:19075177391c 97 if( motor_id == MCTR_CANID_PTORWCH )
sayzyas 0:19075177391c 98 {
sayzyas 0:19075177391c 99 if (motor_no == MOTOR_2 ){
sayzyas 0:19075177391c 100 //motor_current = mcnt_panwdm.read()*100.0f;
sayzyas 0:19075177391c 101 mc_abs_pct = abs((int)((motor_current - motor1_current_center_value)*100.0f));
sayzyas 0:19075177391c 102 // 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 );
sayzyas 0:19075177391c 103
sayzyas 0:19075177391c 104 for( i = 0; i < (mc_abs_pct/10); i++){
sayzyas 0:19075177391c 105 // DEBUG_PRINT_L1(">");
sayzyas 0:19075177391c 106 }
sayzyas 0:19075177391c 107 // DEBUG_PRINT_L4("\r\n");
sayzyas 0:19075177391c 108
sayzyas 0:19075177391c 109 if( motor_dir == MOTOR_FWD ){
sayzyas 0:19075177391c 110 mc_Threshold = (int)motor1_current_fwd_thd;
sayzyas 0:19075177391c 111 // DEBUG_PRINT_L1("Bd0> Upper threshold: %d\r\n", mc_Threshold);
sayzyas 0:19075177391c 112 }
sayzyas 0:19075177391c 113 else{
sayzyas 0:19075177391c 114 mc_Threshold = (int)motor1_current_rvs_thd;
sayzyas 0:19075177391c 115 DEBUG_PRINT_L1("Bd0> Lower threshold: %d\r\n", mc_Threshold);
sayzyas 0:19075177391c 116 }
sayzyas 0:19075177391c 117 if( mc_abs_pct > mc_Threshold ){
sayzyas 0:19075177391c 118 // DEBUG_PRINT_L1("Bd0> **** MC1 Over the Limit [%d] ****\r\n", motor1_lock_count );
sayzyas 0:19075177391c 119 motor_pantiltwch_1_lock_count += 1;
sayzyas 0:19075177391c 120 if( motor_pantiltwch_1_lock_count >= MC_LOCK_COUNT ){
sayzyas 0:19075177391c 121 stdio_mutex.lock(); // Mutex Lock
sayzyas 0:19075177391c 122 flg_motor_pantiltwch_1_lock = 1;
sayzyas 0:19075177391c 123 stdio_mutex.unlock(); // Mutex Release
sayzyas 0:19075177391c 124 // DEBUG_PRINT_L1("Bd0> #### MOTOR1 LOCK ! #### (%d)\r\n", flg_motor_pantiltwch_1_lock);
sayzyas 0:19075177391c 125 }
sayzyas 0:19075177391c 126 }
sayzyas 0:19075177391c 127 else{
sayzyas 0:19075177391c 128 // DEBUG_PRINT_L1("Bd0> Pass\r\n");
sayzyas 0:19075177391c 129 if( motor_pantiltwch_1_lock_count > 0 ) flg_motor_pantiltwch_1_lock -= 1;
sayzyas 0:19075177391c 130 else motor_pantiltwch_1_lock_count = 0;
sayzyas 0:19075177391c 131 }
sayzyas 0:19075177391c 132 }
sayzyas 0:19075177391c 133 }
sayzyas 0:19075177391c 134 else if( motor_id == MCTR_CANID_TRANSFORM )
sayzyas 0:19075177391c 135 {
sayzyas 0:19075177391c 136 }
sayzyas 0:19075177391c 137 else if( motor_id == MCTR_CANID_CRAWLER )
sayzyas 0:19075177391c 138 {
sayzyas 0:19075177391c 139 }
sayzyas 0:19075177391c 140
sayzyas 0:19075177391c 141 return mc_abs_pct;
sayzyas 0:19075177391c 142 }