File content as of revision 2:d0a5ee4e7bb8:

/* 
   DEBUG PRINT MACRO
   http://qiita.com/saltheads/items/e1b0ab54d3d6029c9593
   http://tricky-code.net/nicecode/code10.php
*/
/* Information */
#define LatestUpDate    "2016.11.04"
#define ProgramRevision "Rev 2.40" 
#define Author          "ZisNotRevast"
#define Company         "Revast Co.,Ltd."

//#define __CREATE_SETTING_FILE__
//#define __TARGET_BOARD_CHECK__


//#define __WInchDebug__  // For debugging 



// 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_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__)
#else
    #define DEBUG_PRINT_L0(...) 
#endif
#ifdef __DEBUG_L1__
    #define DEBUG_PRINT_L1(...)  pc.printf(__VA_ARGS__)
#else
    #define DEBUG_PRINT_L1(...) 
#endif
#ifdef __DEBUG_L2__
    #define DEBUG_PRINT_L2(...)  pc.printf(__VA_ARGS__)
#else
    #define DEBUG_PRINT_L2(...) 
#endif
#ifdef __DEBUG_L3__
    #define DEBUG_PRINT_L3(...)  pc.printf(__VA_ARGS__)
#else
    #define DEBUG_PRINT_L3(...) 
#endif
#ifdef __DEBUG_L4__
    #define DEBUG_PRINT_L4(...)  pc.printf(__VA_ARGS__)
#else
    #define DEBUG_PRINT_L4(...) 
#endif
#ifdef __DEBUG_L5__
    #define DEBUG_PRINT_L5(...)  pc.printf(__VA_ARGS__)
#else
    #define DEBUG_PRINT_L5(...) 
#endif

/* ********************************************* */

// 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    
};

/* 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 */

// =========================================================
// 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 >>

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 >>

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