z ysaito / Mbed OS APro_B2DTst_0_Can_Class

2018.07.26

Dependencies:   WebSocketClient

mtrAccess.cpp@0:b3376afd10d8, 2018-07-26 (annotated)

Committer:
sayzyas
Date:
Thu Jul 26 00:20:04 2018 +0000
Revision:
0:b3376afd10d8
2018.07.26

Who changed what in which revision?

UserRevisionLine numberNew contents of line
sayzyas 0:b3376afd10d8 1 #include "mbed.h"
sayzyas 0:b3376afd10d8 2 #include "rtos.h"
sayzyas 0:b3376afd10d8 3 #include "stdio.h"
sayzyas 0:b3376afd10d8 4 #include "common.h"
sayzyas 0:b3376afd10d8 5 //#include "com_func.h"
sayzyas 0:b3376afd10d8 6 #include "mtrAccess.h"
sayzyas 0:b3376afd10d8 7
sayzyas 0:b3376afd10d8 8 Serial sdc2130(p28, p27); // Communicate with RpboteQ Driver by tx, rx
sayzyas 0:b3376afd10d8 9 Mutex stdio_mutex;
sayzyas 0:b3376afd10d8 10
sayzyas 0:b3376afd10d8 11
sayzyas 0:b3376afd10d8 12 mtrAccess::mtrAccess()
sayzyas 0:b3376afd10d8 13 {
sayzyas 0:b3376afd10d8 14 MCTR_CANID_PTORWCH = 3; // PAN,TILT(B1) or Winch(B2) motor ID
sayzyas 0:b3376afd10d8 15 MCTR_CANID_TRANSFORM = 2; // TRANSFORM motor ID
sayzyas 0:b3376afd10d8 16 MCTR_CANID_CRAWLER = 1; // CRAWLER motor ID
sayzyas 0:b3376afd10d8 17
sayzyas 0:b3376afd10d8 18 flg_timerint_motor = false;
sayzyas 0:b3376afd10d8 19 flg_ival_motor_cw = false;
sayzyas 0:b3376afd10d8 20 flg_ival_motor_ccw = false;
sayzyas 0:b3376afd10d8 21 }
sayzyas 0:b3376afd10d8 22
sayzyas 0:b3376afd10d8 23 // ======================================================================
sayzyas 0:b3376afd10d8 24 // Send Motor command to motor controller
sayzyas 0:b3376afd10d8 25 // ======================================================================
sayzyas 0:b3376afd10d8 26 bool mtrAccess::sndCmd2MC(
sayzyas 0:b3376afd10d8 27 int rcan_id, // RoboCAN Motor controller id
sayzyas 0:b3376afd10d8 28 int no, // Motor number (1 or 2)
sayzyas 0:b3376afd10d8 29 int speed // Motor Speed <-- this is 10x data
sayzyas 0:b3376afd10d8 30 ){
sayzyas 0:b3376afd10d8 31 sdc2130.printf( "@%02d!G %d %d\r", rcan_id, no, speed );
sayzyas 0:b3376afd10d8 32 return true;
sayzyas 0:b3376afd10d8 33 }
sayzyas 0:b3376afd10d8 34
sayzyas 0:b3376afd10d8 35
sayzyas 0:b3376afd10d8 36 bool mtrAccess::setBaudRate( int baudrate )
sayzyas 0:b3376afd10d8 37 {
sayzyas 0:b3376afd10d8 38 sdc2130.baud(baudrate);
sayzyas 0:b3376afd10d8 39 return true;
sayzyas 0:b3376afd10d8 40 }
sayzyas 0:b3376afd10d8 41
sayzyas 0:b3376afd10d8 42 // ======================================================================
sayzyas 0:b3376afd10d8 43 // Get command from client and send to motor controller
sayzyas 0:b3376afd10d8 44 // ======================================================================
sayzyas 0:b3376afd10d8 45 bool mtrAccess::cmdControl(
sayzyas 0:b3376afd10d8 46 char* cmd, // Operationg Command
sayzyas 0:b3376afd10d8 47 int sizeofcmd, // Command size
sayzyas 0:b3376afd10d8 48 int speed_m1, // real speed x 10 for motor1
sayzyas 0:b3376afd10d8 49 int speed_m2 // real speed x 10 for motor2
sayzyas 0:b3376afd10d8 50 ){
sayzyas 0:b3376afd10d8 51 // Winch
sayzyas 0:b3376afd10d8 52 if ( !strncmp( cmd, "XX_DRAM", sizeofcmd ) ) {
sayzyas 0:b3376afd10d8 53 sndCmd2MC( MCTR_CANID_PTORWCH, MOTOR_NO1, speed_m1 );
sayzyas 0:b3376afd10d8 54 }
sayzyas 0:b3376afd10d8 55 else if ( !strncmp(cmd, "XX_CABL", sizeofcmd) ) {
sayzyas 0:b3376afd10d8 56 sndCmd2MC( MCTR_CANID_PTORWCH, MOTOR_NO2, speed_m2 );
sayzyas 0:b3376afd10d8 57 }
sayzyas 0:b3376afd10d8 58 else if ( !strncmp( cmd,"XX_BOTH", sizeofcmd) )
sayzyas 0:b3376afd10d8 59 {
sayzyas 0:b3376afd10d8 60 if(( speed_m1 == 0 )||(speed_m2 == 0))
sayzyas 0:b3376afd10d8 61 {
sayzyas 0:b3376afd10d8 62 if( speed_m1 == 0 )
sayzyas 0:b3376afd10d8 63 {
sayzyas 0:b3376afd10d8 64 sndCmd2MC( MCTR_CANID_PTORWCH, MOTOR_NO1, 0 );
sayzyas 0:b3376afd10d8 65 }
sayzyas 0:b3376afd10d8 66 if( speed_m2 == 0 )
sayzyas 0:b3376afd10d8 67 {
sayzyas 0:b3376afd10d8 68 sndCmd2MC( MCTR_CANID_PTORWCH, MOTOR_NO2, 0 );
sayzyas 0:b3376afd10d8 69 }
sayzyas 0:b3376afd10d8 70 }
sayzyas 0:b3376afd10d8 71 else{
sayzyas 0:b3376afd10d8 72 if( flg_ival_motor_cw == true )
sayzyas 0:b3376afd10d8 73 {
sayzyas 0:b3376afd10d8 74 if( move_interval_cw >=0 )
sayzyas 0:b3376afd10d8 75 {
sayzyas 0:b3376afd10d8 76 sndCmd2MC( MCTR_CANID_PTORWCH, MOTOR_NO1, speed_m1 );
sayzyas 0:b3376afd10d8 77 if( flg_timerint_motor == true )
sayzyas 0:b3376afd10d8 78 sndCmd2MC( MCTR_CANID_PTORWCH, MOTOR_NO2, speed_m2 );
sayzyas 0:b3376afd10d8 79 }
sayzyas 0:b3376afd10d8 80 else
sayzyas 0:b3376afd10d8 81 {
sayzyas 0:b3376afd10d8 82 sndCmd2MC( MCTR_CANID_PTORWCH, MOTOR_NO2, speed_m2 );
sayzyas 0:b3376afd10d8 83 if( flg_timerint_motor == true )
sayzyas 0:b3376afd10d8 84 sndCmd2MC( MCTR_CANID_PTORWCH, MOTOR_NO1, speed_m1 );
sayzyas 0:b3376afd10d8 85 }
sayzyas 0:b3376afd10d8 86 }
sayzyas 0:b3376afd10d8 87 else if ( flg_ival_motor_ccw == true )
sayzyas 0:b3376afd10d8 88 {
sayzyas 0:b3376afd10d8 89 if( move_interval_ccw >=0 )
sayzyas 0:b3376afd10d8 90 {
sayzyas 0:b3376afd10d8 91 sndCmd2MC( MCTR_CANID_PTORWCH, MOTOR_NO1, speed_m1 );
sayzyas 0:b3376afd10d8 92 if( flg_timerint_motor == true )
sayzyas 0:b3376afd10d8 93 sndCmd2MC( MCTR_CANID_PTORWCH, MOTOR_NO2, speed_m2 );
sayzyas 0:b3376afd10d8 94 }
sayzyas 0:b3376afd10d8 95 else
sayzyas 0:b3376afd10d8 96 {
sayzyas 0:b3376afd10d8 97 sndCmd2MC( MCTR_CANID_PTORWCH, MOTOR_NO2, speed_m2 );
sayzyas 0:b3376afd10d8 98 if( flg_timerint_motor == true )
sayzyas 0:b3376afd10d8 99 sndCmd2MC( MCTR_CANID_PTORWCH, MOTOR_NO1, speed_m1 );
sayzyas 0:b3376afd10d8 100 }
sayzyas 0:b3376afd10d8 101 }
sayzyas 0:b3376afd10d8 102 }
sayzyas 0:b3376afd10d8 103 }
sayzyas 0:b3376afd10d8 104
sayzyas 0:b3376afd10d8 105 /*
sayzyas 0:b3376afd10d8 106 // Crawler
sayzyas 0:b3376afd10d8 107 else if ( !strncmp( cmd, "XX_CLRF", sizeofcmd ) ) {
sayzyas 0:b3376afd10d8 108 sndCmd2MC( MCTR_CANID_CRAWLER, MOTOR_NO1, speed );
sayzyas 0:b3376afd10d8 109 }
sayzyas 0:b3376afd10d8 110 else if ( !strncmp(cmd, "XX_CLLB", sizeofcmd) ) {
sayzyas 0:b3376afd10d8 111 sndCmd2MC( MCTR_CANID_CRAWLER, MOTOR_NO2, speed );
sayzyas 0:b3376afd10d8 112 }
sayzyas 0:b3376afd10d8 113 // Crawler
sayzyas 0:b3376afd10d8 114 else if ( !strncmp( cmd, "XX_TFRF", sizeofcmd ) ) {
sayzyas 0:b3376afd10d8 115 sndCmd2MC( MCTR_CANID_TRANSFORM, MOTOR_NO1, speed );
sayzyas 0:b3376afd10d8 116 }
sayzyas 0:b3376afd10d8 117 else if ( !strncmp(cmd, "XX_TFLB", sizeofcmd) ) {
sayzyas 0:b3376afd10d8 118 sndCmd2MC( MCTR_CANID_TRANSFORM, MOTOR_NO2, speed );
sayzyas 0:b3376afd10d8 119 }
sayzyas 0:b3376afd10d8 120 // Camera PAN
sayzyas 0:b3376afd10d8 121 else if ( !strncmp(cmd, "XX_CPAN", sizeofcmd) ) {
sayzyas 0:b3376afd10d8 122 sndCmd2MC( MCTR_CANID_PTORWCH, MOTOR_NO2, speed );
sayzyas 0:b3376afd10d8 123 }
sayzyas 0:b3376afd10d8 124 else if ( !strncmp(cmd, "XX_CPAN", sizeofcmd) ) {
sayzyas 0:b3376afd10d8 125 sndCmd2MC( MCTR_CANID_PTORWCH, MOTOR_NO2, speed );
sayzyas 0:b3376afd10d8 126 }
sayzyas 0:b3376afd10d8 127 // Camera PAN
sayzyas 0:b3376afd10d8 128 else if ( !strncmp(cmd, "XX_CTLT", sizeofcmd) ) {
sayzyas 0:b3376afd10d8 129 sndCmd2MC( MCTR_CANID_PTORWCH, MOTOR_NO1, speed );
sayzyas 0:b3376afd10d8 130 }
sayzyas 0:b3376afd10d8 131 else if ( !strncmp(cmd, "XX_CTLT", sizeofcmd) ) {
sayzyas 0:b3376afd10d8 132 sndCmd2MC( MCTR_CANID_PTORWCH, MOTOR_NO1, speed );
sayzyas 0:b3376afd10d8 133 }
sayzyas 0:b3376afd10d8 134 */
sayzyas 0:b3376afd10d8 135 return true;
sayzyas 0:b3376afd10d8 136 }
sayzyas 0:b3376afd10d8 137
sayzyas 0:b3376afd10d8 138 // ============================================================
sayzyas 0:b3376afd10d8 139 // Read motor current
sayzyas 0:b3376afd10d8 140 // ============================================================
sayzyas 0:b3376afd10d8 141 int mtrAccess::readMotorCurrent(
sayzyas 0:b3376afd10d8 142 int motor_id, // Motor CAN ID
sayzyas 0:b3376afd10d8 143 int motor_no, // Motor Number ( 1 or 2 )
sayzyas 0:b3376afd10d8 144 int motor_dir // Motor direction
sayzyas 0:b3376afd10d8 145 ){
sayzyas 0:b3376afd10d8 146
sayzyas 0:b3376afd10d8 147 float motor_current;
sayzyas 0:b3376afd10d8 148 int i;
sayzyas 0:b3376afd10d8 149 int mc_abs_pct = 0;
sayzyas 0:b3376afd10d8 150 int mc_Threshold = 0;
sayzyas 0:b3376afd10d8 151
sayzyas 0:b3376afd10d8 152
sayzyas 0:b3376afd10d8 153 return mc_abs_pct;
sayzyas 0:b3376afd10d8 154 }