File content as of revision 1:fdf87a1a724b:

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

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

/* ********************************************* */
/* Platform definition                           */
/* ********************************************* */
#define PLATFORM "B1Demo"
//#define PLATFORM "B2Demo"


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

// Motor Controller RoboCAN ID definition
// --> define at "mtrAccess.cpp"

//#define NUMBER_OF_MCCMD 7
#define WINCH_OFFSET_VALUE 20


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

enum{
    MOTOR_RFCRW,        // RF crawler
    MOTOR_LBCRW,        // LB crawler
    MOTOR_RFTFM,        // RF Transform
    MOTOR_LBTFM,        // LB transform
    MOTOR_WINCH,        // Winch
    MOTOR_CMPAN,        // Pan/Tilt
    MOTOR_CTILT
};




enum{
    MOTOR_DIR_FWD,      // Motor forwaed rotation
    MOTOR_DIR_RVS,      // Motor reverse rotation
    MOTOR_DIR_STP      // Motor stop
};

/* I2C Command packet to motor controller */
/* For motor controller */
#define NumberOfI2CCommand 12
enum{
    I2C_CP_COMMAND,             // instruction command
    I2C_CP_MOTORNO,             // motor number
    I2C_CP_M_DIR,               // motor rotation direction   
    I2C_CP_M_CNTTH_U,       // motor current limit detection threshold upper byte 
    I2C_CP_M_CNTTH_L,       // motor current limit detection threshold lower byte 
    I2C_CP_WDRAM_DIA_UPPER,     // winch dram motor diameter upper   
    I2C_CP_WDRAM_DIA_LOWER,     // winch dram motor diameter lower    
    I2C_CP_CCABLE_DIA_UPPER,    // cable diameter upper byte 
    I2C_CP_CCABLE_DIA_LOWER,    // cable diameter lower byte 
    I2C_CP_RESOLVER_RESO,       // resolver resolution (bit)
    I2C_CP_PRESET_CPOS_UPPER,   // preset position upper
    I2C_CP_PRESET_CPOS_LOWER   // preset position lower 
};
    
/* 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    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 {
    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    pan_mtr_ithd_f;      // 2 Current threshold winch dram Motor ( forward )
    uint16_t    pan_mtr_ithd_r;      // 2 Current threshold winch dram Motor ( reverse )
    uint16_t    tlt_mtr_ithd_f;      // 2 Current threshold winch No2 Motor ( forward )
    uint16_t    tlt_mtr_ithd_r;      // 2 Current threshold winch No2 Motor ( reverse )
    uint8_t     pan_mtr_hspd_f;      // 1 Motor speed winch dram motor high speed ( forward )
    uint8_t     pan_mtr_hspd_r;      // 1 Motor speed winch dram motor high speed ( reverse )
    uint8_t     tlt_mtr_hspd_f;      // 1 Motor speed winch No2 motor high speed ( forward )
    uint8_t     tlt_mtr_hspd_r;      // 1 Motor speed winch No2 motor high speed ( 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
} ptl_SetValue_t;                    // << 24 byte >>

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    no2_mtr_ithd_f;      // 2 Current threshold winch No2 Motor ( forward )
    uint16_t    no2_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     no2_mtr_hspd_f;      // 1 Motor speed winch No2 motor high speed ( forward )
    uint8_t     no2_mtr_hspd_r;      // 1 Motor speed winch No2 motor high 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
    uint8_t     reserved_4;          // 1 reserved for future use
    uint8_t     reserved_5;          // 1 reserved for future use
    uint8_t     reserved_6;          // 1 reserved for future use
    uint8_t     reserved_7;          // 1 reserved for future use
} wch_SetValue_t;                    // << 24 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 {
    crawler_SetValue_t  crwCtrl;
    tfm_SetValue_t      tfmCtrl;
    ptl_SetValue_t      ptlCtrl;
    wch_SetValue_t      wchCtrl;
} 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