z ysaito
2018.07.26
Dependencies: QEI mbed-rtos mbed
Revision 3:85eb7e954bfa, committed 2018-07-26
- Comitter:
- sayzyas
- Date:
- Thu Jul 26 00:21:04 2018 +0000
- Parent:
- 2:c62dc496b79a
- Commit message:
- 2018.07.26
Changed in this revision
diff -r c62dc496b79a -r 85eb7e954bfa 4_main.cpp
--- 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;
}
diff -r c62dc496b79a -r 85eb7e954bfa common.h
--- 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
diff -r c62dc496b79a -r 85eb7e954bfa mbed-rtos.lib --- 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
diff -r c62dc496b79a -r 85eb7e954bfa mbed.bld --- 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