2018.07.26

Dependencies:   QEI mbed-rtos mbed

Files at this revision

API Documentation at this revision

Comitter:
sayzyas
Date:
Thu Jul 26 00:21:04 2018 +0000
Parent:
2:c62dc496b79a
Commit message:
2018.07.26

Changed in this revision

4_main.cpp Show annotated file Show diff for this revision Revisions of this file
common.h Show annotated file Show diff for this revision Revisions of this file
mbed-rtos.lib Show annotated file Show diff for this revision Revisions of this file
mbed.bld Show annotated file Show diff for this revision Revisions of this file
--- a/4_main.cpp	Thu Apr 14 10:27:21 2016 +0000
+++ b/4_main.cpp	Thu Jul 26 00:21:04 2018 +0000
@@ -6,6 +6,8 @@
  * Date(Latest update) 2015.12.21(Mon)
  * --------------------------------------------------------
  * Article
+ *   Notification: Moving and Fix Winch rotation direction
+ *   is opposit.
  * 
  * --------------------------------------------------------
  *
@@ -47,7 +49,11 @@
 
 Serial pc(USBTX, USBRX); // tx, rx
 
+#ifdef FFWinchPhaseSetting // For Fix Fallong Winch
+QEI wheel( P0_26, P0_15, NC, ROTATE_PER_RESOLUTION, QEI::X2_ENCODING );
+#else //For Falling Position Moving Winch
 QEI wheel( P0_15, P0_26, NC, ROTATE_PER_RESOLUTION, QEI::X2_ENCODING );
+#endif // FFWinchPhaseSetting
 /*
   Dram r = 60mm
   Cable fai = 3
@@ -117,8 +123,9 @@
     
     int flg_readCurrent = 0;
     
-    int pulse;
-    int16_t distance;
+    int         pulse;
+    int16_t     distance;
+    int16_t     position_offset = 0;;
 
     uint16_t    dram_diameter;
     uint16_t    ccable_diameter;
@@ -128,7 +135,7 @@
     
     slave.address(i2c_saddress);
     
-    DEBUG_PRINT_L1("*** Start Resolver thread ***\n");
+    DEBUG_PRINT_L1("*** Start Resolver thread ***\r\n");
      
     /* 
      * I2C Access 
@@ -144,38 +151,73 @@
             //    DEBUG_PRINT_L1("the slave has not been addressed\r\n");
             //    read_motor_current(2);
                 break;
+            //
+            // Slave(this) <== Master
+            //
             case I2CSlave::ReadAddressed:
                 /* Master is reading winch current position here */
              //   DEBUG_PRINT_L1("the master has requested a read from this slave\r\n");
                 pulse = wheel.getPulses();
 
-                distance = (int16_t)( pulse * ( (double)( ((dram_diameter+ccable_diameter)/100) * PAI / (pow(2.0, (double)rresolution)*4 ) ) ) );
-            //    distance = (int16_t)( pulse * ( REAL_THREAD_DIAMETER * PAI / ROTATION_PULSE_PER_1ROUND ));
-
+                distance = (int16_t)( pulse * ( (double)( (((double)dram_diameter+(double)ccable_diameter)/(double)100) * (double)PAI / (pow(2.0, (double)rresolution)*4 ) ) ) );
+             //   distance = (int16_t)((double)distance * (double)0.99776);
+                distance = (int16_t)((double)distance * (double)0.9986);
+             //   distance = (int16_t)((double)distance * (double)0.999);
+             //   distance = (int16_t)((double)distance * (double)1.0);
+                // -------------------------------
+                distance += position_offset;    // 2016.11.17
+                 // -------------------------------
+            //    distance = (int16_t)( pulse * ( REAL_THREAD_DIAMETER * PAI / ROTATIO
+            
                 DEBUG_PRINT_L1("Bd4> PULSE: %07d, ", pulse);
