z ysaito
2018.08.03 1st commit of demo main res ctrl
Diff: P1_1Y_main.cpp
- Revision:
- 0:f8373bf202a6
- Child:
- 1:85e8e2c8f283
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/P1_1Y_main.cpp Thu Jul 26 00:19:47 2018 +0000
@@ -0,0 +1,435 @@
+/**********************************************************
+ * 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>
+#include "mcchk.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
+
+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);
+}
+
+// ========================================================================
+// Main Function
+// ========================================================================
+int main() {
+
+ char buf[12]; // command buffer
+ char msg[9];
+
+ int pulse;
+ double distance_d = 0.0f;
+ int16_t distance;
+ int16_t position_offset = 0;;
+
+ uint16_t dram_diameter;
+ uint16_t dis_correct_value;
+ uint8_t rresolution;
+
+ int tmp;
+ mcchk mcc;
+
+ pc.baud(115200);
+
+ slave.address(I2C_ADDRESS_RESOLVER);
+
+ 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");
+
+ wheel.reset();
+
+
+
+ while(1){
+
+ led2 = LED_OFF;
+ led3 = LED_OFF;
+
+ int i = slave.receive();
+ slave.read(buf, NumberOfI2CCommand);
+
+ switch (i) {
+ case I2CSlave::NoData:
+ // DEBUG_PRINT_L1("the slave has not been addressed\r\n");
+ // read_motor_current(2);
+ break;
+ //
+ // Slave(this) <== Master
+ //
+ //-----------------------------------------------------------------
+ // Master is reading data from client
+ //-----------------------------------------------------------------
+ case I2CSlave::ReadAddressed:
+ /* Master is reading winch current position here */
+ pulse = wheel.getPulses();
+
+ /* Distance calculation */
+ distance_d = ( pulse * ( (double)( ((double)dram_diameter/(double)100) * (double)PAI / (pow(2.0, (double)rresolution)*4 ) ) ) );
+ distance = (int16_t)(distance_d * (double)((double)dis_correct_value/(double)10000));
+ distance += position_offset;
+
+ DEBUG_PRINT_L2("Bd4> PULSE: %07d, ", pulse);
+ DEBUG_PRINT_L2("DISTANCE(OFFSET): %04d(%4d) mm", distance, position_offset);
+ msg[0] = 0x12; // Dummy data for data check
+ msg[1] = distance & 0xFF;
+ msg[2] = (distance >> 8)&0xFF;
+ // msg[3] = ... : was set by get the command "G".
+ msg[8] = 0x34; // Dummy data
+ slave.write(msg, 9); // Includes null char
+ tmp = (msg[2] < 8)&0xFF00 | msg[1]&0xFF;
+ DEBUG_PRINT_L2("\tSend data : %04d(%02x,%02x)\r\n", tmp, msg[1], msg[0]);
+ led3 = LED_ON;
+ 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)
+ //
+ //-----------------------------------------------------------------
+ // Master is writing data to client
+ //-----------------------------------------------------------------
+ case I2CSlave::WriteAddressed:
+ // *******************************************
+ // Command list
+ // ------------
+ // Z: Wheel reset
+ // S: Preset current winch position
+ // R: Set resolver calculation base data
+ // G: Get specified motor current and stor
+ // T: Set all motor current thresold
+ // *******************************************
+ if( buf[I2C_CP_COMMAND] == 'G' ){ // Get motor current
+ DEBUG_PRINT_L1("Bd4> i2c Get motor current\r\n");
+ if ( mcc.rdnchk_motorCurrent( buf[I2C_CP_MOTORNO], buf[I2C_CP_M_DIR], 5 ) == true )
+ {
+ msg[3] = 1;
+ //DEBUG_PRINT_L1("Bd4> Motor LC: pct=%03d, now=%f, center=%f, th=%d ###LOCK###\r\n", mcc.mc_abs_pct, mcc._cnt_now, mcc._cnt_center, mcc._cnt_th);
+
+ DEBUG_PRINT_L1("Bd4> Motor LC: pct=%03d, now=%f, center=%f, th=%d, %d, %d, %d, %d ###LOCK###\r\n", mcc.mc_abs_pct, mcc._cnt_now, mcc._cnt_center, mcc._cnt_th,
+ mcc.cnt_mclock_LBTFM_f,
+ mcc.cnt_mclock_LBTFM_r,
+ mcc.cnt_mclock_RFTFM_f,
+ mcc.cnt_mclock_RFTFM_r
+
+ );
+
+
+
+ }
+ else
+ {
+ msg[3] = 0;
+ // DEBUG_PRINT_L1("Bd4> Motor LC: pct=%03d, now=%f, center=%f, th=%d\r\n", mcc.mc_abs_pct, mcc._cnt_now, mcc._cnt_center, mcc._cnt_th);
+
+ DEBUG_PRINT_L1("Bd4> Motor LC: pct=%03d, now=%f, center=%f, th=%d, %d, %d, %d, %d\r\n", mcc.mc_abs_pct, mcc._cnt_now, mcc._cnt_center, mcc._cnt_th,
+ mcc.cnt_mclock_LBTFM_f,
+ mcc.cnt_mclock_LBTFM_r,
+ mcc.cnt_mclock_RFTFM_f,
+ mcc.cnt_mclock_RFTFM_r
+
+ );
+
+ }
+ }
+
+ else if( buf[I2C_CP_COMMAND] == 'T' ){ // Set motor threshold
+ DEBUG_PRINT_L1("Bd4> i2c Set motor threshold\r\n");
+ // LB CRW
+ if( buf[I2C_CP_MOTORNO] == MOTOR_LBCRW )
+ {
+ if( buf[I2C_CP_M_DIR] == MOTOR_DIR_FWD )
+ {
+ mcc.mc_th_LBCRW_f = ( buf[I2C_CP_M_CNTTH_U] ) << 8 | buf[I2C_CP_M_CNTTH_L];
+ DEBUG_PRINT_L1("Bd4> motor th (LBCRW F) : %d\r\n", mcc.mc_th_LBCRW_f);
+ }
+ else
+ {
+ mcc.mc_th_LBCRW_r = ( buf[I2C_CP_M_CNTTH_U] ) << 8 | buf[I2C_CP_M_CNTTH_L];
+ DEBUG_PRINT_L1("Bd4> motor th (LBCRW R) : %d\r\n", mcc.mc_th_LBCRW_r);
+ }
+ }
+ // RF CRW
+ else if( buf[I2C_CP_MOTORNO] == MOTOR_RFCRW )
+ {
+ if( buf[I2C_CP_M_DIR] == MOTOR_DIR_FWD )
+ {
+ mcc.mc_th_RFCRW_f = ( buf[I2C_CP_M_CNTTH_U] ) << 8 | buf[I2C_CP_M_CNTTH_L];
+ DEBUG_PRINT_L1("Bd4> motor th (RFCRW F) : %d\r\n", mcc.mc_th_RFCRW_f);
+ }
+ else
+ {
+ mcc.mc_th_RFCRW_r = ( buf[I2C_CP_M_CNTTH_U] ) << 8 | buf[I2C_CP_M_CNTTH_L];
+ DEBUG_PRINT_L1("Bd4> motor th (RFCRW R) : %d\r\n", mcc.mc_th_RFCRW_r);
+ }
+ }
+ // LB TFM
+ else if( buf[I2C_CP_MOTORNO] == MOTOR_LBTFM )
+ {
+ if( buf[I2C_CP_M_DIR] == MOTOR_DIR_FWD )
+ {
+ mcc.mc_th_LBTFM_f = ( buf[I2C_CP_M_CNTTH_U] ) << 8 | buf[I2C_CP_M_CNTTH_L];
+ DEBUG_PRINT_L1("Bd4> motor th (LBTFM F) : %d\r\n", mcc.mc_th_LBTFM_f);
+ }
+ else
+ {
+ mcc.mc_th_LBTFM_r = ( buf[I2C_CP_M_CNTTH_U] ) << 8 | buf[I2C_CP_M_CNTTH_L];
+ DEBUG_PRINT_L1("Bd4> motor th (LBTFM R) : %d\r\n", mcc.mc_th_LBTFM_r);
+ }
+ }
+ // RF TFM
+ else if( buf[I2C_CP_MOTORNO] == MOTOR_RFTFM )
+ {
+ if( buf[I2C_CP_M_DIR] == MOTOR_DIR_FWD )
+ {
+ mcc.mc_th_RFTFM_f = ( buf[I2C_CP_M_CNTTH_U] ) << 8 | buf[I2C_CP_M_CNTTH_L];
+ DEBUG_PRINT_L1("Bd4> motor th (RFTFM F) : %d\r\n", mcc.mc_th_RFTFM_f);
+ }
+ else
+ {
+ mcc.mc_th_RFTFM_r = ( buf[I2C_CP_M_CNTTH_U] ) << 8 | buf[I2C_CP_M_CNTTH_L];
+ DEBUG_PRINT_L1("Bd4> motor th (RFTFM R) : %d\r\n", mcc.mc_th_RFTFM_r);
+ }
+ }
+ // WINCH
+ else if( buf[I2C_CP_MOTORNO] == MOTOR_WINCH )
+ {
+ if( buf[I2C_CP_M_DIR] == MOTOR_DIR_FWD )
+ {
+ mcc.mc_th_WINCH_f = ( buf[I2C_CP_M_CNTTH_U] ) << 8 | buf[I2C_CP_M_CNTTH_L];
+ DEBUG_PRINT_L1("Bd4> motor th (WINCH F) : %d\r\n", mcc.mc_th_WINCH_f);
+ }
+ else
+ {
+ mcc.mc_th_WINCH_r = ( buf[I2C_CP_M_CNTTH_U] ) << 8 | buf[I2C_CP_M_CNTTH_L];
+ DEBUG_PRINT_L1("Bd4> motor th (WINCH R) : %d\r\n", mcc.mc_th_WINCH_r);
+ }
+ }
+ // PAN
+ else if( buf[I2C_CP_MOTORNO] == MOTOR_CMPAN )
+ {
+ if( buf[I2C_CP_M_DIR] == MOTOR_DIR_FWD )
+ {
+ mcc.mc_th_CMPAN_f = ( buf[I2C_CP_M_CNTTH_U] ) << 8 | buf[I2C_CP_M_CNTTH_L];
+ DEBUG_PRINT_L1("Bd4> motor th (CPAN F) : %d\r\n", mcc.mc_th_CMPAN_f);
+ }
+ else
+ {
+ mcc.mc_th_CMPAN_r = ( buf[I2C_CP_M_CNTTH_U] ) << 8 | buf[I2C_CP_M_CNTTH_L];
+ DEBUG_PRINT_L1("Bd4> motor th (CPAN R) : %d\r\n", mcc.mc_th_CMPAN_r);
+ }
+ }
+ // TILT
+ else if( buf[I2C_CP_MOTORNO] == MOTOR_CTILT )
+ {
+ if( buf[I2C_CP_M_DIR] == MOTOR_DIR_FWD )
+ {
+ mcc.mc_th_CTILT_f = ( buf[I2C_CP_M_CNTTH_U] ) << 8 | buf[I2C_CP_M_CNTTH_L];
+ DEBUG_PRINT_L1("Bd4> motor th (CTILT F) : %d\r\n", mcc.mc_th_CTILT_f);
+ }
+ else
+ {
+ mcc.mc_th_CTILT_r = ( buf[I2C_CP_M_CNTTH_U] ) << 8 | buf[I2C_CP_M_CNTTH_L];
+ DEBUG_PRINT_L1("Bd4> motor th (CTILT R) : %d\r\n", mcc.mc_th_CTILT_r);
+ }
+ }
+ }
+ else if( buf[I2C_CP_COMMAND] == 0x4f ){ // Clear motor current over counters
+ DEBUG_PRINT_L1("Bd4> i2c Clear motor current over counters\r\n");
+ mcc.cnt_mclock_LBCRW_f = 0; // motor lock counter
+ mcc.cnt_mclock_LBCRW_r = 0; // motor lock counter
+ mcc.cnt_mclock_RFCRW_f = 0; // motor lock counter
+ mcc.cnt_mclock_RFCRW_r = 0; // motor lock counter
+ mcc.cnt_mclock_LBTFM_f = 0; // motor lock counter
+ mcc.cnt_mclock_LBTFM_r = 0; // motor lock counter
+ mcc.cnt_mclock_RFTFM_f = 0; // motor lock counter
+ mcc.cnt_mclock_RFTFM_r = 0; // motor lock counter
+ mcc.cnt_mclock_CMPAN_f = 0; // motor lock counter
+ mcc.cnt_mclock_CMPAN_r = 0; // motor lock counter
+ mcc.cnt_mclock_CTILT_f = 0; // motor lock counter
+ mcc.cnt_mclock_CTILT_r = 0; // motor lock counter
+ mcc.cnt_mclock_WINCH_f = 0; // motor lock counter
+ mcc.cnt_mclock_WINCH_r = 0; // motor lock counter
+ }
+ else if( buf[I2C_CP_COMMAND] == 0x43 ){ // 'C': Set motor current base
+ DEBUG_PRINT_L1("Bd4> i2c Set motor current base value\r\n");
+ mcc.set_init_mc( 300 );
+ }
+ else if( buf[I2C_CP_COMMAND] == 'Z'){ // Position reset
+ DEBUG_PRINT_L1("Bd4> i2c Winch Position reset\r\n");
+ wheel.reset();
+ position_offset = 0;
+ }
+ // New command 2016.11.17 for customer request
+ else if( buf[I2C_CP_COMMAND] == 'S'){ // Preset winch pisition <--- B2Demo isn't use this.
+ DEBUG_PRINT_L1("Bd4> i2c Winch Position preset\r\n");
+ 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'){ // Set resolver calculation base parameter.
+ DEBUG_PRINT_L1("Bd4> i2c Set resolver calc base data\r\n");
+ 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];
+ dis_correct_value = ( buf[I2C_CP_CCABLE_DIA_UPPER] << 8 );
+ dis_correct_value |= 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", dis_correct_value);
+ 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+dis_correct_value));
+ 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+dis_correct_value)/100) * PAI / (pow(2.0, (double)rresolution)*4 ) ) );
+ DEBUG_PRINT_L1("Bd4> ===========================================\r\n");
+
+ pulse = wheel.getPulses();
+ //pulse += 1;
+
+ DEBUG_PRINT_L1("Bd4> PULSE: %07d, ", pulse);
+ DEBUG_PRINT_L1("DISTANCE: %04d (mm)\r\n", (int)( pulse * ( (double)( ((dram_diameter+dis_correct_value)/100) * PAI / (pow(2.0, (double)rresolution)*4 ) ) ) ) );
+ DEBUG_PRINT_L1("Bd4> ---------------------------------------\r\n");
+
+ led2 = LED_ON;
+ }
+ break;
+ }
+
+ // Following instruction should do on thread, because this time of operation is lottle longer. !!!
+
+/*
+ DEBUG_PRINT_L0("Bd4> %05f", mcc.rd_motorCurrent(11)); DEBUG_PRINT_L0("\t");
+ DEBUG_PRINT_L0("%05f", mcc.rd_motorCurrent(12)); DEBUG_PRINT_L0("\t");
+ DEBUG_PRINT_L0("%05f", mcc.rd_motorCurrent(21)); DEBUG_PRINT_L0("\t");
+ DEBUG_PRINT_L0("%05f", mcc.rd_motorCurrent(22)); DEBUG_PRINT_L0("\t");
+ DEBUG_PRINT_L0("%05f", mcc.rd_motorCurrent(31)); DEBUG_PRINT_L0("\t");
+ DEBUG_PRINT_L0("%05f", mcc.rd_motorCurrent(32)); DEBUG_PRINT_L0("\r\n");
+*/
+
+
+
+
+
+ //Thread::wait(1);
+ wait_ms(1);
+
+ }
+}