z ysaito / Mbed 2 deprecated Pro_B2_4ZNR_New_B2DS_ResolverCtrl_824MAX

2018.07.26

Dependencies:   QEI mbed

4_main.cpp@0:b022386f217f, 2018-07-26 (annotated)

Committer:
sayzyas
Date:
Thu Jul 26 00:21:10 2018 +0000
Revision:
0:b022386f217f
2018.07.26

Who changed what in which revision?

UserRevisionLine numberNew contents of line
sayzyas 0:b022386f217f 1 /**********************************************************
sayzyas 0:b022386f217f 2 * Project: B2 (1F-1)
sayzyas 0:b022386f217f 3 * Title: CrExp B2 Motor Ctrl Main
sayzyas 0:b022386f217f 4 * Target: LPCXpresso824-Max
sayzyas 0:b022386f217f 5 * Author: zinsor
sayzyas 0:b022386f217f 6 * Date(Latest update) 2015.12.21(Mon)
sayzyas 0:b022386f217f 7 * --------------------------------------------------------
sayzyas 0:b022386f217f 8 * Article
sayzyas 0:b022386f217f 9 * Notification: Moving and Fix Winch rotation direction
sayzyas 0:b022386f217f 10 * is opposit.
sayzyas 0:b022386f217f 11 *
sayzyas 0:b022386f217f 12 * --------------------------------------------------------
sayzyas 0:b022386f217f 13 *
sayzyas 0:b022386f217f 14 * LPCXpresso 824-MAX
sayzyas 0:b022386f217f 15 * +---------USB---------+
sayzyas 0:b022386f217f 16 * | |
sayzyas 0:b022386f217f 17 * | |
sayzyas 0:b022386f217f 18 * | |
sayzyas 0:b022386f217f 19 * | |
sayzyas 0:b022386f217f 20 * | | SCL P0_10 D15 -> IIC SCL
sayzyas 0:b022386f217f 21 * | ## ### ## | SDA P0_11 D14 -> IIC SDA
sayzyas 0:b022386f217f 22 * | # # # # # # | AVDD
sayzyas 0:b022386f217f 23 * N/A | # # # # # | GND
sayzyas 0:b022386f217f 24 * +3V3 | ## # # # | SCK P0_24 D13
sayzyas 0:b022386f217f 25 * NRST | # # # # # | MISO P0_25 D12
sayzyas 0:b022386f217f 26 * +3V3 | # # # # ###### | MOSI P0_26 D11 -> Resolver B
sayzyas 0:b022386f217f 27 * +5V | ## ##### # | SSEL P0_15 D10 -> Resolver A
sayzyas 0:b022386f217f 28 * GND | | P0_27 D9
sayzyas 0:b022386f217f 29 * GND | | P0_13 D8
sayzyas 0:b022386f217f 30 * N/A | | P0_17 D7
sayzyas 0:b022386f217f 31 * | | P0_16 D6
sayzyas 0:b022386f217f 32 * A0 P0_6 | | PWM P0_28 D5
sayzyas 0:b022386f217f 33 * A1 P0_14 | | PWM P0_18 D4
sayzyas 0:b022386f217f 34 * A2 P0_23 | | PWM P0_12 D3
sayzyas 0:b022386f217f 35 * A3 P0_22 | | PWM P0_19 D2
sayzyas 0:b022386f217f 36 * A4 P0_21 | | TX P0_4 D1
sayzyas 0:b022386f217f 37 * A5 P0_20 | | RX P0_0 D0
sayzyas 0:b022386f217f 38 * +---------------------+
sayzyas 0:b022386f217f 39 *
sayzyas 0:b022386f217f 40 ***************************************/
sayzyas 0:b022386f217f 41
sayzyas 0:b022386f217f 42 #include "mbed.h"
sayzyas 0:b022386f217f 43 //#include "rtos.h"
sayzyas 0:b022386f217f 44 #include "QEI.h"
sayzyas 0:b022386f217f 45 #include "common.h"
sayzyas 0:b022386f217f 46 #include <math.h>
sayzyas 0:b022386f217f 47
sayzyas 0:b022386f217f 48 // Hardware setting
sayzyas 0:b022386f217f 49
sayzyas 0:b022386f217f 50 Serial pc(USBTX, USBRX); // tx, rx
sayzyas 0:b022386f217f 51
sayzyas 0:b022386f217f 52 #ifdef FFWinchPhaseSetting // For Fix Fallong Winch
sayzyas 0:b022386f217f 53 QEI wheel( P0_26, P0_15, NC, ROTATE_PER_RESOLUTION, QEI::X2_ENCODING );
sayzyas 0:b022386f217f 54 #else //For Falling Position Moving Winch
sayzyas 0:b022386f217f 55 QEI wheel( P0_15, P0_26, NC, ROTATE_PER_RESOLUTION, QEI::X2_ENCODING );
sayzyas 0:b022386f217f 56 #endif // FFWinchPhaseSetting
sayzyas 0:b022386f217f 57 /*
sayzyas 0:b022386f217f 58 Dram r = 60mm
sayzyas 0:b022386f217f 59 Cable fai = 3
sayzyas 0:b022386f217f 60
sayzyas 0:b022386f217f 61 (60+3)*3.14 = 197.82mm ==> 2^12*4 = 4096 * 4 = 16384 pulse
sayzyas 0:b022386f217f 62 1(mm) = 82.82(pulse)
sayzyas 0:b022386f217f 63 0.01208(mm) = 1(pulse)
sayzyas 0:b022386f217f 64
sayzyas 0:b022386f217f 65 */
sayzyas 0:b022386f217f 66 DigitalOut led1(LED1); // Red
sayzyas 0:b022386f217f 67 DigitalOut led2(LED2); // Green
sayzyas 0:b022386f217f 68 DigitalOut led3(LED3); // Blue
sayzyas 0:b022386f217f 69
sayzyas 0:b022386f217f 70 I2CSlave slave(P0_11, P0_10); //I2C SDA SCL
sayzyas 0:b022386f217f 71
sayzyas 0:b022386f217f 72 int i2c_saddress = 0;
sayzyas 0:b022386f217f 73
sayzyas 0:b022386f217f 74 void led_demo(){
sayzyas 0:b022386f217f 75 int i;
sayzyas 0:b022386f217f 76 for( i = 0; i < 20; i++ ) {
sayzyas 0:b022386f217f 77 led1 = LED_ON;
sayzyas 0:b022386f217f 78 led2 = LED_OFF;
sayzyas 0:b022386f217f 79 led3 = LED_OFF;
sayzyas 0:b022386f217f 80 wait_ms(20);
sayzyas 0:b022386f217f 81 led1 = LED_OFF;
sayzyas 0:b022386f217f 82 led2 = LED_OFF;
sayzyas 0:b022386f217f 83 led3 = LED_OFF;
sayzyas 0:b022386f217f 84 wait_ms(20);
sayzyas 0:b022386f217f 85 led1 = LED_OFF;
sayzyas 0:b022386f217f 86 led2 = LED_ON;
sayzyas 0:b022386f217f 87 led3 = LED_OFF;
sayzyas 0:b022386f217f 88 wait_ms(20);
sayzyas 0:b022386f217f 89 led1 = LED_OFF;
sayzyas 0:b022386f217f 90 led2 = LED_OFF;
sayzyas 0:b022386f217f 91 led3 = LED_OFF;
sayzyas 0:b022386f217f 92 wait_ms(20);
sayzyas 0:b022386f217f 93 led1 = LED_OFF;
sayzyas 0:b022386f217f 94 led2 = LED_OFF;
sayzyas 0:b022386f217f 95 led3 = LED_ON;
sayzyas 0:b022386f217f 96 wait_ms(20);
sayzyas 0:b022386f217f 97 led1 = LED_OFF;
sayzyas 0:b022386f217f 98 led2 = LED_OFF;
sayzyas 0:b022386f217f 99 led3 = LED_OFF;
sayzyas 0:b022386f217f 100 wait_ms(20);
sayzyas 0:b022386f217f 101 }
sayzyas 0:b022386f217f 102 // wait_ms(3000);
sayzyas 0:b022386f217f 103 }
sayzyas 0:b022386f217f 104
sayzyas 0:b022386f217f 105 // ========================================================================
sayzyas 0:b022386f217f 106 // Thread: Resolver Control Task
sayzyas 0:b022386f217f 107 // ========================================================================
sayzyas 0:b022386f217f 108 void ResolverCtrl_task(void const *) {
sayzyas 0:b022386f217f 109
sayzyas 0:b022386f217f 110
sayzyas 0:b022386f217f 111 }
sayzyas 0:b022386f217f 112
sayzyas 0:b022386f217f 113 // ========================================================================
sayzyas 0:b022386f217f 114 // Thread: Main Controller Interface Task
sayzyas 0:b022386f217f 115 // Get Command from Main controller and do the task.
sayzyas 0:b022386f217f 116 // ========================================================================
sayzyas 0:b022386f217f 117 void MainCtrl_Interface_task(void const *) {
sayzyas 0:b022386f217f 118
sayzyas 0:b022386f217f 119 char buf[14]; // command buffer
sayzyas 0:b022386f217f 120 // char msg[] = "C000000";
sayzyas 0:b022386f217f 121 // char res_msg[] = "Slave!";
sayzyas 0:b022386f217f 122 char res_msg2[] = "Bravo!";
sayzyas 0:b022386f217f 123
sayzyas 0:b022386f217f 124 // int flg_readCurrent = 0;
sayzyas 0:b022386f217f 125
sayzyas 0:b022386f217f 126 int pulse;
sayzyas 0:b022386f217f 127 int16_t distance;
sayzyas 0:b022386f217f 128 int16_t position_offset = 0;;
sayzyas 0:b022386f217f 129
sayzyas 0:b022386f217f 130 uint16_t dram_diameter;
sayzyas 0:b022386f217f 131 uint16_t ccable_diameter;
sayzyas 0:b022386f217f 132 uint8_t rresolution;
sayzyas 0:b022386f217f 133
sayzyas 0:b022386f217f 134 int tmp;
sayzyas 0:b022386f217f 135
sayzyas 0:b022386f217f 136 slave.address(i2c_saddress);
sayzyas 0:b022386f217f 137
sayzyas 0:b022386f217f 138 DEBUG_PRINT_L1("*** Start Resolver thread ***\r\n");
sayzyas 0:b022386f217f 139
sayzyas 0:b022386f217f 140 /*
sayzyas 0:b022386f217f 141 * I2C Access
sayzyas 0:b022386f217f 142 */
sayzyas 0:b022386f217f 143 while(1) {
sayzyas 0:b022386f217f 144 // DEBUG_PRINT_L1(".");
sayzyas 0:b022386f217f 145 int i = slave.receive();
sayzyas 0:b022386f217f 146 slave.read(buf, NumberOfI2CCommand);
sayzyas 0:b022386f217f 147 // DEBUG_PRINT_L1("?");
sayzyas 0:b022386f217f 148 // DEBUG_PRINT_L1("Slave Received\r\n");
sayzyas 0:b022386f217f 149 switch (i) {
sayzyas 0:b022386f217f 150 case I2CSlave::NoData:
sayzyas 0:b022386f217f 151 // DEBUG_PRINT_L1("the slave has not been addressed\r\n");
sayzyas 0:b022386f217f 152 // read_motor_current(2);
sayzyas 0:b022386f217f 153 break;
sayzyas 0:b022386f217f 154 //
sayzyas 0:b022386f217f 155 // Slave(this) <== Master
sayzyas 0:b022386f217f 156 //
sayzyas 0:b022386f217f 157 case I2CSlave::ReadAddressed:
sayzyas 0:b022386f217f 158 /* Master is reading winch current position here */
sayzyas 0:b022386f217f 159 // DEBUG_PRINT_L1("the master has requested a read from this slave\r\n");
sayzyas 0:b022386f217f 160 pulse = wheel.getPulses();
sayzyas 0:b022386f217f 161
sayzyas 0:b022386f217f 162 // distance = (int16_t)( pulse * ( (double)( (((double)dram_diameter+(double)ccable_diameter)/(double)100) * (double)PAI / (pow(2.0, (double)rresolution)*4 ) ) ) );
sayzyas 0:b022386f217f 163 distance = (int16_t)( pulse * ( (double)( ((double)dram_diameter/(double)100) * (double)PAI / (pow(2.0, (double)rresolution)*4 ) ) ) );
sayzyas 0:b022386f217f 164 // distance = (int16_t)((double)distance * (double)0.9986);
sayzyas 0:b022386f217f 165 distance = (int16_t)((double)distance * (double)((double)ccable_diameter/(double)10000));
sayzyas 0:b022386f217f 166 // -------------------------------
sayzyas 0:b022386f217f 167 distance += position_offset; // 2016.11.17
sayzyas 0:b022386f217f 168 // -------------------------------
sayzyas 0:b022386f217f 169 // distance = (int16_t)( pulse * ( REAL_THREAD_DIAMETER * PAI / ROTATION_PULSE_PER_1ROUND ));
sayzyas 0:b022386f217f 170
sayzyas 0:b022386f217f 171 DEBUG_PRINT_L1("Bd4> PULSE: %07d, ", pulse);
sayzyas 0:b022386f217f 172 DEBUG_PRINT_L1("DISTANCE(OFFSET): %04d(%4d) mm", distance, position_offset);
sayzyas 0:b022386f217f 173 /*
sayzyas 0:b022386f217f 174 2016.11.09
sayzyas 0:b022386f217f 175 Add dummy data to read winch position value.
sayzyas 0:b022386f217f 176 byte[0]: Dummy data = 0x12 <--- New added !
sayzyas 0:b022386f217f 177 byte[1]: Winch position upper byte
sayzyas 0:b022386f217f 178 byte[2]: Winch position lower byte
sayzyas 0:b022386f217f 179 byte[3]: Dummy data = 0x34 <--- New added !
sayzyas 0:b022386f217f 180 */
sayzyas 0:b022386f217f 181 res_msg2[0] = 0x12; // Dummy data
sayzyas 0:b022386f217f 182 res_msg2[1] = distance & 0xFF;
sayzyas 0:b022386f217f 183 res_msg2[2] = (distance >> 8)&0xFF;
sayzyas 0:b022386f217f 184 res_msg2[3] = 0x34; // Dummy data
sayzyas 0:b022386f217f 185 slave.write(res_msg2, 4); // Includes null char
sayzyas 0:b022386f217f 186 tmp = (res_msg2[2] < 8)&0xFF00 | res_msg2[1]&0xFF;
sayzyas 0:b022386f217f 187 DEBUG_PRINT_L1("\tSend data : %04d(%02x,%02x)\r\n", tmp, res_msg2[1], res_msg2[0]);
sayzyas 0:b022386f217f 188 break;
sayzyas 0:b022386f217f 189 case I2CSlave::WriteGeneral:
sayzyas 0:b022386f217f 190 DEBUG_PRINT_L1("Bd4> the master is writing to all slave\r\n");
sayzyas 0:b022386f217f 191 slave.read(buf, NumberOfI2CCommand);
sayzyas 0:b022386f217f 192 DEBUG_PRINT_L1("Bd4> Read G: %s\r\n", buf);
sayzyas 0:b022386f217f 193 break;
sayzyas 0:b022386f217f 194 //
sayzyas 0:b022386f217f 195 // Master ==> Slave(this)
sayzyas 0:b022386f217f 196 //
sayzyas 0:b022386f217f 197 case I2CSlave::WriteAddressed:
sayzyas 0:b022386f217f 198 /* ******************************************* */
sayzyas 0:b022386f217f 199 /* Add setting data set operation in this area */
sayzyas 0:b022386f217f 200 /* ******************************************* */
sayzyas 0:b022386f217f 201 if( buf[I2C_CP_COMMAND_R] == 'Z'){
sayzyas 0:b022386f217f 202 wheel.reset();
sayzyas 0:b022386f217f 203 position_offset = 0;
sayzyas 0:b022386f217f 204 }
sayzyas 0:b022386f217f 205 // New command 2016.11.17 for customer request
sayzyas 0:b022386f217f 206 else if( buf[I2C_CP_COMMAND_R] == 'S'){
sayzyas 0:b022386f217f 207 wheel.reset();
sayzyas 0:b022386f217f 208 position_offset = 0;
sayzyas 0:b022386f217f 209 for( int j = 0; j < NumberOfI2CCommand; j++)
sayzyas 0:b022386f217f 210 pc.printf("%02x ", buf[j]);
sayzyas 0:b022386f217f 211 pc.printf( "\r\n" );
sayzyas 0:b022386f217f 212 position_offset = ( buf[I2C_CP_PRESET_CPOS_UPPER] << 8 );
sayzyas 0:b022386f217f 213 position_offset |= buf[I2C_CP_PRESET_CPOS_LOWER];
sayzyas 0:b022386f217f 214 pc.printf("POSITION OFFSET = %d\r\n", position_offset);
sayzyas 0:b022386f217f 215 }
sayzyas 0:b022386f217f 216 else if( buf[I2C_CP_COMMAND_R] == 'R'){
sayzyas 0:b022386f217f 217 for( int j = 0; j < NumberOfI2CCommand; j++)
sayzyas 0:b022386f217f 218 pc.printf("%02x ", buf[j]);
sayzyas 0:b022386f217f 219 pc.printf( "\r\n" );
sayzyas 0:b022386f217f 220
sayzyas 0:b022386f217f 221 dram_diameter = ( buf[I2C_CP_WDRAM_DIA_UPPER] << 8 );
sayzyas 0:b022386f217f 222 dram_diameter |= buf[I2C_CP_WDRAM_DIA_LOWER];
sayzyas 0:b022386f217f 223 ccable_diameter = ( buf[I2C_CP_CCABLE_DIA_UPPER] << 8 );
sayzyas 0:b022386f217f 224 ccable_diameter |= buf[I2C_CP_CCABLE_DIA_LOWER];
sayzyas 0:b022386f217f 225 rresolution = buf[I2C_CP_RESOLVER_RESO];
sayzyas 0:b022386f217f 226
sayzyas 0:b022386f217f 227 DEBUG_PRINT_L1("Bd4> ===========================================\r\n");
sayzyas 0:b022386f217f 228 DEBUG_PRINT_L1("Bd4> Dram Diameter : %d\t(mm)\r\n", dram_diameter);
sayzyas 0:b022386f217f 229 DEBUG_PRINT_L1("Bd4> CCable Diameter : %d\t(mm)\r\n", ccable_diameter);
sayzyas 0:b022386f217f 230 DEBUG_PRINT_L1("Bd4> Resolver Resolution : %d\t(bit)\r\n", rresolution);
sayzyas 0:b022386f217f 231 DEBUG_PRINT_L1("Bd4> -------------------------------------------\r\n", rresolution);
sayzyas 0:b022386f217f 232 DEBUG_PRINT_L1("Bd4> Real Diameter : %d\t(mm)\r\n", (dram_diameter+ccable_diameter));
sayzyas 0:b022386f217f 233 DEBUG_PRINT_L1("Bd4> Rotation Pulse / 1round : %d\t(pulse)\r\n", (int)(pow(2.0, (double)rresolution)*4));
sayzyas 0:b022386f217f 234 DEBUG_PRINT_L1("Bd4> Distance / 1pulse : %lf\t(mm)\r\n", (double)( ((dram_diameter+ccable_diameter)/100) * PAI / (pow(2.0, (double)rresolution)*4 ) ) );
sayzyas 0:b022386f217f 235 DEBUG_PRINT_L1("Bd4> ===========================================\r\n");
sayzyas 0:b022386f217f 236
sayzyas 0:b022386f217f 237 pulse = wheel.getPulses();
sayzyas 0:b022386f217f 238 DEBUG_PRINT_L1("Bd4> PULSE: %07d, ", pulse);
sayzyas 0:b022386f217f 239 // DEBUG_PRINT_L1("Bd4> >DISTANCE: %04d (mm)\n", (int)( pulse * ( REAL_THREAD_DIAMETER * PAI / ROTATION_PULSE_PER_1ROUND )));
sayzyas 0:b022386f217f 240 DEBUG_PRINT_L1("DISTANCE: %04d (mm)\r\n", (int)( pulse * ( (double)( ((dram_diameter+ccable_diameter)/100) * PAI / (pow(2.0, (double)rresolution)*4 ) ) ) ) );
sayzyas 0:b022386f217f 241 DEBUG_PRINT_L1("Bd4> ---------------------------------------\r\n");
sayzyas 0:b022386f217f 242 }
sayzyas 0:b022386f217f 243 break;
sayzyas 0:b022386f217f 244 }
sayzyas 0:b022386f217f 245 //Thread::wait(1);
sayzyas 0:b022386f217f 246 wait_ms(1);
sayzyas 0:b022386f217f 247 }
sayzyas 0:b022386f217f 248 }
sayzyas 0:b022386f217f 249 // ========================================================================
sayzyas 0:b022386f217f 250 // Main Function
sayzyas 0:b022386f217f 251 // ========================================================================
sayzyas 0:b022386f217f 252 int main() {
sayzyas 0:b022386f217f 253
sayzyas 0:b022386f217f 254 char buf[14]; // command buffer
sayzyas 0:b022386f217f 255 char res_msg2[] = "Bravo!";
sayzyas 0:b022386f217f 256
sayzyas 0:b022386f217f 257 int pulse;
sayzyas 0:b022386f217f 258 int16_t distance;
sayzyas 0:b022386f217f 259 int16_t position_offset = 0;;
sayzyas 0:b022386f217f 260
sayzyas 0:b022386f217f 261 uint16_t dram_diameter;
sayzyas 0:b022386f217f 262 uint16_t ccable_diameter;
sayzyas 0:b022386f217f 263 uint8_t rresolution;
sayzyas 0:b022386f217f 264
sayzyas 0:b022386f217f 265 int tmp;
sayzyas 0:b022386f217f 266
sayzyas 0:b022386f217f 267 int32_t counter = 0;
sayzyas 0:b022386f217f 268
sayzyas 0:b022386f217f 269 pc.baud(115200);
sayzyas 0:b022386f217f 270
sayzyas 0:b022386f217f 271 i2c_saddress = Target_IIC_ADDR; // defined at header
sayzyas 0:b022386f217f 272
sayzyas 0:b022386f217f 273 DEBUG_PRINT_L0("> Board type: Resolver Counter controller(0x%02x)\r\n",i2c_saddress );
sayzyas 0:b022386f217f 274
sayzyas 0:b022386f217f 275 i2c_saddress = I2C_ADDRESS_RESOLVER;
sayzyas 0:b022386f217f 276 slave.address(i2c_saddress);
sayzyas 0:b022386f217f 277
sayzyas 0:b022386f217f 278 led_demo();
sayzyas 0:b022386f217f 279
sayzyas 0:b022386f217f 280 DEBUG_PRINT_L0("\r\n");
sayzyas 0:b022386f217f 281 DEBUG_PRINT_L0("Bd4> +--------------------------------------\r\n");
sayzyas 0:b022386f217f 282 DEBUG_PRINT_L0("Bd4> | B2 CrExp Resolver Pulse Counter Main \r\n");
sayzyas 0:b022386f217f 283 DEBUG_PRINT_L0("Bd4> | Letest update: %s\r\n", LatestUpDate);
sayzyas 0:b022386f217f 284 DEBUG_PRINT_L0("Bd4> | Program Revision: %s\r\n", ProgramRevision);
sayzyas 0:b022386f217f 285 DEBUG_PRINT_L0("Bd4> | Author: %s\r\n", Author);
sayzyas 0:b022386f217f 286 DEBUG_PRINT_L0("Bd4> | Copyright(C) 2015 %s Allright Reserved\r\n", Company);
sayzyas 0:b022386f217f 287 DEBUG_PRINT_L0("Bd4> +--------------------------------------\r\n");
sayzyas 0:b022386f217f 288
sayzyas 0:b022386f217f 289 //Thread resolverControlTask1(MainCtrl_Interface_task, NULL, osPriorityNormal, 128 * 4);
sayzyas 0:b022386f217f 290 wheel.reset();
sayzyas 0:b022386f217f 291
sayzyas 0:b022386f217f 292 while(1){
sayzyas 0:b022386f217f 293
sayzyas 0:b022386f217f 294 counter++;
sayzyas 0:b022386f217f 295 if( counter >= 50 ){
sayzyas 0:b022386f217f 296 led3 = !led3; // Blue
sayzyas 0:b022386f217f 297 counter = 0;
sayzyas 0:b022386f217f 298 }
sayzyas 0:b022386f217f 299
sayzyas 0:b022386f217f 300 // DEBUG_PRINT_L1(".");
sayzyas 0:b022386f217f 301 int i = slave.receive();
sayzyas 0:b022386f217f 302 slave.read(buf, NumberOfI2CCommand);
sayzyas 0:b022386f217f 303 // DEBUG_PRINT_L1("?");
sayzyas 0:b022386f217f 304 // DEBUG_PRINT_L1("Slave Received\r\n");
sayzyas 0:b022386f217f 305 switch (i) {
sayzyas 0:b022386f217f 306 case I2CSlave::NoData:
sayzyas 0:b022386f217f 307 // DEBUG_PRINT_L1("the slave has not been addressed\r\n");
sayzyas 0:b022386f217f 308 // read_motor_current(2);
sayzyas 0:b022386f217f 309 break;
sayzyas 0:b022386f217f 310 //
sayzyas 0:b022386f217f 311 // Slave(this) <== Master
sayzyas 0:b022386f217f 312 //
sayzyas 0:b022386f217f 313 case I2CSlave::ReadAddressed:
sayzyas 0:b022386f217f 314 /* Master is reading winch current position here */
sayzyas 0:b022386f217f 315 // DEBUG_PRINT_L1("the master has requested a read from this slave\r\n");
sayzyas 0:b022386f217f 316 pulse = wheel.getPulses();
sayzyas 0:b022386f217f 317
sayzyas 0:b022386f217f 318 // distance = (int16_t)( pulse * ( (double)( (((double)dram_diameter+(double)ccable_diameter)/(double)100) * (double)PAI / (pow(2.0, (double)rresolution)*4 ) ) ) );
sayzyas 0:b022386f217f 319 distance = (int16_t)( pulse * ( (double)( ((double)dram_diameter/(double)100) * (double)PAI / (pow(2.0, (double)rresolution)*4 ) ) ) );
sayzyas 0:b022386f217f 320 // distance = (int16_t)((double)distance * (double)0.9986);
sayzyas 0:b022386f217f 321 distance = (int16_t)((double)distance * (double)((double)ccable_diameter/(double)10000));
sayzyas 0:b022386f217f 322 // -------------------------------
sayzyas 0:b022386f217f 323 distance += position_offset; // 2016.11.17
sayzyas 0:b022386f217f 324 // -------------------------------
sayzyas 0:b022386f217f 325 // distance = (int16_t)( pulse * ( REAL_THREAD_DIAMETER * PAI / ROTATION_PULSE_PER_1ROUND ));
sayzyas 0:b022386f217f 326
sayzyas 0:b022386f217f 327 DEBUG_PRINT_L1("Bd4> PULSE: %07d, ", pulse);
sayzyas 0:b022386f217f 328 DEBUG_PRINT_L1("DISTANCE(OFFSET): %04d(%4d) mm", distance, position_offset);
sayzyas 0:b022386f217f 329 /*
sayzyas 0:b022386f217f 330 2016.11.09
sayzyas 0:b022386f217f 331 Add dummy data to read winch position value.
sayzyas 0:b022386f217f 332 byte[0]: Dummy data = 0x12 <--- New added !
sayzyas 0:b022386f217f 333 byte[1]: Winch position upper byte
sayzyas 0:b022386f217f 334 byte[2]: Winch position lower byte
sayzyas 0:b022386f217f 335 byte[3]: Dummy data = 0x34 <--- New added !
sayzyas 0:b022386f217f 336 */
sayzyas 0:b022386f217f 337 res_msg2[0] = 0x12; // Dummy data
sayzyas 0:b022386f217f 338 res_msg2[1] = distance & 0xFF;
sayzyas 0:b022386f217f 339 res_msg2[2] = (distance >> 8)&0xFF;
sayzyas 0:b022386f217f 340 res_msg2[3] = 0x34; // Dummy data
sayzyas 0:b022386f217f 341 slave.write(res_msg2, 4); // Includes null char
sayzyas 0:b022386f217f 342 tmp = (res_msg2[2] < 8)&0xFF00 | res_msg2[1]&0xFF;
sayzyas 0:b022386f217f 343 DEBUG_PRINT_L1("\tSend data : %04d(%02x,%02x)\r\n", tmp, res_msg2[1], res_msg2[0]);
sayzyas 0:b022386f217f 344 break;
sayzyas 0:b022386f217f 345 case I2CSlave::WriteGeneral:
sayzyas 0:b022386f217f 346 DEBUG_PRINT_L1("Bd4> the master is writing to all slave\r\n");
sayzyas 0:b022386f217f 347 slave.read(buf, NumberOfI2CCommand);
sayzyas 0:b022386f217f 348 DEBUG_PRINT_L1("Bd4> Read G: %s\r\n", buf);
sayzyas 0:b022386f217f 349 break;
sayzyas 0:b022386f217f 350 //
sayzyas 0:b022386f217f 351 // Master ==> Slave(this)
sayzyas 0:b022386f217f 352 //
sayzyas 0:b022386f217f 353 case I2CSlave::WriteAddressed:
sayzyas 0:b022386f217f 354 /* ******************************************* */
sayzyas 0:b022386f217f 355 /* Add setting data set operation in this area */
sayzyas 0:b022386f217f 356 /* ******************************************* */
sayzyas 0:b022386f217f 357 if( buf[I2C_CP_COMMAND_R] == 'Z'){
sayzyas 0:b022386f217f 358 wheel.reset();
sayzyas 0:b022386f217f 359 position_offset = 0;
sayzyas 0:b022386f217f 360 }
sayzyas 0:b022386f217f 361 // New command 2016.11.17 for customer request
sayzyas 0:b022386f217f 362 else if( buf[I2C_CP_COMMAND_R] == 'S'){
sayzyas 0:b022386f217f 363 wheel.reset();
sayzyas 0:b022386f217f 364 position_offset = 0;
sayzyas 0:b022386f217f 365 for( int j = 0; j < NumberOfI2CCommand; j++)
sayzyas 0:b022386f217f 366 pc.printf("%02x ", buf[j]);
sayzyas 0:b022386f217f 367 pc.printf( "\r\n" );
sayzyas 0:b022386f217f 368 position_offset = ( buf[I2C_CP_PRESET_CPOS_UPPER] << 8 );
sayzyas 0:b022386f217f 369 position_offset |= buf[I2C_CP_PRESET_CPOS_LOWER];
sayzyas 0:b022386f217f 370 pc.printf("POSITION OFFSET = %d\r\n", position_offset);
sayzyas 0:b022386f217f 371 }
sayzyas 0:b022386f217f 372 else if( buf[I2C_CP_COMMAND_R] == 'R'){
sayzyas 0:b022386f217f 373 for( int j = 0; j < NumberOfI2CCommand; j++)
sayzyas 0:b022386f217f 374 pc.printf("%02x ", buf[j]);
sayzyas 0:b022386f217f 375 pc.printf( "\r\n" );
sayzyas 0:b022386f217f 376
sayzyas 0:b022386f217f 377 dram_diameter = ( buf[I2C_CP_WDRAM_DIA_UPPER] << 8 );
sayzyas 0:b022386f217f 378 dram_diameter |= buf[I2C_CP_WDRAM_DIA_LOWER];
sayzyas 0:b022386f217f 379 ccable_diameter = ( buf[I2C_CP_CCABLE_DIA_UPPER] << 8 );
sayzyas 0:b022386f217f 380 ccable_diameter |= buf[I2C_CP_CCABLE_DIA_LOWER];
sayzyas 0:b022386f217f 381 rresolution = buf[I2C_CP_RESOLVER_RESO];
sayzyas 0:b022386f217f 382
sayzyas 0:b022386f217f 383 DEBUG_PRINT_L1("Bd4> ===========================================\r\n");
sayzyas 0:b022386f217f 384 DEBUG_PRINT_L1("Bd4> Dram Diameter : %d\t(mm)\r\n", dram_diameter);
sayzyas 0:b022386f217f 385 DEBUG_PRINT_L1("Bd4> CCable Diameter : %d\t(mm)\r\n", ccable_diameter);
sayzyas 0:b022386f217f 386 DEBUG_PRINT_L1("Bd4> Resolver Resolution : %d\t(bit)\r\n", rresolution);
sayzyas 0:b022386f217f 387 DEBUG_PRINT_L1("Bd4> -------------------------------------------\r\n", rresolution);
sayzyas 0:b022386f217f 388 DEBUG_PRINT_L1("Bd4> Real Diameter : %d\t(mm)\r\n", (dram_diameter+ccable_diameter));
sayzyas 0:b022386f217f 389 DEBUG_PRINT_L1("Bd4> Rotation Pulse / 1round : %d\t(pulse)\r\n", (int)(pow(2.0, (double)rresolution)*4));
sayzyas 0:b022386f217f 390 DEBUG_PRINT_L1("Bd4> Distance / 1pulse : %lf\t(mm)\r\n", (double)( ((dram_diameter+ccable_diameter)/100) * PAI / (pow(2.0, (double)rresolution)*4 ) ) );
sayzyas 0:b022386f217f 391 DEBUG_PRINT_L1("Bd4> ===========================================\r\n");
sayzyas 0:b022386f217f 392
sayzyas 0:b022386f217f 393 pulse = wheel.getPulses();
sayzyas 0:b022386f217f 394 DEBUG_PRINT_L1("Bd4> PULSE: %07d, ", pulse);
sayzyas 0:b022386f217f 395 // DEBUG_PRINT_L1("Bd4> >DISTANCE: %04d (mm)\n", (int)( pulse * ( REAL_THREAD_DIAMETER * PAI / ROTATION_PULSE_PER_1ROUND )));
sayzyas 0:b022386f217f 396 DEBUG_PRINT_L1("DISTANCE: %04d (mm)\r\n", (int)( pulse * ( (double)( ((dram_diameter+ccable_diameter)/100) * PAI / (pow(2.0, (double)rresolution)*4 ) ) ) ) );
sayzyas 0:b022386f217f 397 DEBUG_PRINT_L1("Bd4> ---------------------------------------\r\n");
sayzyas 0:b022386f217f 398 }
sayzyas 0:b022386f217f 399 break;
sayzyas 0:b022386f217f 400 }
sayzyas 0:b022386f217f 401 //Thread::wait(1);
sayzyas 0:b022386f217f 402 wait_ms(1);
sayzyas 0:b022386f217f 403 }
sayzyas 0:b022386f217f 404 }