Revision 9:a4cc7dc2d92f, committed 2020-09-14

Comitter:

sayzyas

Date:

Mon Sep 14 06:38:16 2020 +0000

Parent:

8:23b1fba109b0

Commit message:

1

Changed in this revision

--- a/EthernetInterface.lib	Tue Feb 14 16:20:16 2017 +0000
+++ /dev/null	Thu Jan 01 00:00:00 1970 +0000
@@ -1,1 +0,0 @@
-http://mbed.org/users/mbed_official/code/EthernetInterface/#183490eb1b4a

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/F7_Ethernet.lib	Mon Sep 14 06:38:16 2020 +0000
@@ -0,0 +1,1 @@
+http://mbed.org/users/DieterGraef/code/F7_Ethernet/#28ba13dd96f7

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/common.h	Mon Sep 14 06:38:16 2020 +0000
@@ -0,0 +1,338 @@
+/* 
+   DEBUG PRINT MACRO
+   http://qiita.com/saltheads/items/e1b0ab54d3d6029c9593
+   http://tricky-code.net/nicecode/code10.php
+*/
+/* Information */
+#define LatestUpDate    "2020.07.02"
+#define ProgramRevision "Rev 0.01" 
+#define Author          "y.saito"
+#define Company         "Sanei Co.,Ltd."
+
+//#define __CREATE_SETTING_FILE__
+//#define __TARGET_BOARD_CHECK__
+
+
+//#define __WInchDebug__  // For debugging 
+
+
+// Comment out if net setting is FIX.
+// #define __NET_SETTING_FROM_LFS__
+
+
+// Read motor current 
+#define __READ_TFM_MOTOR_CURRENT__
+
+#define __IIC_COMAMND_SEND__
+
+#define __SET_WINCH_MOTOR_SPEED_BY_VOLUME__
+
+/********************************************** 
+ For Debugging
+***********************************************/ 
+#define __DEBUG_L0__
+#define __DEBUG_L1__
+#define __DEBUG_L2__
+#define __DEBUG_L3__
+#define __DEBUG_L4__
+#define __DEBUG_L5__
+
+#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
+
+
+
+/* *****
+ Constant definition
+***** */
+#define BLINKING_RATE_MS  100 /* Blinking rate */
+
+
+#define DIR_CCW 1
+#define DIR_CW 0
+#define MOTOR_ON 1
+#define MOTOR_OFF 0
+
+#define L_001mm 1000 * 2
+#define L_002mm 2000 * 2
+#define L_003mm 3000 * 2
+#define L_004mm 4000 * 2
+#define L_005mm 5000 * 2
+#define L_006mm 6000 * 2
+#define L_007mm 7000 * 2
+#define L_008mm 8000 * 2
+#define L_009mm 9000 * 2
+#define L_010mm 10000 * 2
+#define L_050mm 50000 * 2
+#define L_100mm 100000 * 2
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+/* 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,        // Sub command
+    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,
+};
+
+/* Game Pad defnition */
+#define GAMEPAD_VID_LOGICOOL        0x046d
+#define GAMEPAD_PID_LOGICOOL_F710   0xc219
+#define GAMEPAD_PID_LOGICOOL_F310   0xc216
+
+#define GAMEPAD_VID_SANWA           0x12bd
+#define GAMEPAD_PID_SANWA_JYP70US   0xc003
+
+#define GAMEPAD_VID_ELECOM          0x056e
+#define GAMEPAD_PID_ELECOM_JCU3613M 0x2003
+
+#define GAMEPAD_VID_RSTHANDY        0x1234
+#define GAMEPAD_PID_RSTHANDY        0x0006
+
+
+/* Ether net */
+#define TCP_SERVER_PORT   10002
+#define UDP_SERVER_PORT   10000
+//#define ECHO_SERVER_PORT   7
+
+#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    sv_WDM_ith_F;           // 2 Current Threshold Winch Dram Motor Fwd rotation.
+    uint16_t    sv_WDM_ith_R;           // 2 Current Threshold Winch Dram Motor Rvs rotation.
+    uint16_t    sv_WRM_ith_F;           // 2 Current Threshold Winch .... Motor Fwd rotation.
+    uint16_t    sv_WRM_ith_R;           // 2 Current Threshold Winch .... Motor Rvs rotation.
+    uint8_t     sv_WDM_hsrto_F;         // 1 Winch Dram Motor high speed ratio Fwd
+    uint8_t     sv_WDM_hsrto_R;         // 1 Winch Dram Motor high speed ratio Rvs
+    uint8_t     sv_WDM_lsrto_F;         // 1 Winch Dram Motor low speed ratio Fwd 
+    uint8_t     sv_WDM_lsrto_R;         // 1 Winch Dram Motor low speed ratio Rvs 
+    uint8_t     sv_WRM_hsrto_F;         // 1 Winch .... Motor high speed limit Fwd
+    uint8_t     sv_WRM_hsrto_R;         // 1 Winch .... Motor high speed limit Rvs
+    uint8_t     sv_WRM_lsrto_F;         // 1 Winch .... Motor low speed ratio Fwd 
+    uint8_t     sv_WRM_lsrto_R;         // 1 Winch .... Motor low speed ratio Rvs   
+    uint16_t    sv_WRS_DramDmrx100;     // 2 Winch Dram diameter x 100;
+    uint16_t    sv_WRS_CCableDmrx100;   // 2 Winch CCable diameter x 100
+    uint8_t     sv_WRS_RResolution;     // 1 Winch Resolver resolution (bit)
+    uint8_t     reserved;               // 1 reserved for future use
+} winch_SetValue_t;                     // 22 byte
+
+typedef struct {
+    uint16_t    sv_RFTM_ith_F;          // 2 RF transform motor Fwd roration current threshold 
+    uint16_t    sv_RFTM_ith_R;          // 2 RF transform motor Rvs roration current threshold
+    uint16_t    sv_LBTM_ith_F;          // 2 LB transform motor Fwd roration current threshold
+    uint16_t    sv_LBTM_ith_R;          // 2 LB transform motor Rvs roration current threshold
+    uint8_t     sv_RFTM_srto_F;         // 1 RF transform motor Fwd rotation speed ratio
+    uint8_t     sv_RFTM_srto_R;         // 1 RF transform motor Rvs rotation speed ratio
+    uint8_t     sv_LBTM_srto_F;         // 1 LB transform motor Fwd rotation speed ratio
+    uint8_t     sv_LBTM_srto_R;         // 1 LB transform motor Rvs rotation speed ratio
+} tfm_SetValue_t;                       // 12 byte 
+
+typedef struct {
+    uint16_t    sv_RFCM_ith_F;      // 2 RF Crawler motor Fwd rotation current threshold
+    uint16_t    sv_RFCM_ith_R;      // 2 RF Crawler motor Rvs rotation current threshold
+    uint16_t    sv_LBCM_ith_F;      // 2 LB Crawler motor Fwd rotation current threshold
+    uint16_t    sv_LBCM_ith_R;      // 2 LB Crawler motor Rvs rotation current threshold
+    uint8_t     sv_RFCM_srto_F;     // 1 RF Crawler motor Fwd rotation speed limnit
+    uint8_t     sv_RFCM_srto_R;     // 1 RF Crawler motor Rvs rotation speed limnit
+    uint8_t     sv_LBCM_srto_F;     // 1 LB Crawler motor Fwd rotation speed limnit     
+    uint8_t     sv_LBCM_srto_R;     // 1 LB Crawler motor Rvs rotation speed limnit
+    uint8_t     sv_RFCM_dzu;        // 1 R-Joystick upper dead zone width
+    uint8_t     sv_RFCM_dzc;        // 1 R-Joystick center value
+    uint8_t     sv_RFCM_dzl;        // 1 R-Joystick lower dead zone width
+    uint8_t     sv_LBCM_dzu;        // 1 L-Joystick upper dead zone width
+    uint8_t     sv_LBCM_dzc;        // 1 L-Joystick center value
+    uint8_t     sv_LBCM_dzl;        // 1 L-Joystick lower dead zone width
+    uint8_t     reserved1;          // 1
+    uint8_t     reserved2;          // 1
+} crawler_SetValue_t;               // 20 byte
+
+typedef struct {
+    uint8_t sv_centerValue;
+    uint8_t sv_nsenseBand_s;
+    uint8_t sv_nsenseBand_w;
+} js_SetValue_t;
+
+typedef struct SetValue {
+//    operation_t             operation;
+    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

--- a/main.cpp	Tue Feb 14 16:20:16 2017 +0000
+++ b/main.cpp	Mon Sep 14 06:38:16 2020 +0000
@@ -1,51 +1,231 @@
 #include "mbed.h"
+#include "rtos.h"
 #include "EthernetInterface.h"
+#include "common.h"
+#include "stdio.h"
  
-#define ECHO_SERVER_PORT   7
+#define ECHO_SERVER_PORT   10004
+
+/* Debug port */
+Serial pc(USBTX, USBRX); // USBSerial serial setting
+
+/* GPIO setting */
+DigitalOut led1(LED1); // on board green
+DigitalOut led2(LED2); // on board blue 
+DigitalOut led3(LED3); // on board red
+
+DigitalInOut OneAxisSliderPulse(PE_11, PIN_OUTPUT, OpenDrain, 0);
+DigitalInOut OneAxisSliderDir(PE_9, PIN_OUTPUT, OpenDrain, 0);
+// DigitalOut OneAxisSliderPulse(PE_9);
+//DigitalOut OneAxisSliderDir(PE_11);
+
+DigitalOut OneAxisSliderORG(PF_3);
+DigitalOut OneAxisSliderReset(PF_15);
+DigitalOut OneAxisSliderServoOn(PF_11);
+
+DigitalOut Relay(PC_8);
+
+//DigitalInOut lamp_out(p23, PIN_OUTPUT, OpenDrain, 0);
+//DigitalInOut lamp_out(p23, PIN_OUTPUT, OpenDrain, 0);
+
+
+DigitalOut out1( PG_9 );
+AnalogOut linear_out( PA_5 );
+AnalogOut linear_out2( PA_4 );
+
+DigitalIn limit1_din(PG_5);    // limit1 in 
+DigitalIn limit2_din(PG_6);    // limit2 in
+AnalogIn weight_ain(PC_2);   // analog input of load cell
+
+
+/* Network setting */
+UDPSocket   userver; // UDP Server
+const char* ip_address = "192.168.31.31";
+const char* subnet_mask = "255.255.255.0";
+const char* gateway = "192.168.31.0";
+
+Thread thread1;
+Thread thread2;
+Thread thread3;   
+Thread thread4;   
+
+void move_distance( int dir , int dis_mm );
+
  
-int main (void) {
+/* Thread routine */
+void OneAxisSliderCtrl_thread() 
+{
+    OneAxisSliderReset = 1;
+    led2 = 1;
+    
+    wait(1);
+    led2 = 0;
+
+    OneAxisSliderReset = 0;
+    
+    wait_ms(3000);
+    
+    led3 = 1;
+    printf("Servo on\r\n");
+    
+    // ORG-S: OFF
+    
+    OneAxisSliderServoOn = 1;
+    printf("Servo on\r\n");
+    
+    
+    
+    wait(1);
+    OneAxisSliderORG = 1;
+    printf("Move to Origin\r\n");
+
+    // **************************************
+    // while( ORG-S singnal is ON(=0V))
+    // if slider is located to ORIGIN then OGG-S signal will be ON(0V) until next RESET or SERVO off
+    // ORG-S: OFF
+    // **************************************
+
+    
+    wait_ms(15000);
+    
+    wait(1);
+
+    printf("1st move\r\n");
+    move_distance( DIR_CCW, L_010mm * 30 );
+
+    wait_ms(8000);
+    
+    while (true) 
+    {
+        while (true)
+        {
+            printf("CW move\r\n");
+            move_distance( DIR_CW, L_010mm * 20 );
+            
+            wait_ms(8000);
+            
+  
+  
+            printf("CCW move\r\n");
+            move_distance( DIR_CCW, L_010mm * 20 );
+      
+            break;
+        }
+        wait(1);
+        printf(" weight: %d\r\n", weight_ain.read_u16());
+    }
+}
+
+
+
+
+void move_distance( int dir , int dis_mm )
+{
+    OneAxisSliderDir = dir;
+    wait(0.1);
+    for( int i = 0; i < dis_mm; i++ )
+    {
+        OneAxisSliderPulse = MOTOR_ON;
+        wait_us(5);
+        OneAxisSliderPulse = MOTOR_OFF;
+        wait_us(5);    
+    }
+}
+    
+
+
+void led3_thread() 
+{
+    while (true) 
+    {
+        Relay = 0;
+    //    Thread::wait(BLINKING_RATE_MS-20);
+        Thread::wait( 100 );
+        Relay = 1;
+        Thread::wait( 100 );
+    }
+}
+
+void linear_thread()
+{
+    while( true )
+    {
+        for( double i=0.0; i<1.0; i+=0.001) {
+            linear_out = i;
+            Thread::wait(50);
+        }        
+    }
+}
+
+void network_thread()
+{
     EthernetInterface eth;
-    eth.init(); //Use DHCP
+    eth.init( ip_address, subnet_mask, gateway ); //Use DHCP
     eth.connect();
-    printf("\nServer IP Address is %s\n", eth.getIPAddress());
+    printf("\nServer IP Address is %s\r\n", eth.getIPAddress());
     
     TCPSocketServer server;
     server.bind(ECHO_SERVER_PORT);
     server.listen();
-    
-    while (true) {
-        printf("\nWait for new connection...\n");
+        
+    while (true) 
+    {
+        printf("\nWait for new connection...\r\n");
         TCPSocketConnection client;
         server.accept(client);
         client.set_blocking(false, 1500); // Timeout after (1.5)s
         
-        printf("Connection from: %s\n", client.get_address());
+        printf("Connection from: %s\r\n", client.get_address());
         char buffer[256];
-        while (true) {
+        while (true) 
+        {
             int n = client.receive(buffer, sizeof(buffer));
             if (n <= 0) break;
             
+            led1 = 1;
             // print received message to terminal
             buffer[n] = '\0';
-            printf("Received message from Client :'%s'\n",buffer);
+            printf("Received message from Client :'%s'\r\n",buffer);
             
-            // reverse the message
-            char temp;
-            for(int f = 0, l = n-1; f<l; f++,l--){
-                temp = buffer[f];
-                buffer[f] = buffer[l];
-                buffer[l] = temp;
-                }
-            
-            // print reversed message to terminal
-            printf("Sending message to Client: '%s'\n",buffer);
-            
+            if( !strcmp( buffer, "SLD_P0_0000" ) )
+            {
+                printf("Command: SLD move to position 0\r\n");
+            }
             // Echo received message back to client
             client.send_all(buffer, n);
             if (n <= 0) break;
         }
-        
         client.close();
     }
 }
- 
\ No newline at end of file
+
+
+
+
+
+int main (void) {
+    
+    pc.baud(115200); // Set UART(USB) baudrate
+    DEBUG_PRINT_L0("\r\n");
+    DEBUG_PRINT_L0(" +--------------------------------------------------------------\r\n");
+    DEBUG_PRINT_L0(" | This is: Main Control Program of .....\r\n");
+    DEBUG_PRINT_L0(" +--------------------------------------------------------------\r\n");    
+  
+    limit1_din.mode(PullUp);
+    limit2_din.mode(PullUp);
+//    Thread thread1(linear_thread, NULL, osPriorityNormal, 128 * 4);
+//    Thread thread2(led2_thread, NULL, osPriorityNormal, 128 * 4);
+//    Thread thread3(led3_thread, NULL, osPriorityNormal, 128 * 4);
+    thread1.start(linear_thread);
+    thread2.start(OneAxisSliderCtrl_thread);
+    thread3.start(led3_thread);
+    thread4.start(network_thread);
+
+    //while (true) 
+    //{
+        //led1 = !led1;
+        //Thread::wait(1);
+        //SliderPulse = !SliderPulse;
+
+    //}
+}
\ No newline at end of file