z ysaito
2018.07.26
Dependencies: TextLCD USBDevice mbed-rtos mbed
Diff: common.h
- Revision:
- 2:d0a5ee4e7bb8
- Parent:
- 1:368ba89c2e6b
diff -r 368ba89c2e6b -r d0a5ee4e7bb8 common.h
--- a/common.h Mon Mar 28 00:09:18 2016 +0000
+++ b/common.h Thu Jul 26 00:20:57 2018 +0000
@@ -1,17 +1,54 @@
-
+/*
+ DEBUG PRINT MACRO
+ http://qiita.com/saltheads/items/e1b0ab54d3d6029c9593
+ http://tricky-code.net/nicecode/code10.php
+*/
/* Information */
-#define LatestUpDate "2016.02.17"
-#define ProgramRevision "Rev. 0.95"
-#define Author "Y.Saito"
+#define LatestUpDate "2016.11.04"
+#define ProgramRevision "Rev 2.40"
+#define Author "ZisNotRevast"
#define Company "Revast Co.,Ltd."
-#define Targetmbed "mbed LPC1768"
+
+//#define __CREATE_SETTING_FILE__
+//#define __TARGET_BOARD_CHECK__
+
+
+//#define __WInchDebug__ // For debugging
+
+
-/* Debug macro */
+// Read motor current
+#define __READ_TFM_MOTOR_CURRENT__
+
+#define __IIC_COMAMND_SEND__
+
+#define __SET_WINCH_MOTOR_SPEED_BY_VOLUME__
+
+// ======================================================================
+// For Debugging
+// ======================================================================
+//#define __DEBUG_PRINT_SW__ // Display SW Status to console
#define __DEBUG_L0__
-#define __DEBUG_L1__
-#define __DEBUG_L2__
-//#define __DEBUG_L3__
+//#define __DEBUG_L1__
+//#define __DEBUG_L2__
+#define __DEBUG_L3__
//#define __DEBUG_L4__
+//#define __DEBUG_L5__
+
+#define __DEBUG_WINCH_DATA__
+
+
+#ifdef __DEBUG_WINCH_DATA__
+ #define DEBUG_PRINT_WINCH_DATA(...) pc.printf(__VA_ARGS__)
+#else
+ #define DEBUG_PRINT_WINCH_DATA(...)
+#endif
+
+#ifdef __DEBUG_PRINT_SW__
+ #define DEBUG_PRINT_SW(...) pc.printf(__VA_ARGS__)
+#else
+ #define DEBUG_PRINT_SW(...)
+#endif
#ifdef __DEBUG_L0__
#define DEBUG_PRINT_L0(...) pc.printf(__VA_ARGS__)
@@ -38,24 +75,281 @@
#else
#define DEBUG_PRINT_L4(...)
#endif
+#ifdef __DEBUG_L5__
+ #define DEBUG_PRINT_L5(...) pc.printf(__VA_ARGS__)
+#else
+ #define DEBUG_PRINT_L5(...)
+#endif
-/* ***************** */
-/* Target definition */
-/* ***************** */
-#define i2c_addr_handy (0x50); // Handy Controller
-#define i2c_addr_winch (0x40); // Ctrl Board1 : Winch
-#define i2c_addr_tfansform (0x30); // Ctrl Board2 : Transform
-#define i2c_addr_crawler (0x20); // Ctrl Board3 : Crawler
+/* ********************************************* */
+
+// Motor Controller RoboCAN ID definition
+#define MCTR_CANID_PANTILTWCH 2 // Pan/Tilt or Winch motor's ID
+#define MCTR_CANID_TFM 3 // Trabsform motor's ID
+#define MCTR_CANID_CRW 1 // Crawler motor's ID
+
+#define NUMBER_OF_MCCMD 7
+#define WINCH_OFFSET_VALUE 20
+
+/*
+ B1/B2 demo machine controller control command list
+*/
+enum{
+ XX_WICH, // Winch
+ XX_CLRF, // R Crawler
+ XX_CLLB, // L Crawler
+ XX_TFRF, // R Transform
+ XX_TFLB, // R Transform
+ ZTRF_I_1, // RF-I Transform ON
+ ZTRF_I_0, // RF-I Transform OFF
+ ZTRF_K_1, // RF-K Transform ON
+ ZTRF_K_0, // RF-K Transform OFF
+ ZTLB_I_1, // LB-I Transform ON
+ ZTLB_I_0, // LB-I Transform OFF
+ ZTLB_K_1, // LB-K Transform ON
+ ZTLB_K_0, // LB-K Transform OFF
+ ZCP_FW_1, // Camera PAN Forward ON
+ ZCP_FW_0, // Camera PAN Forward OFF
+ ZCP_RS_1, // Camera PAN Reverse ON
+ ZCP_RS_0, // Camera PAN Reverse OFF
+ ZCT_FW_1, // Camera TILT Forward ON
+ ZCT_FW_0, // Camera TILT Forward OFF
+ ZCT_RS_1, // Camera TILT Reverse ON
+ ZCT_RS_0, // Camera TILT Reverse OFF
+ ZESHAPE_I, // Shape I : Switch ON
+ ZESHAPE_K, // Shape K : Switch OFF
+ ZTFCWP_VD, // TFM,CRW Part valid : Switch ON
+ ZWICHP_VD // Winch Part valid : Switch OFF
+};
+
+
+enum{
+ MOTOR_NO0,
+ MOTOR_NO1,
+ MOTOR_NO2,
+ MOTOR_NO3
+};
+
+enum{
+ MOTOR_FORWARD,
+ MOTOR_REVERSE,
+ MOTOR_STOP
+};
+
+#define MC_LOCK_COUNT 5
+
+/* ********************************************* */
+
+#define MOTOR_1 '1'
+#define MOTOR_2 '2'
+
+#define MOTOR_
+
+/* Above is still under construction ..... */
+
+#define __DISP_GAMAPAD_STATUS_ALL__
+#define __DISP_WRITE_VALUE__
+
+#define I2C_ADDRESS_HANDY 0x20
+#define I2C_ADDRESS_WINCH 0x10
+#define I2C_ADDRESS_TRANSFORM 0x08
+#define I2C_ADDRESS_CRAWLER 0x04
+#define I2C_ADDRESS_RESOLVER 0x02
+
+
+#define _OK_ 1
+#define _NG_ 0
+#define _FAIL_ 0
+
+#define ON 1
+#define OFF 0
+
+#define LFS_READ_COUNT 3
+#define TARGET_CHECK_COUNT 10
+
+//#define __DSP_MOTOR_CURRENT_
+
+/* Usually this should be comment out */
+//#define _COMMUNCATE_PC_BY_SERIAL_
+
+// Should validate this definition when you use DHCP.
+//#define __ETERNET_DHCP__
+
+#define NumberOfPcCommand 11
+#define NumberOfI2CCommand 14
+
+/* I2C Command packet to motor controller */
+/* For motor controller */
+enum{
+ I2C_CP_PREAMBLE, // Preamble of command packet
+ I2C_CP_COMMAND, // instruction command
+ I2C_CP_M1_DIR, // motor1 rotation direction
+ I2C_CP_M1_SPEED, // motor1 rotation speed
+ I2C_CP_M1_FWD_CNTTH_U, // motor1 current limit detection threshold upper byte
+ I2C_CP_M1_FWD_CNTTH_L, // motor1 current limit detection threshold lower byte
+ I2C_CP_M1_RVS_CNTTH_U, // motor1 current limit detection threshold upper byte
+ I2C_CP_M1_RVS_CNTTH_L, // motor1 current limit detection threshold lower byte
+ I2C_CP_M2_DIR, // motor2 rotation direction
+ I2C_CP_M2_SPEED, // motor2 rotation speed
+ I2C_CP_M2_FWD_CNTTH_U, // motor2 current limit detection threshold upper byte
+ I2C_CP_M2_FWD_CNTTH_L, // motor2 current limit detection threshold lower byte
+ I2C_CP_M2_RVS_CNTTH_U, // motor2 current limit detection threshold upper byte
+ I2C_CP_M2_RVS_CNTTH_L, // motor2 current limit detection threshold lower byte
+};
+/* For resolver reader controller */
+enum{
+ I2C_CP_PREAMBLE_R, // Preamble of command packet
+ I2C_CP_COMMAND_R, // instruction command
+ I2C_CP_WDRAM_DIA_UPPER, // motor1 rotation direction
+ I2C_CP_WDRAM_DIA_LOWER, // motor1 rotation speed
+ I2C_CP_CCABLE_DIA_UPPER, // motor1 current limit detection threshold upper byte
+ I2C_CP_CCABLE_DIA_LOWER, // motor1 current limit detection threshold lower byte
+ I2C_CP_RESOLVER_RESO, // motor1 current limit detection threshold upper byte
+ I2C_CP_PRESET_CPOS_UPPER, // reserved
+ I2C_CP_PRESET_CPOS_LOWER, // reserved
+ I2C_CP_RES3, // reserved
+ I2C_CP_RES4, // reserved
+ I2C_CP_RES5, // reserved
+ I2C_CP_RES6, // reserved
+ I2C_CP_RES7, // reserved
+};
-#define Target_IIC_ADDR i2c_addr_handy // For Winch motor controller
-//#define Target_IIC_ADDR i2c_addr_tfansform // For Transform controller
-//#define Target_IIC_ADDR i2c_addr_crawler // For Crawler motor controller
+/* Winch Operating mode */
+enum{
+ WINCH_POSITION_CLEAR,
+ WINCH_PRESET_BASEDATA,
+ WINCH_MMODE_RELATIVE,
+ WINCH_MMODE_ABSOLUTE,
+ WINCH_STEPDOWN_BTN_ON,
+ WINCH_STEPDOWN_BTN_OFF,
+ WINCH_STEPUP_BTN_ON,
+ WINCH_STEPUP_BTN_OFF,
+ WINCH_U_STEPDOWN_BTN_ON,
+ WINCH_U_STEPDOWN_BTN_OFF,
+ WINCH_U_STEPUP_BTN_ON,
+ WINCH_U_STEPUP_BTN_OFF,
+ WINCH_PRESET_POSITION,
+};
+
+/* Ether net */
+#define TCP_PCCSERVER_PORT 10004
+#define TCP_CMDSERVER_PORT 10002
+#define UDP_SERVER_PORT 10000
+
+#define MOTOR_FWD 'F' /* Forward Rotation */
+#define MOTOR_RVS 'R' /* Reverse Rotation */
+#define MOTOR_STP 'S' /* Stop */
-#define NumberOfI2CCommand 10
+// =========================================================
+// Setting Values
+// =========================================================
+typedef struct {
+ int8_t sv_JS_ShapeMode; // JS Shape: 0=I, 1=KO
+ int8_t sv_JS_OpeMode; // JS Operation: 0=single, 1=dual
+ int8_t sv_WinchValid; // Winch function valid or not: 0=Tfm, Crawler valid, 1=Winch valid
+ int8_t res;
+} basic_operation_t;
+
+
+
+
+typedef struct {
+ uint16_t drm_mtr_ithd_f; // 2 Current threshold winch dram Motor ( forward )
+ uint16_t drm_mtr_ithd_r; // 2 Current threshold winch dram Motor ( reverse )
+ uint16_t tmp_mtr_ithd_f; // 2 Current threshold winch No2 Motor ( forward )
+ uint16_t tmp_mtr_ithd_r; // 2 Current threshold winch No2 Motor ( reverse )
+ uint8_t drm_mtr_hspd_f; // 1 Motor speed winch dram motor high speed ( forward )
+ uint8_t drm_mtr_hspd_r; // 1 Motor speed winch dram motor high speed ( reverse )
+ uint8_t drm_mtr_lspd_f; // 1 Motor speed winch dram motor low speed ( forward )
+ uint8_t drm_mtr_lspd_r; // 1 Motor speed winch dram motor low speed ( reverse )
+ uint8_t tmp_mtr_hspd_f; // 1 Motor speed winch No2 motor high speed ( forward )
+ uint8_t tmp_mtr_hspd_r; // 1 Motor speed winch No2 motor high speed ( reverse )
+ uint8_t tmp_mtr_lspd_f; // 1 Motor speed winch No2 motor low speed ( forward )
+ uint8_t tmp_mtr_lspd_r; // 1 Motor speed winch No2 motor low speed ( reverse )
+ uint16_t dram_dmtr_x100; // 2 Winch dram diameter x 100
+ uint16_t adj_val_x10000; // 2 Winch adjust value x 100
+ uint8_t res_resolution; // 1 Winch resolver resolution (bit)
+ uint8_t reserved_1; // 1 reserved for future use
+ uint8_t reserved_2; // 1 reserved for future use
+ uint8_t reserved_3; // 1 reserved for future use
+} winch_SetValue_t; // << 24 byte >>
-#define DEAD_ZONE_BAND_WIDTH 5
-#define CALIBRATION_COUNT 300
+typedef struct {
+ uint16_t rf_mtr_ithd_f; // 2 Current threshold RF motor ( forward )
+ uint16_t rf_mtr_ithd_r; // 2 Current threshold RF motor ( reverse )
+ uint16_t lb_mtr_ithd_f; // 2 Current threshold LB motor ( forward )
+ uint16_t lb_mtr_ithd_r; // 2 Current threshold LB motor ( reverse )
+ uint8_t rf_mtr_hspd_f; // 1 Motor speed RF ( forward )
+ uint8_t rf_mtr_hspd_r; // 1 Motor speed RF ( reverse )
+ uint8_t lb_mtr_hspd_f; // 1 Motor speed LB ( forward )
+ uint8_t lb_mtr_hspd_r; // 1 Motor speed LB ( reverse )
+ uint8_t reserved_1; // 1 reserved for future use
+ uint8_t reserved_2; // 1 reserved for future use
+ uint8_t reserved_3; // 1 reserved for future use
+ uint8_t reserved_4; // 1 reserved for future use
+} tfm_SetValue_t; // << 16 byte >>
+
+typedef struct {
+ uint16_t rf_mtr_ithd_f; // 2 Current threshold RF motor ( forward )
+ uint16_t rf_mtr_ithd_r; // 2 Current threshold RF motor ( reverse )
+ uint16_t lb_mtr_ithd_f; // 2 Current threshold LB motor ( forward )
+ uint16_t lb_mtr_ithd_r; // 2 Current threshold LB motor ( reverse )
+ uint8_t rf_mtr_hspd_f; // 1 Motor speed RF ( forward )
+ uint8_t rf_mtr_hspd_r; // 1 Motor speed RF ( reverse )
+ uint8_t lb_mtr_hspd_f; // 1 Motor speed LB ( forward )
+ uint8_t lb_mtr_hspd_r; // 1 Motor speed LB ( reverse )
+ uint8_t reserved_1; // 1 reserved for future use
+ uint8_t reserved_2; // 1 reserved for future use
+ uint8_t reserved_3; // 1 reserved for future use
+ uint8_t reserved_4; // 1 reserved for future use
+} crawler_SetValue_t; // << 16 byte >>
-#define MOTOR_FWD_ROTATION 0
-#define MOTOR_RVS_ROTATION 1
+typedef struct {
+ uint8_t rjs_centervalue; // 1 R JoyStick center value
+ uint8_t rjs_u_dead_zone; // 1 R Joystick upper dead zone width
+ uint8_t rjs_l_dead_zone; // 1 R Joystick lower dead zone width
+ uint8_t ljs_centervalue; // 1 L JoyStick center value
+ uint8_t ljs_u_dead_zone; // 1 L Joystick upper dead zone width
+ uint8_t ljs_l_dead_zone; // 1 L Joystick lower dead zone width
+ uint8_t reserved_1; // 1 reserved for future use
+ uint8_t reserved_2; // 1 reserved for future use
+} js_SetValue_t; // << 8 byte >>
+
+typedef struct SetValue {
+ winch_SetValue_t winchCtrl;
+ tfm_SetValue_t tfmCtrl;
+ crawler_SetValue_t crawlerCtrl;
+ js_SetValue_t jsCtrl;
+} setValue_t;
+
+
+
+#define SLOWDOWN_DISTANCE 23 // <--10 2016.06.01
+#define SLOWDOWN_NEAR_DISTANCE 3
+/*
+typedef struct {
+ int16_t dt_WinchCntPosition; // Current winch posittion
+ int16_t dt_WinchRtvValue; // Winch Moving distance
+ int16_t dt_WinchDstPosition; // Destination Position
+ char operation;
+ int8_t dt_WinchMotorCurrent; // Winch motor current
+} winchData_t;
+*/
+
+typedef struct {
+ int16_t dt_WinchCntPosition; // Current winch posittion
+ int16_t dt_WinchRtvValue; // Winch Moving distance
+ int16_t dt_WinchDstPosition; // Destination Position
+ uint8_t operation;
+ uint8_t dt_WinchMotor1Current; // Winch motor 1 current
+ uint8_t dt_WinchMotor2Current; // Winch motor 2 current
+ uint8_t res;
+} winchData_t;
+
+
+#define ROTATE_PER_RESOLUTION 24
+
+#define __READ_CURRENT_AT_CIF_TASK__
+
+//mv_WinchMvData