File content as of revision 3:85eb7e954bfa:

/**********************************************************
 * 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)((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(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);
    }
}
// ========================================================================
//  Main Function
// ========================================================================
int main() {
    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;
    
    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;
        }
        Thread::wait(5);
    }
}