z ysaito
2018.07.26
Revision 0:b022386f217f, committed 2018-07-26
- Comitter:
- sayzyas
- Date:
- Thu Jul 26 00:21:10 2018 +0000
- Commit message:
- 2018.07.26
Changed in this revision
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/4_main.cpp Thu Jul 26 00:21:10 2018 +0000
@@ -0,0 +1,404 @@
+/**********************************************************
+ * Project: B2 (1F-1)
+ * Title: CrExp B2 Motor Ctrl Main
+ * Target: LPCXpresso824-Max
+ * Author: zinsor
+ * Date(Latest update) 2015.12.21(Mon)
+ * --------------------------------------------------------
+ * Article
+ * Notification: Moving and Fix Winch rotation direction
+ * is opposit.
+ *
+ * --------------------------------------------------------
+ *
+ * LPCXpresso 824-MAX
+ * +---------USB---------+
+ * | |
+ * | |
+ * | |
+ * | |
+ * | | SCL P0_10 D15 -> IIC SCL
+ * | ## ### ## | SDA P0_11 D14 -> IIC SDA
+ * | # # # # # # | AVDD
+ * N/A | # # # # # | GND
+ * +3V3 | ## # # # | SCK P0_24 D13
+ * NRST | # # # # # | MISO P0_25 D12
+ * +3V3 | # # # # ###### | MOSI P0_26 D11 -> Resolver B
+ * +5V | ## ##### # | SSEL P0_15 D10 -> Resolver A
+ * GND | | P0_27 D9
+ * GND | | P0_13 D8
+ * N/A | | P0_17 D7
+ * | | P0_16 D6
+ * A0 P0_6 | | PWM P0_28 D5
+ * A1 P0_14 | | PWM P0_18 D4
+ * A2 P0_23 | | PWM P0_12 D3
+ * A3 P0_22 | | PWM P0_19 D2
+ * A4 P0_21 | | TX P0_4 D1
+ * A5 P0_20 | | RX P0_0 D0
+ * +---------------------+
+ *
+ ***************************************/
+
+#include "mbed.h"
+//#include "rtos.h"
+#include "QEI.h"
+#include "common.h"
+#include <math.h>
+
+// Hardware setting
+
+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
+
+ (60+3)*3.14 = 197.82mm ==> 2^12*4 = 4096 * 4 = 16384 pulse
+ 1(mm) = 82.82(pulse)
+ 0.01208(mm) = 1(pulse)
+
+*/
+DigitalOut led1(LED1); // Red
+DigitalOut led2(LED2); // Green
+DigitalOut led3(LED3); // Blue
+
+I2CSlave slave(P0_11, P0_10); //I2C SDA SCL
+
+int i2c_saddress = 0;
+
+void led_demo(){
+ int i;
+ for( i = 0; i < 20; i++ ) {
+ led1 = LED_ON;
+ led2 = LED_OFF;
+ led3 = LED_OFF;
+ wait_ms(20);
+ led1 = LED_OFF;
+ led2 = LED_OFF;
+ led3 = LED_OFF;
+ wait_ms(20);
+ led1 = LED_OFF;
+ led2 = LED_ON;
+ led3 = LED_OFF;
+ wait_ms(20);
+ led1 = LED_OFF;
+ led2 = LED_OFF;
+ led3 = LED_OFF;
+ wait_ms(20);
+ led1 = LED_OFF;
+ led2 = LED_OFF;
+ led3 = LED_ON;
+ wait_ms(20);
+ led1 = LED_OFF;
+ led2 = LED_OFF;
+ led3 = LED_OFF;
+ wait_ms(20);
+ }
+// wait_ms(3000);
+}
+
+// ========================================================================
+// Thread: Resolver Control Task
+// ========================================================================
+void ResolverCtrl_task(void const *) {
+
+
+}
+
+// ========================================================================
+// Thread: Main Controller Interface Task
+// Get Command from Main controller and do the task.
+// ========================================================================
+void MainCtrl_Interface_task(void const *) {
+
+ char buf[14]; // command buffer
+// char msg[] = "C000000";
+// char res_msg[] = "Slave!";
+ char res_msg2[] = "Bravo!";
+
+// int flg_readCurrent = 0;
+
+ int pulse;
+ int16_t distance;
+ int16_t position_offset = 0;;
+
+ uint16_t dram_diameter;
+ uint16_t ccable_diameter;
+ uint8_t rresolution;
+
+ int tmp;
+
+ slave.address(i2c_saddress);
+
+ DEBUG_PRINT_L1("*** Start Resolver thread ***\r\n");
+
+ /*
+ * I2C Access
+ */
+ while(1) {
+ // DEBUG_PRINT_L1(".");
+ int i = slave.receive();
+ slave.read(buf, NumberOfI2CCommand);
+ // DEBUG_PRINT_L1("?");
+ // DEBUG_PRINT_L1("Slave Received\r\n");
+ switch (i) {
+ case I2CSlave::NoData:
+ // 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)( (((double)dram_diameter+(double)ccable_diameter)/(double)100) * (double)PAI / (pow(2.0, (double)rresolution)*4 ) ) ) );
+ distance = (int16_t)( pulse * ( (double)( ((double)dram_diameter/(double)100) * (double)PAI / (pow(2.0, (double)rresolution)*4 ) ) ) );
+ // distance = (int16_t)((double)distance * (double)0.9986);
+ distance = (int16_t)((double)distance * (double)((double)ccable_diameter/(double)10000));
+ // -------------------------------
+ distance += position_offset; // 2016.11.17
+ // -------------------------------
+ // distance = (int16_t)( pulse * ( REAL_THREAD_DIAMETER * PAI / ROTATION_PULSE_PER_1ROUND ));
+
+ DEBUG_PRINT_L1("Bd4> PULSE: %07d, ", pulse);
+ 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;
+ }
+ // 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];
+ ccable_diameter = ( buf[I2C_CP_CCABLE_DIA_UPPER] << 8 );
+ ccable_diameter |= buf[I2C_CP_CCABLE_DIA_LOWER];
+ rresolution = buf[I2C_CP_RESOLVER_RESO];
+
+ 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)\r\n", (int)( pulse * ( (double)( ((dram_diameter+ccable_diameter)/100) * PAI / (pow(2.0, (double)rresolution)*4 ) ) ) ) );
+ DEBUG_PRINT_L1("Bd4> ---------------------------------------\r\n");
+ }
+ break;
+ }
+ //Thread::wait(1);
+ wait_ms(1);
+ }
+}
+// ========================================================================
+// Main Function
+// ========================================================================
+int main() {
+
+ char buf[14]; // command buffer
+ char res_msg2[] = "Bravo!";
+
+ int pulse;
+ int16_t distance;
+ int16_t position_offset = 0;;
+
+ uint16_t dram_diameter;
+ uint16_t ccable_diameter;
+ uint8_t rresolution;
+
+ int tmp;
+
+ int32_t counter = 0;
+
+ pc.baud(115200);
+
+ i2c_saddress = Target_IIC_ADDR; // defined at header
+
+ DEBUG_PRINT_L0("> Board type: Resolver Counter controller(0x%02x)\r\n",i2c_saddress );
+
+ i2c_saddress = I2C_ADDRESS_RESOLVER;
+ slave.address(i2c_saddress);
+
+ led_demo();
+
+ DEBUG_PRINT_L0("\r\n");
+ DEBUG_PRINT_L0("Bd4> +--------------------------------------\r\n");
+ DEBUG_PRINT_L0("Bd4> | B2 CrExp Resolver Pulse Counter Main \r\n");
+ DEBUG_PRINT_L0("Bd4> | Letest update: %s\r\n", LatestUpDate);
+ DEBUG_PRINT_L0("Bd4> | Program Revision: %s\r\n", ProgramRevision);
+ DEBUG_PRINT_L0("Bd4> | Author: %s\r\n", Author);
+ DEBUG_PRINT_L0("Bd4> | Copyright(C) 2015 %s Allright Reserved\r\n", Company);
+ DEBUG_PRINT_L0("Bd4> +--------------------------------------\r\n");
+
+ //Thread resolverControlTask1(MainCtrl_Interface_task, NULL, osPriorityNormal, 128 * 4);
+ wheel.reset();
+
+ while(1){
+
+ counter++;
+ if( counter >= 50 ){
+ led3 = !led3; // Blue
+ counter = 0;
+ }
+
+ // DEBUG_PRINT_L1(".");
+ int i = slave.receive();
+ slave.read(buf, NumberOfI2CCommand);
+ // DEBUG_PRINT_L1("?");
+ // DEBUG_PRINT_L1("Slave Received\r\n");
+ switch (i) {
+ case I2CSlave::NoData:
+ // 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)( (((double)dram_diameter+(double)ccable_diameter)/(double)100) * (double)PAI / (pow(2.0, (double)rresolution)*4 ) ) ) );
+ distance = (int16_t)( pulse * ( (double)( ((double)dram_diameter/(double)100) * (double)PAI / (pow(2.0, (double)rresolution)*4 ) ) ) );
+ // distance = (int16_t)((double)distance * (double)0.9986);
+ distance = (int16_t)((double)distance * (double)((double)ccable_diameter/(double)10000));
+ // -------------------------------
+ distance += position_offset; // 2016.11.17
+ // -------------------------------
+ // distance = (int16_t)( pulse * ( REAL_THREAD_DIAMETER * PAI / ROTATION_PULSE_PER_1ROUND ));
+
+ DEBUG_PRINT_L1("Bd4> PULSE: %07d, ", pulse);
+ 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;
+ }
+ // 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];
+ ccable_diameter = ( buf[I2C_CP_CCABLE_DIA_UPPER] << 8 );
+ ccable_diameter |= buf[I2C_CP_CCABLE_DIA_LOWER];
+ rresolution = buf[I2C_CP_RESOLVER_RESO];
+
+ 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)\r\n", (int)( pulse * ( (double)( ((dram_diameter+ccable_diameter)/100) * PAI / (pow(2.0, (double)rresolution)*4 ) ) ) ) );
+ DEBUG_PRINT_L1("Bd4> ---------------------------------------\r\n");
+ }
+ break;
+ }
+ //Thread::wait(1);
+ wait_ms(1);
+ }
+}
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/QEI.lib Thu Jul 26 00:21:10 2018 +0000 @@ -0,0 +1,1 @@ +http://mbed.org/users/aberk/code/QEI/#5c2ad81551aa
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/common.h Thu Jul 26 00:21:10 2018 +0000
@@ -0,0 +1,94 @@
+/*
+ * mbed LPC824-Max header
+ *
+ */
+
+// 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__)
+#else
+#define DEBUG_PRINT(...) ;
+//#define DEBUG_PRINT(...) 1 ? (void)0 : pc.printf(__VA_ARGS__)
+#endif
+/* Information */
+#define LatestUpDate "2016.09.27"
+#define ProgramRevision "Rev 2.30"
+#define Author "Y.Saito(zinsor)"
+#define Company "Revast Co.,Ltd"
+
+
+/* Debug macro */
+#define __DEBUG_L0__
+#define __DEBUG_L1__
+#define __DEBUG_L2__
+#define __DEBUG_L3__
+#define __DEBUG_L4__
+
+#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
+
+#define NumberOfI2CCommand 14
+
+/* 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
+};
+
+#define ROTATE_PER_RESOLUTION 24
+#define REAL_THREAD_DIAMETER 63 // 60+3
+//#define ROTATION_DISTANCE 197.82f // (60+3)*3.14
+#define ROTATION_PULSE_PER_1ROUND 16384 // 2^12(bit) * 4
+#define PAI 3.14159265359
+
+/* ***************** */
+/* Target definition */
+/* ***************** */
+#define I2C_ADDRESS_RESOLVER 0x02
+#define Target_IIC_ADDR 0x02 // For Resolver count controller
+
+
+/* Command Packet */
+#define CLEAR_RESOLVER_POSITION 'X'
+#define HELLO_PACKET 'A'
+
+#define LED_ON 0
+#define LED_OFF 1
\ No newline at end of file
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/mbed.bld Thu Jul 26 00:21:10 2018 +0000 @@ -0,0 +1,1 @@ +http://mbed.org/users/mbed_official/code/mbed/builds/ef9c61f8c49f \ No newline at end of file