-                DEBUG_PRINT_L1("DISTANCE: %04d (mm)", distance);
-                res_msg2[0] = distance & 0xFF;
-                res_msg2[1] = (distance >> 8)&0xFF;
-                slave.write(res_msg2, 2); // Includes null char
-                tmp = (res_msg2[1] < 8)&0xFF00 | res_msg2[0]&0xFF;
-                DEBUG_PRINT_L1("\tSend data : %04d(%02x,%02x)\n", tmp, res_msg2[1], res_msg2[0]);
+                DEBUG_PRINT_L1("DISTANCE(OFFSET): %04d(%4d) mm", distance, position_offset);
+                /*
+                2016.11.09
+                  Add dummy data to read winch position value.
+                    byte[0]: Dummy data = 0x12 <--- New added !
+                    byte[1]: Winch position upper byte
+                    byte[2]: Winch position lower byte
+                    byte[3]: Dummy data = 0x34 <--- New added !
+                */
+                res_msg2[0] = 0x12; // Dummy data
+                res_msg2[1] = distance & 0xFF;
+                res_msg2[2] = (distance >> 8)&0xFF;
+                res_msg2[3] = 0x34; // Dummy data
+                slave.write(res_msg2, 4); // Includes null char
+                tmp = (res_msg2[2] < 8)&0xFF00 | res_msg2[1]&0xFF;
+                DEBUG_PRINT_L1("\tSend data : %04d(%02x,%02x)\r\n", tmp, res_msg2[1], res_msg2[0]);
                 break;
             case I2CSlave::WriteGeneral:
                 DEBUG_PRINT_L1("Bd4> the master is writing to all slave\r\n"); 
                 slave.read(buf, NumberOfI2CCommand);
                 DEBUG_PRINT_L1("Bd4> Read G: %s\r\n", buf);
                 break;
+            //
+            // Master ==> Slave(this)
+            //
             case I2CSlave::WriteAddressed:
                 /* ******************************************* */
                 /* Add setting data set operation in this area */
                 /* ******************************************* */
                 if( buf[I2C_CP_COMMAND_R] == 'Z'){
                     wheel.reset();
+                    position_offset = 0;
                 }
-                else{
+                // New command 2016.11.17 for customer request
+                else if( buf[I2C_CP_COMMAND_R] == 'S'){
+                    wheel.reset();
+                    position_offset = 0;
                     for( int j = 0; j < NumberOfI2CCommand; j++)
                         pc,printf("%02x ", buf[j]); 
                     pc.printf( "\r\n" );
+                    position_offset = ( buf[I2C_CP_PRESET_CPOS_UPPER] << 8 ); 
+                    position_offset |= buf[I2C_CP_PRESET_CPOS_LOWER];
+                    pc.printf("POSITION OFFSET = %d\r\n", position_offset);
+                }
+                else if( buf[I2C_CP_COMMAND_R] == 'R'){
+                    for( int j = 0; j < NumberOfI2CCommand; j++)
+                        pc.printf("%02x ", buf[j]); 
+                    pc.printf( "\r\n" );
                     
                     dram_diameter = ( buf[I2C_CP_WDRAM_DIA_UPPER] << 8 ); 
                     dram_diameter |= buf[I2C_CP_WDRAM_DIA_LOWER];
@@ -183,21 +225,21 @@
                     ccable_diameter |= buf[I2C_CP_CCABLE_DIA_LOWER];
                     rresolution = buf[I2C_CP_RESOLVER_RESO];
                     
-                    DEBUG_PRINT_L1("Bd4> ===========================================\n");
-                    DEBUG_PRINT_L1("Bd4> Dram Diameter           : %d\t(mm)\n", dram_diameter/100);
-                    DEBUG_PRINT_L1("Bd4> CCable Diameter         : %d\t(mm)\n", ccable_diameter/100);
-                    DEBUG_PRINT_L1("Bd4> Resolver Resolution     : %d\t(bit)\n", rresolution);
-                    DEBUG_PRINT_L1("Bd4> -------------------------------------------\n", rresolution);
-                    DEBUG_PRINT_L1("Bd4> Real Diameter           : %d\t(mm)\n", (dram_diameter+ccable_diameter)/100);
-                    DEBUG_PRINT_L1("Bd4> Rotation Pulse / 1round : %d\t(pulse)\n", (int)(pow(2.0, (double)rresolution)*4));
-                    DEBUG_PRINT_L1("Bd4> Distance / 1pulse       : %lf\t(mm)\n",  (double)( ((dram_diameter+ccable_diameter)/100) * PAI / (pow(2.0, (double)rresolution)*4 ) ) );                   
-                    DEBUG_PRINT_L1("Bd4> ===========================================\n");
+                    DEBUG_PRINT_L1("Bd4> ===========================================\r\n");
+                    DEBUG_PRINT_L1("Bd4> Dram Diameter           : %d\t(mm)\r\n", dram_diameter);
+                    DEBUG_PRINT_L1("Bd4> CCable Diameter         : %d\t(mm)\r\n", ccable_diameter);
+                    DEBUG_PRINT_L1("Bd4> Resolver Resolution     : %d\t(bit)\r\n", rresolution);
+                    DEBUG_PRINT_L1("Bd4> -------------------------------------------\r\n", rresolution);
+                    DEBUG_PRINT_L1("Bd4> Real Diameter           : %d\t(mm)\r\n", (dram_diameter+ccable_diameter));
+                    DEBUG_PRINT_L1("Bd4> Rotation Pulse / 1round : %d\t(pulse)\r\n", (int)(pow(2.0, (double)rresolution)*4));
+                    DEBUG_PRINT_L1("Bd4> Distance / 1pulse       : %lf\t(mm)\r\n",  (double)( ((dram_diameter+ccable_diameter)/100) * PAI / (pow(2.0, (double)rresolution)*4 ) ) );                   
+                    DEBUG_PRINT_L1("Bd4> ===========================================\r\n");
                     
                     pulse = wheel.getPulses();
                     DEBUG_PRINT_L1("Bd4> PULSE: %07d, ", pulse);
                 //    DEBUG_PRINT_L1("Bd4> >DISTANCE: %04d (mm)\n", (int)( pulse * ( REAL_THREAD_DIAMETER * PAI / ROTATION_PULSE_PER_1ROUND )));
-                    DEBUG_PRINT_L1("DISTANCE: %04d (mm)\n", (int)( pulse * ( (double)( ((dram_diameter+ccable_diameter)/100) * PAI / (pow(2.0, (double)rresolution)*4 ) ) ) ) );
-                    DEBUG_PRINT_L1("Bd4> ---------------------------------------\n");
+                    DEBUG_PRINT_L1("DISTANCE: %04d (mm)\r\n", (int)( pulse * ( (double)( ((dram_diameter+ccable_diameter)/100) * PAI / (pow(2.0, (double)rresolution)*4 ) ) ) ) );
+                    DEBUG_PRINT_L1("Bd4> ---------------------------------------\r\n");
                 }
                 break;
         }
