z ysaito
/
Pro_B2_2ZNR_HandyCtrler
Important changes to repositories hosted on mbed.com
Mbed hosted mercurial repositories are deprecated and are due to be permanently deleted in July 2026.
To keep a copy of this software download the repository Zip archive or clone locally using Mercurial.
It is also possible to export all your personal repositories from the account settings page.
Dependencies: TextLCD USBDevice mbed-rtos mbed
Embed:
(wiki syntax)
Show/hide line numbers
common.h
00001 /* 00002 DEBUG PRINT MACRO 00003 http://qiita.com/saltheads/items/e1b0ab54d3d6029c9593 00004 http://tricky-code.net/nicecode/code10.php 00005 */ 00006 /* Information */ 00007 #define LatestUpDate "2016.11.04" 00008 #define ProgramRevision "Rev 2.40" 00009 #define Author "ZisNotRevast" 00010 #define Company "Revast Co.,Ltd." 00011 00012 //#define __CREATE_SETTING_FILE__ 00013 //#define __TARGET_BOARD_CHECK__ 00014 00015 00016 //#define __WInchDebug__ // For debugging 00017 00018 00019 00020 // Read motor current 00021 #define __READ_TFM_MOTOR_CURRENT__ 00022 00023 #define __IIC_COMAMND_SEND__ 00024 00025 #define __SET_WINCH_MOTOR_SPEED_BY_VOLUME__ 00026 00027 // ====================================================================== 00028 // For Debugging 00029 // ====================================================================== 00030 //#define __DEBUG_PRINT_SW__ // Display SW Status to console 00031 #define __DEBUG_L0__ 00032 //#define __DEBUG_L1__ 00033 //#define __DEBUG_L2__ 00034 #define __DEBUG_L3__ 00035 //#define __DEBUG_L4__ 00036 //#define __DEBUG_L5__ 00037 00038 #define __DEBUG_WINCH_DATA__ 00039 00040 00041 #ifdef __DEBUG_WINCH_DATA__ 00042 #define DEBUG_PRINT_WINCH_DATA(...) pc.printf(__VA_ARGS__) 00043 #else 00044 #define DEBUG_PRINT_WINCH_DATA(...) 00045 #endif 00046 00047 #ifdef __DEBUG_PRINT_SW__ 00048 #define DEBUG_PRINT_SW(...) pc.printf(__VA_ARGS__) 00049 #else 00050 #define DEBUG_PRINT_SW(...) 00051 #endif 00052 00053 #ifdef __DEBUG_L0__ 00054 #define DEBUG_PRINT_L0(...) pc.printf(__VA_ARGS__) 00055 #else 00056 #define DEBUG_PRINT_L0(...) 00057 #endif 00058 #ifdef __DEBUG_L1__ 00059 #define DEBUG_PRINT_L1(...) pc.printf(__VA_ARGS__) 00060 #else 00061 #define DEBUG_PRINT_L1(...) 00062 #endif 00063 #ifdef __DEBUG_L2__ 00064 #define DEBUG_PRINT_L2(...) pc.printf(__VA_ARGS__) 00065 #else 00066 #define DEBUG_PRINT_L2(...) 00067 #endif 00068 #ifdef __DEBUG_L3__ 00069 #define DEBUG_PRINT_L3(...) pc.printf(__VA_ARGS__) 00070 #else 00071 #define DEBUG_PRINT_L3(...) 00072 #endif 00073 #ifdef __DEBUG_L4__ 00074 #define DEBUG_PRINT_L4(...) pc.printf(__VA_ARGS__) 00075 #else 00076 #define DEBUG_PRINT_L4(...) 00077 #endif 00078 #ifdef __DEBUG_L5__ 00079 #define DEBUG_PRINT_L5(...) pc.printf(__VA_ARGS__) 00080 #else 00081 #define DEBUG_PRINT_L5(...) 00082 #endif 00083 00084 /* ********************************************* */ 00085 00086 // Motor Controller RoboCAN ID definition 00087 #define MCTR_CANID_PANTILTWCH 2 // Pan/Tilt or Winch motor's ID 00088 #define MCTR_CANID_TFM 3 // Trabsform motor's ID 00089 #define MCTR_CANID_CRW 1 // Crawler motor's ID 00090 00091 #define NUMBER_OF_MCCMD 7 00092 #define WINCH_OFFSET_VALUE 20 00093 00094 /* 00095 B1/B2 demo machine controller control command list 00096 */ 00097 enum{ 00098 XX_WICH, // Winch 00099 XX_CLRF, // R Crawler 00100 XX_CLLB, // L Crawler 00101 XX_TFRF, // R Transform 00102 XX_TFLB, // R Transform 00103 ZTRF_I_1, // RF-I Transform ON 00104 ZTRF_I_0, // RF-I Transform OFF 00105 ZTRF_K_1, // RF-K Transform ON 00106 ZTRF_K_0, // RF-K Transform OFF 00107 ZTLB_I_1, // LB-I Transform ON 00108 ZTLB_I_0, // LB-I Transform OFF 00109 ZTLB_K_1, // LB-K Transform ON 00110 ZTLB_K_0, // LB-K Transform OFF 00111 ZCP_FW_1, // Camera PAN Forward ON 00112 ZCP_FW_0, // Camera PAN Forward OFF 00113 ZCP_RS_1, // Camera PAN Reverse ON 00114 ZCP_RS_0, // Camera PAN Reverse OFF 00115 ZCT_FW_1, // Camera TILT Forward ON 00116 ZCT_FW_0, // Camera TILT Forward OFF 00117 ZCT_RS_1, // Camera TILT Reverse ON 00118 ZCT_RS_0, // Camera TILT Reverse OFF 00119 ZESHAPE_I, // Shape I : Switch ON 00120 ZESHAPE_K, // Shape K : Switch OFF 00121 ZTFCWP_VD, // TFM,CRW Part valid : Switch ON 00122 ZWICHP_VD // Winch Part valid : Switch OFF 00123 }; 00124 00125 00126 enum{ 00127 MOTOR_NO0, 00128 MOTOR_NO1, 00129 MOTOR_NO2, 00130 MOTOR_NO3 00131 }; 00132 00133 enum{ 00134 MOTOR_FORWARD, 00135 MOTOR_REVERSE, 00136 MOTOR_STOP 00137 }; 00138 00139 #define MC_LOCK_COUNT 5 00140 00141 /* ********************************************* */ 00142 00143 #define MOTOR_1 '1' 00144 #define MOTOR_2 '2' 00145 00146 #define MOTOR_ 00147 00148 /* Above is still under construction ..... */ 00149 00150 #define __DISP_GAMAPAD_STATUS_ALL__ 00151 #define __DISP_WRITE_VALUE__ 00152 00153 #define I2C_ADDRESS_HANDY 0x20 00154 #define I2C_ADDRESS_WINCH 0x10 00155 #define I2C_ADDRESS_TRANSFORM 0x08 00156 #define I2C_ADDRESS_CRAWLER 0x04 00157 #define I2C_ADDRESS_RESOLVER 0x02 00158 00159 00160 #define _OK_ 1 00161 #define _NG_ 0 00162 #define _FAIL_ 0 00163 00164 #define ON 1 00165 #define OFF 0 00166 00167 #define LFS_READ_COUNT 3 00168 #define TARGET_CHECK_COUNT 10 00169 00170 //#define __DSP_MOTOR_CURRENT_ 00171 00172 /* Usually this should be comment out */ 00173 //#define _COMMUNCATE_PC_BY_SERIAL_ 00174 00175 // Should validate this definition when you use DHCP. 00176 //#define __ETERNET_DHCP__ 00177 00178 #define NumberOfPcCommand 11 00179 #define NumberOfI2CCommand 14 00180 00181 /* I2C Command packet to motor controller */ 00182 /* For motor controller */ 00183 enum{ 00184 I2C_CP_PREAMBLE, // Preamble of command packet 00185 I2C_CP_COMMAND, // instruction command 00186 I2C_CP_M1_DIR, // motor1 rotation direction 00187 I2C_CP_M1_SPEED, // motor1 rotation speed 00188 I2C_CP_M1_FWD_CNTTH_U, // motor1 current limit detection threshold upper byte 00189 I2C_CP_M1_FWD_CNTTH_L, // motor1 current limit detection threshold lower byte 00190 I2C_CP_M1_RVS_CNTTH_U, // motor1 current limit detection threshold upper byte 00191 I2C_CP_M1_RVS_CNTTH_L, // motor1 current limit detection threshold lower byte 00192 I2C_CP_M2_DIR, // motor2 rotation direction 00193 I2C_CP_M2_SPEED, // motor2 rotation speed 00194 I2C_CP_M2_FWD_CNTTH_U, // motor2 current limit detection threshold upper byte 00195 I2C_CP_M2_FWD_CNTTH_L, // motor2 current limit detection threshold lower byte 00196 I2C_CP_M2_RVS_CNTTH_U, // motor2 current limit detection threshold upper byte 00197 I2C_CP_M2_RVS_CNTTH_L, // motor2 current limit detection threshold lower byte 00198 }; 00199 /* For resolver reader controller */ 00200 enum{ 00201 I2C_CP_PREAMBLE_R, // Preamble of command packet 00202 I2C_CP_COMMAND_R, // instruction command 00203 I2C_CP_WDRAM_DIA_UPPER, // motor1 rotation direction 00204 I2C_CP_WDRAM_DIA_LOWER, // motor1 rotation speed 00205 I2C_CP_CCABLE_DIA_UPPER, // motor1 current limit detection threshold upper byte 00206 I2C_CP_CCABLE_DIA_LOWER, // motor1 current limit detection threshold lower byte 00207 I2C_CP_RESOLVER_RESO, // motor1 current limit detection threshold upper byte 00208 I2C_CP_PRESET_CPOS_UPPER, // reserved 00209 I2C_CP_PRESET_CPOS_LOWER, // reserved 00210 I2C_CP_RES3, // reserved 00211 I2C_CP_RES4, // reserved 00212 I2C_CP_RES5, // reserved 00213 I2C_CP_RES6, // reserved 00214 I2C_CP_RES7, // reserved 00215 }; 00216 00217 /* Winch Operating mode */ 00218 enum{ 00219 WINCH_POSITION_CLEAR, 00220 WINCH_PRESET_BASEDATA, 00221 WINCH_MMODE_RELATIVE, 00222 WINCH_MMODE_ABSOLUTE, 00223 WINCH_STEPDOWN_BTN_ON, 00224 WINCH_STEPDOWN_BTN_OFF, 00225 WINCH_STEPUP_BTN_ON, 00226 WINCH_STEPUP_BTN_OFF, 00227 WINCH_U_STEPDOWN_BTN_ON, 00228 WINCH_U_STEPDOWN_BTN_OFF, 00229 WINCH_U_STEPUP_BTN_ON, 00230 WINCH_U_STEPUP_BTN_OFF, 00231 WINCH_PRESET_POSITION, 00232 }; 00233 00234 /* Ether net */ 00235 #define TCP_PCCSERVER_PORT 10004 00236 #define TCP_CMDSERVER_PORT 10002 00237 #define UDP_SERVER_PORT 10000 00238 00239 #define MOTOR_FWD 'F' /* Forward Rotation */ 00240 #define MOTOR_RVS 'R' /* Reverse Rotation */ 00241 #define MOTOR_STP 'S' /* Stop */ 00242 00243 // ========================================================= 00244 // Setting Values 00245 // ========================================================= 00246 typedef struct { 00247 int8_t sv_JS_ShapeMode; // JS Shape: 0=I, 1=KO 00248 int8_t sv_JS_OpeMode; // JS Operation: 0=single, 1=dual 00249 int8_t sv_WinchValid; // Winch function valid or not: 0=Tfm, Crawler valid, 1=Winch valid 00250 int8_t res; 00251 } basic_operation_t; 00252 00253 00254 00255 00256 typedef struct { 00257 uint16_t drm_mtr_ithd_f; // 2 Current threshold winch dram Motor ( forward ) 00258 uint16_t drm_mtr_ithd_r; // 2 Current threshold winch dram Motor ( reverse ) 00259 uint16_t tmp_mtr_ithd_f; // 2 Current threshold winch No2 Motor ( forward ) 00260 uint16_t tmp_mtr_ithd_r; // 2 Current threshold winch No2 Motor ( reverse ) 00261 uint8_t drm_mtr_hspd_f; // 1 Motor speed winch dram motor high speed ( forward ) 00262 uint8_t drm_mtr_hspd_r; // 1 Motor speed winch dram motor high speed ( reverse ) 00263 uint8_t drm_mtr_lspd_f; // 1 Motor speed winch dram motor low speed ( forward ) 00264 uint8_t drm_mtr_lspd_r; // 1 Motor speed winch dram motor low speed ( reverse ) 00265 uint8_t tmp_mtr_hspd_f; // 1 Motor speed winch No2 motor high speed ( forward ) 00266 uint8_t tmp_mtr_hspd_r; // 1 Motor speed winch No2 motor high speed ( reverse ) 00267 uint8_t tmp_mtr_lspd_f; // 1 Motor speed winch No2 motor low speed ( forward ) 00268 uint8_t tmp_mtr_lspd_r; // 1 Motor speed winch No2 motor low speed ( reverse ) 00269 uint16_t dram_dmtr_x100; // 2 Winch dram diameter x 100 00270 uint16_t adj_val_x10000; // 2 Winch adjust value x 100 00271 uint8_t res_resolution; // 1 Winch resolver resolution (bit) 00272 uint8_t reserved_1; // 1 reserved for future use 00273 uint8_t reserved_2; // 1 reserved for future use 00274 uint8_t reserved_3; // 1 reserved for future use 00275 } winch_SetValue_t; // << 24 byte >> 00276 00277 typedef struct { 00278 uint16_t rf_mtr_ithd_f; // 2 Current threshold RF motor ( forward ) 00279 uint16_t rf_mtr_ithd_r; // 2 Current threshold RF motor ( reverse ) 00280 uint16_t lb_mtr_ithd_f; // 2 Current threshold LB motor ( forward ) 00281 uint16_t lb_mtr_ithd_r; // 2 Current threshold LB motor ( reverse ) 00282 uint8_t rf_mtr_hspd_f; // 1 Motor speed RF ( forward ) 00283 uint8_t rf_mtr_hspd_r; // 1 Motor speed RF ( reverse ) 00284 uint8_t lb_mtr_hspd_f; // 1 Motor speed LB ( forward ) 00285 uint8_t lb_mtr_hspd_r; // 1 Motor speed LB ( reverse ) 00286 uint8_t reserved_1; // 1 reserved for future use 00287 uint8_t reserved_2; // 1 reserved for future use 00288 uint8_t reserved_3; // 1 reserved for future use 00289 uint8_t reserved_4; // 1 reserved for future use 00290 } tfm_SetValue_t; // << 16 byte >> 00291 00292 typedef struct { 00293 uint16_t rf_mtr_ithd_f; // 2 Current threshold RF motor ( forward ) 00294 uint16_t rf_mtr_ithd_r; // 2 Current threshold RF motor ( reverse ) 00295 uint16_t lb_mtr_ithd_f; // 2 Current threshold LB motor ( forward ) 00296 uint16_t lb_mtr_ithd_r; // 2 Current threshold LB motor ( reverse ) 00297 uint8_t rf_mtr_hspd_f; // 1 Motor speed RF ( forward ) 00298 uint8_t rf_mtr_hspd_r; // 1 Motor speed RF ( reverse ) 00299 uint8_t lb_mtr_hspd_f; // 1 Motor speed LB ( forward ) 00300 uint8_t lb_mtr_hspd_r; // 1 Motor speed LB ( reverse ) 00301 uint8_t reserved_1; // 1 reserved for future use 00302 uint8_t reserved_2; // 1 reserved for future use 00303 uint8_t reserved_3; // 1 reserved for future use 00304 uint8_t reserved_4; // 1 reserved for future use 00305 } crawler_SetValue_t; // << 16 byte >> 00306 00307 typedef struct { 00308 uint8_t rjs_centervalue; // 1 R JoyStick center value 00309 uint8_t rjs_u_dead_zone; // 1 R Joystick upper dead zone width 00310 uint8_t rjs_l_dead_zone; // 1 R Joystick lower dead zone width 00311 uint8_t ljs_centervalue; // 1 L JoyStick center value 00312 uint8_t ljs_u_dead_zone; // 1 L Joystick upper dead zone width 00313 uint8_t ljs_l_dead_zone; // 1 L Joystick lower dead zone width 00314 uint8_t reserved_1; // 1 reserved for future use 00315 uint8_t reserved_2; // 1 reserved for future use 00316 } js_SetValue_t; // << 8 byte >> 00317 00318 typedef struct SetValue { 00319 winch_SetValue_t winchCtrl; 00320 tfm_SetValue_t tfmCtrl; 00321 crawler_SetValue_t crawlerCtrl; 00322 js_SetValue_t jsCtrl; 00323 } setValue_t; 00324 00325 00326 00327 00328 #define SLOWDOWN_DISTANCE 23 // <--10 2016.06.01 00329 #define SLOWDOWN_NEAR_DISTANCE 3 00330 /* 00331 typedef struct { 00332 int16_t dt_WinchCntPosition; // Current winch posittion 00333 int16_t dt_WinchRtvValue; // Winch Moving distance 00334 int16_t dt_WinchDstPosition; // Destination Position 00335 char operation; 00336 int8_t dt_WinchMotorCurrent; // Winch motor current 00337 } winchData_t; 00338 */ 00339 00340 typedef struct { 00341 int16_t dt_WinchCntPosition; // Current winch posittion 00342 int16_t dt_WinchRtvValue; // Winch Moving distance 00343 int16_t dt_WinchDstPosition; // Destination Position 00344 uint8_t operation; 00345 uint8_t dt_WinchMotor1Current; // Winch motor 1 current 00346 uint8_t dt_WinchMotor2Current; // Winch motor 2 current 00347 uint8_t res; 00348 } winchData_t; 00349 00350 00351 #define ROTATE_PER_RESOLUTION 24 00352 00353 #define __READ_CURRENT_AT_CIF_TASK__ 00354 00355 //mv_WinchMvData
Generated on Sun Jul 31 2022 07:29:17 by
1.7.2