--- a/common.h	Thu Apr 14 10:27:21 2016 +0000
+++ b/common.h	Thu Jul 26 00:21:04 2018 +0000
@@ -3,6 +3,9 @@
  *
  */
 
+// If you use moving type winch , then should comment out followings !!
+// #define FFWinchPhaseSetting  // Valid if Fix falling winch system is using.
+
 #define __DEBUG__
 #ifdef __DEBUG__
 #define DEBUG_PRINT(...)  pc.printf(__VA_ARGS__)
@@ -11,9 +14,9 @@
 //#define DEBUG_PRINT(...)  1 ? (void)0 : pc.printf(__VA_ARGS__)
 #endif
 /* Information */
-#define LatestUpDate "2015.12.21"
-#define ProgramRevision "RC2015.12.21" 
-#define Author "Sayzyas"
+#define LatestUpDate "2016.09.27"
+#define ProgramRevision "Rev 2.30" 
+#define Author "Y.Saito(zinsor)"
 #define Company "Revast Co.,Ltd"
 
 
@@ -61,8 +64,8 @@
     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_RES1,                // reserved   
-    I2C_CP_RES2,                // reserved   
+    I2C_CP_PRESET_CPOS_UPPER,   // reserved   
+    I2C_CP_PRESET_CPOS_LOWER,   // reserved   
     I2C_CP_RES3,                // reserved   
     I2C_CP_RES4,                // reserved   
     I2C_CP_RES5,                // reserved  
--- a/mbed-rtos.lib	Thu Apr 14 10:27:21 2016 +0000
+++ b/mbed-rtos.lib	Thu Jul 26 00:21:04 2018 +0000
@@ -1,1 +1,1 @@
-http://mbed.org/users/mbed_official/code/mbed-rtos/#3d9d2b8b8f17
+http://mbed.org/users/mbed_official/code/mbed-rtos/#58563e6cba1e
--- a/mbed.bld	Thu Apr 14 10:27:21 2016 +0000
+++ b/mbed.bld	Thu Jul 26 00:21:04 2018 +0000
@@ -1,1 +1,1 @@
-http://mbed.org/users/mbed_official/code/mbed/builds/f141b2784e32
\ No newline at end of file
+https://mbed.org/users/mbed_official/code/mbed/builds/e2bfab296f20
\ No newline at end of file