z ysaito / Mbed 2 deprecated CPro_DemoB1B2_4_ResAnaCtrl

2018.08.03 1st commit of demo main res ctrl

Dependencies:   QEI mbed

P1_1Y_main.cpp@1:85e8e2c8f283, 2018-08-03 (annotated)

Committer:
sayzyas
Date:
Fri Aug 03 02:28:22 2018 +0000
Revision:
1:85e8e2c8f283
Parent:
0:f8373bf202a6 
2018.08.03

Who changed what in which revision?

UserRevisionLine numberNew contents of line
sayzyas 0:f8373bf202a6 1 /**********************************************************
sayzyas 0:f8373bf202a6 2 * Project: B2 (1F-1)
sayzyas 0:f8373bf202a6 3 * Title: CrExp B2 Motor Ctrl Main
sayzyas 0:f8373bf202a6 4 * Target: LPCXpresso824-Max
sayzyas 0:f8373bf202a6 5 * Author: zinsor
sayzyas 0:f8373bf202a6 6 * Date(Latest update) 2015.12.21(Mon)
sayzyas 0:f8373bf202a6 7 * --------------------------------------------------------
sayzyas 0:f8373bf202a6 8 * Article
sayzyas 0:f8373bf202a6 9 * Notification: Moving and Fix Winch rotation direction
sayzyas 0:f8373bf202a6 10 * is opposit.
sayzyas 0:f8373bf202a6 11 *
sayzyas 0:f8373bf202a6 12 * --------------------------------------------------------
sayzyas 0:f8373bf202a6 13 *
sayzyas 0:f8373bf202a6 14 * LPCXpresso 824-MAX
sayzyas 0:f8373bf202a6 15 * +---------USB---------+
sayzyas 0:f8373bf202a6 16 * | |
sayzyas 0:f8373bf202a6 17 * | |
sayzyas 0:f8373bf202a6 18 * | |
sayzyas 0:f8373bf202a6 19 * | |
sayzyas 0:f8373bf202a6 20 * | | SCL P0_10 D15 -> IIC SCL
sayzyas 0:f8373bf202a6 21 * | ## ### ## | SDA P0_11 D14 -> IIC SDA
sayzyas 0:f8373bf202a6 22 * | # # # # # # | AVDD
sayzyas 0:f8373bf202a6 23 * N/A | # # # # # | GND
sayzyas 0:f8373bf202a6 24 * +3V3 | ## # # # | SCK P0_24 D13
sayzyas 0:f8373bf202a6 25 * NRST | # # # # # | MISO P0_25 D12
sayzyas 0:f8373bf202a6 26 * +3V3 | # # # # ###### | MOSI P0_26 D11 -> Resolver B
sayzyas 0:f8373bf202a6 27 * +5V | ## ##### # | SSEL P0_15 D10 -> Resolver A
sayzyas 0:f8373bf202a6 28 * GND | | P0_27 D9
sayzyas 0:f8373bf202a6 29 * GND | | P0_13 D8
sayzyas 0:f8373bf202a6 30 * N/A | | P0_17 D7
sayzyas 0:f8373bf202a6 31 * | | P0_16 D6
sayzyas 0:f8373bf202a6 32 * A0 P0_6 | | PWM P0_28 D5
sayzyas 0:f8373bf202a6 33 * A1 P0_14 | | PWM P0_18 D4
sayzyas 0:f8373bf202a6 34 * A2 P0_23 | | PWM P0_12 D3
sayzyas 0:f8373bf202a6 35 * A3 P0_22 | | PWM P0_19 D2
sayzyas 0:f8373bf202a6 36 * A4 P0_21 | | TX P0_4 D1
sayzyas 0:f8373bf202a6 37 * A5 P0_20 | | RX P0_0 D0
sayzyas 0:f8373bf202a6 38 * +---------------------+
sayzyas 0:f8373bf202a6 39 *
sayzyas 0:f8373bf202a6 40 ***************************************/
sayzyas 0:f8373bf202a6 41
sayzyas 0:f8373bf202a6 42 #include "mbed.h"
sayzyas 0:f8373bf202a6 43 //#include "rtos.h"
sayzyas 0:f8373bf202a6 44 #include "QEI.h"
sayzyas 0:f8373bf202a6 45 #include "common.h"
sayzyas 0:f8373bf202a6 46 #include <math.h>
sayzyas 0:f8373bf202a6 47 #include "mcchk.h"
sayzyas 0:f8373bf202a6 48
sayzyas 0:f8373bf202a6 49 // Hardware setting
sayzyas 0:f8373bf202a6 50
sayzyas 0:f8373bf202a6 51 Serial pc(USBTX, USBRX); // tx, rx
sayzyas 0:f8373bf202a6 52
sayzyas 0:f8373bf202a6 53 #ifdef FFWinchPhaseSetting // For Fix Fallong Winch
sayzyas 0:f8373bf202a6 54 QEI wheel( P0_26, P0_15, NC, ROTATE_PER_RESOLUTION, QEI::X2_ENCODING );
sayzyas 0:f8373bf202a6 55 #else //For Falling Position Moving Winch
sayzyas 0:f8373bf202a6 56 QEI wheel( P0_15, P0_26, NC, ROTATE_PER_RESOLUTION, QEI::X2_ENCODING );
sayzyas 0:f8373bf202a6 57 #endif // FFWinchPhaseSetting
sayzyas 0:f8373bf202a6 58
sayzyas 0:f8373bf202a6 59 /*
sayzyas 0:f8373bf202a6 60 Dram r = 60mm
sayzyas 0:f8373bf202a6 61 Cable fai = 3
sayzyas 0:f8373bf202a6 62
sayzyas 0:f8373bf202a6 63 (60+3)*3.14 = 197.82mm ==> 2^12*4 = 4096 * 4 = 16384 pulse
sayzyas 0:f8373bf202a6 64 1(mm) = 82.82(pulse)
sayzyas 0:f8373bf202a6 65 0.01208(mm) = 1(pulse)
sayzyas 0:f8373bf202a6 66
sayzyas 0:f8373bf202a6 67 */
sayzyas 0:f8373bf202a6 68 DigitalOut led1(LED1); // Red
sayzyas 0:f8373bf202a6 69 DigitalOut led2(LED2); // Green
sayzyas 0:f8373bf202a6 70 DigitalOut led3(LED3); // Blue
sayzyas 0:f8373bf202a6 71
sayzyas 0:f8373bf202a6 72 I2CSlave slave(P0_11, P0_10); //I2C SDA SCL
sayzyas 0:f8373bf202a6 73
sayzyas 0:f8373bf202a6 74 void led_demo(){
sayzyas 0:f8373bf202a6 75 int i;
sayzyas 0:f8373bf202a6 76 for( i = 0; i < 20; i++ ) {
sayzyas 0:f8373bf202a6 77 led1 = LED_ON;
sayzyas 0:f8373bf202a6 78 led2 = LED_OFF;
sayzyas 0:f8373bf202a6 79 led3 = LED_OFF;
sayzyas 0:f8373bf202a6 80 wait_ms(20);
sayzyas 0:f8373bf202a6 81 led1 = LED_OFF;
sayzyas 0:f8373bf202a6 82 led2 = LED_OFF;
sayzyas 0:f8373bf202a6 83 led3 = LED_OFF;
sayzyas 0:f8373bf202a6 84 wait_ms(20);
sayzyas 0:f8373bf202a6 85 led1 = LED_OFF;
sayzyas 0:f8373bf202a6 86 led2 = LED_ON;
sayzyas 0:f8373bf202a6 87 led3 = LED_OFF;
sayzyas 0:f8373bf202a6 88 wait_ms(20);
sayzyas 0:f8373bf202a6 89 led1 = LED_OFF;
sayzyas 0:f8373bf202a6 90 led2 = LED_OFF;
sayzyas 0:f8373bf202a6 91 led3 = LED_OFF;
sayzyas 0:f8373bf202a6 92 wait_ms(20);
sayzyas 0:f8373bf202a6 93 led1 = LED_OFF;
sayzyas 0:f8373bf202a6 94 led2 = LED_OFF;
sayzyas 0:f8373bf202a6 95 led3 = LED_ON;
sayzyas 0:f8373bf202a6 96 wait_ms(20);
sayzyas 0:f8373bf202a6 97 led1 = LED_OFF;
sayzyas 0:f8373bf202a6 98 led2 = LED_OFF;
sayzyas 0:f8373bf202a6 99 led3 = LED_OFF;
sayzyas 0:f8373bf202a6 100 wait_ms(20);
sayzyas 0:f8373bf202a6 101 }
sayzyas 0:f8373bf202a6 102 // wait_ms(3000);
sayzyas 0:f8373bf202a6 103 }
sayzyas 0:f8373bf202a6 104
sayzyas 0:f8373bf202a6 105 // ========================================================================
sayzyas 0:f8373bf202a6 106 // Main Function
sayzyas 0:f8373bf202a6 107 // ========================================================================
sayzyas 0:f8373bf202a6 108 int main() {
sayzyas 0:f8373bf202a6 109
sayzyas 0:f8373bf202a6 110 char buf[12]; // command buffer
sayzyas 0:f8373bf202a6 111 char msg[9];
sayzyas 0:f8373bf202a6 112
sayzyas 0:f8373bf202a6 113 int pulse;
sayzyas 0:f8373bf202a6 114 double distance_d = 0.0f;
sayzyas 0:f8373bf202a6 115 int16_t distance;
sayzyas 0:f8373bf202a6 116 int16_t position_offset = 0;;
sayzyas 0:f8373bf202a6 117
sayzyas 0:f8373bf202a6 118 uint16_t dram_diameter;
sayzyas 0:f8373bf202a6 119 uint16_t dis_correct_value;
sayzyas 0:f8373bf202a6 120 uint8_t rresolution;
sayzyas 0:f8373bf202a6 121
sayzyas 0:f8373bf202a6 122 int tmp;
sayzyas 0:f8373bf202a6 123 mcchk mcc;
sayzyas 0:f8373bf202a6 124
sayzyas 0:f8373bf202a6 125 pc.baud(115200);
sayzyas 0:f8373bf202a6 126
sayzyas 0:f8373bf202a6 127 slave.address(I2C_ADDRESS_RESOLVER);
sayzyas 0:f8373bf202a6 128
sayzyas 0:f8373bf202a6 129 led_demo();
sayzyas 0:f8373bf202a6 130
sayzyas 0:f8373bf202a6 131 DEBUG_PRINT_L0("\r\n");
sayzyas 0:f8373bf202a6 132 DEBUG_PRINT_L0("Bd4> +--------------------------------------\r\n");
sayzyas 0:f8373bf202a6 133 DEBUG_PRINT_L0("Bd4> | B2 CrExp Resolver Pulse Counter Main \r\n");
sayzyas 0:f8373bf202a6 134 DEBUG_PRINT_L0("Bd4> | Letest update: %s\r\n", LatestUpDate);
sayzyas 0:f8373bf202a6 135 DEBUG_PRINT_L0("Bd4> | Program Revision: %s\r\n", ProgramRevision);
sayzyas 0:f8373bf202a6 136 DEBUG_PRINT_L0("Bd4> | Author: %s\r\n", Author);
sayzyas 0:f8373bf202a6 137 DEBUG_PRINT_L0("Bd4> | Copyright(C) 2015 %s Allright Reserved\r\n", Company);
sayzyas 0:f8373bf202a6 138 DEBUG_PRINT_L0("Bd4> +--------------------------------------\r\n");
sayzyas 0:f8373bf202a6 139
sayzyas 0:f8373bf202a6 140 wheel.reset();
sayzyas 0:f8373bf202a6 141
sayzyas 0:f8373bf202a6 142
sayzyas 0:f8373bf202a6 143
sayzyas 0:f8373bf202a6 144 while(1){
sayzyas 0:f8373bf202a6 145
sayzyas 0:f8373bf202a6 146 led2 = LED_OFF;
sayzyas 0:f8373bf202a6 147 led3 = LED_OFF;
sayzyas 0:f8373bf202a6 148
sayzyas 0:f8373bf202a6 149 int i = slave.receive();
sayzyas 0:f8373bf202a6 150 slave.read(buf, NumberOfI2CCommand);
sayzyas 0:f8373bf202a6 151
sayzyas 0:f8373bf202a6 152 switch (i) {
sayzyas 0:f8373bf202a6 153 case I2CSlave::NoData:
sayzyas 0:f8373bf202a6 154 // DEBUG_PRINT_L1("the slave has not been addressed\r\n");
sayzyas 0:f8373bf202a6 155 // read_motor_current(2);
sayzyas 0:f8373bf202a6 156 break;
sayzyas 0:f8373bf202a6 157 //
sayzyas 0:f8373bf202a6 158 // Slave(this) <== Master
sayzyas 0:f8373bf202a6 159 //
sayzyas 0:f8373bf202a6 160 //-----------------------------------------------------------------
sayzyas 0:f8373bf202a6 161 // Master is reading data from client
sayzyas 0:f8373bf202a6 162 //-----------------------------------------------------------------
sayzyas 0:f8373bf202a6 163 case I2CSlave::ReadAddressed:
sayzyas 0:f8373bf202a6 164 /* Master is reading winch current position here */
sayzyas 0:f8373bf202a6 165 pulse = wheel.getPulses();
sayzyas 0:f8373bf202a6 166
sayzyas 0:f8373bf202a6 167 /* Distance calculation */
sayzyas 0:f8373bf202a6 168 distance_d = ( pulse * ( (double)( ((double)dram_diameter/(double)100) * (double)PAI / (pow(2.0, (double)rresolution)*4 ) ) ) );
sayzyas 0:f8373bf202a6 169 distance = (int16_t)(distance_d * (double)((double)dis_correct_value/(double)10000));
sayzyas 0:f8373bf202a6 170 distance += position_offset;
sayzyas 0:f8373bf202a6 171
sayzyas 0:f8373bf202a6 172 DEBUG_PRINT_L2("Bd4> PULSE: %07d, ", pulse);
sayzyas 0:f8373bf202a6 173 DEBUG_PRINT_L2("DISTANCE(OFFSET): %04d(%4d) mm", distance, position_offset);
sayzyas 0:f8373bf202a6 174 msg[0] = 0x12; // Dummy data for data check
sayzyas 0:f8373bf202a6 175 msg[1] = distance & 0xFF;
sayzyas 0:f8373bf202a6 176 msg[2] = (distance >> 8)&0xFF;
sayzyas 0:f8373bf202a6 177 // msg[3] = ... : was set by get the command "G".
sayzyas 0:f8373bf202a6 178 msg[8] = 0x34; // Dummy data
sayzyas 0:f8373bf202a6 179 slave.write(msg, 9); // Includes null char
sayzyas 0:f8373bf202a6 180 tmp = (msg[2] < 8)&0xFF00 | msg[1]&0xFF;
sayzyas 0:f8373bf202a6 181 DEBUG_PRINT_L2("\tSend data : %04d(%02x,%02x)\r\n", tmp, msg[1], msg[0]);
sayzyas 0:f8373bf202a6 182 led3 = LED_ON;
sayzyas 0:f8373bf202a6 183 break;
sayzyas 0:f8373bf202a6 184
sayzyas 0:f8373bf202a6 185 case I2CSlave::WriteGeneral:
sayzyas 0:f8373bf202a6 186 DEBUG_PRINT_L1("Bd4> the master is writing to all slave\r\n");
sayzyas 0:f8373bf202a6 187 slave.read(buf, NumberOfI2CCommand);
sayzyas 0:f8373bf202a6 188 DEBUG_PRINT_L1("Bd4> Read G: %s\r\n", buf);
sayzyas 0:f8373bf202a6 189 break;
sayzyas 0:f8373bf202a6 190
sayzyas 0:f8373bf202a6 191 //
sayzyas 0:f8373bf202a6 192 // Master ==> Slave(this)
sayzyas 0:f8373bf202a6 193 //
sayzyas 0:f8373bf202a6 194 //-----------------------------------------------------------------
sayzyas 0:f8373bf202a6 195 // Master is writing data to client
sayzyas 0:f8373bf202a6 196 //-----------------------------------------------------------------
sayzyas 0:f8373bf202a6 197 case I2CSlave::WriteAddressed:
sayzyas 0:f8373bf202a6 198 // *******************************************
sayzyas 0:f8373bf202a6 199 // Command list
sayzyas 0:f8373bf202a6 200 // ------------
sayzyas 0:f8373bf202a6 201 // Z: Wheel reset
sayzyas 0:f8373bf202a6 202 // S: Preset current winch position
sayzyas 0:f8373bf202a6 203 // R: Set resolver calculation base data
sayzyas 0:f8373bf202a6 204 // G: Get specified motor current and stor
sayzyas 0:f8373bf202a6 205 // T: Set all motor current thresold
sayzyas 0:f8373bf202a6 206 // *******************************************
sayzyas 0:f8373bf202a6 207 if( buf[I2C_CP_COMMAND] == 'G' ){ // Get motor current
sayzyas 0:f8373bf202a6 208 DEBUG_PRINT_L1("Bd4> i2c Get motor current\r\n");
sayzyas 0:f8373bf202a6 209 if ( mcc.rdnchk_motorCurrent( buf[I2C_CP_MOTORNO], buf[I2C_CP_M_DIR], 5 ) == true )
sayzyas 0:f8373bf202a6 210 {
sayzyas 0:f8373bf202a6 211 msg[3] = 1;
sayzyas 0:f8373bf202a6 212 //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);
sayzyas 0:f8373bf202a6 213
sayzyas 0:f8373bf202a6 214 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,
sayzyas 0:f8373bf202a6 215 mcc.cnt_mclock_LBTFM_f,
sayzyas 0:f8373bf202a6 216 mcc.cnt_mclock_LBTFM_r,
sayzyas 0:f8373bf202a6 217 mcc.cnt_mclock_RFTFM_f,
sayzyas 0:f8373bf202a6 218 mcc.cnt_mclock_RFTFM_r
sayzyas 0:f8373bf202a6 219
sayzyas 0:f8373bf202a6 220 );
sayzyas 0:f8373bf202a6 221 }
sayzyas 0:f8373bf202a6 222 else
sayzyas 0:f8373bf202a6 223 {
sayzyas 0:f8373bf202a6 224 msg[3] = 0;
sayzyas 0:f8373bf202a6 225 // 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);
sayzyas 0:f8373bf202a6 226
sayzyas 0:f8373bf202a6 227 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,
sayzyas 0:f8373bf202a6 228 mcc.cnt_mclock_LBTFM_f,
sayzyas 0:f8373bf202a6 229 mcc.cnt_mclock_LBTFM_r,
sayzyas 0:f8373bf202a6 230 mcc.cnt_mclock_RFTFM_f,
sayzyas 0:f8373bf202a6 231 mcc.cnt_mclock_RFTFM_r
sayzyas 0:f8373bf202a6 232
sayzyas 0:f8373bf202a6 233 );
sayzyas 0:f8373bf202a6 234
sayzyas 0:f8373bf202a6 235 }
sayzyas 0:f8373bf202a6 236 }
sayzyas 0:f8373bf202a6 237
sayzyas 0:f8373bf202a6 238 else if( buf[I2C_CP_COMMAND] == 'T' ){ // Set motor threshold
sayzyas 0:f8373bf202a6 239 DEBUG_PRINT_L1("Bd4> i2c Set motor threshold\r\n");
sayzyas 0:f8373bf202a6 240 // LB CRW
sayzyas 0:f8373bf202a6 241 if( buf[I2C_CP_MOTORNO] == MOTOR_LBCRW )
sayzyas 0:f8373bf202a6 242 {
sayzyas 0:f8373bf202a6 243 if( buf[I2C_CP_M_DIR] == MOTOR_DIR_FWD )
sayzyas 0:f8373bf202a6 244 {
sayzyas 0:f8373bf202a6 245 mcc.mc_th_LBCRW_f = ( buf[I2C_CP_M_CNTTH_U] ) << 8 | buf[I2C_CP_M_CNTTH_L];
sayzyas 0:f8373bf202a6 246 DEBUG_PRINT_L1("Bd4> motor th (LBCRW F) : %d\r\n", mcc.mc_th_LBCRW_f);
sayzyas 0:f8373bf202a6 247 }
sayzyas 0:f8373bf202a6 248 else
sayzyas 0:f8373bf202a6 249 {
sayzyas 0:f8373bf202a6 250 mcc.mc_th_LBCRW_r = ( buf[I2C_CP_M_CNTTH_U] ) << 8 | buf[I2C_CP_M_CNTTH_L];
sayzyas 0:f8373bf202a6 251 DEBUG_PRINT_L1("Bd4> motor th (LBCRW R) : %d\r\n", mcc.mc_th_LBCRW_r);
sayzyas 0:f8373bf202a6 252 }
sayzyas 0:f8373bf202a6 253 }
sayzyas 0:f8373bf202a6 254 // RF CRW
sayzyas 0:f8373bf202a6 255 else if( buf[I2C_CP_MOTORNO] == MOTOR_RFCRW )
sayzyas 0:f8373bf202a6 256 {
sayzyas 0:f8373bf202a6 257 if( buf[I2C_CP_M_DIR] == MOTOR_DIR_FWD )
sayzyas 0:f8373bf202a6 258 {
sayzyas 0:f8373bf202a6 259 mcc.mc_th_RFCRW_f = ( buf[I2C_CP_M_CNTTH_U] ) << 8 | buf[I2C_CP_M_CNTTH_L];
sayzyas 0:f8373bf202a6 260 DEBUG_PRINT_L1("Bd4> motor th (RFCRW F) : %d\r\n", mcc.mc_th_RFCRW_f);
sayzyas 0:f8373bf202a6 261 }
sayzyas 0:f8373bf202a6 262 else
sayzyas 0:f8373bf202a6 263 {
sayzyas 0:f8373bf202a6 264 mcc.mc_th_RFCRW_r = ( buf[I2C_CP_M_CNTTH_U] ) << 8 | buf[I2C_CP_M_CNTTH_L];
sayzyas 0:f8373bf202a6 265 DEBUG_PRINT_L1("Bd4> motor th (RFCRW R) : %d\r\n", mcc.mc_th_RFCRW_r);
sayzyas 0:f8373bf202a6 266 }
sayzyas 0:f8373bf202a6 267 }
sayzyas 0:f8373bf202a6 268 // LB TFM
sayzyas 0:f8373bf202a6 269 else if( buf[I2C_CP_MOTORNO] == MOTOR_LBTFM )
sayzyas 0:f8373bf202a6 270 {
sayzyas 0:f8373bf202a6 271 if( buf[I2C_CP_M_DIR] == MOTOR_DIR_FWD )
sayzyas 0:f8373bf202a6 272 {
sayzyas 0:f8373bf202a6 273 mcc.mc_th_LBTFM_f = ( buf[I2C_CP_M_CNTTH_U] ) << 8 | buf[I2C_CP_M_CNTTH_L];
sayzyas 0:f8373bf202a6 274 DEBUG_PRINT_L1("Bd4> motor th (LBTFM F) : %d\r\n", mcc.mc_th_LBTFM_f);
sayzyas 0:f8373bf202a6 275 }
sayzyas 0:f8373bf202a6 276 else
sayzyas 0:f8373bf202a6 277 {
sayzyas 0:f8373bf202a6 278 mcc.mc_th_LBTFM_r = ( buf[I2C_CP_M_CNTTH_U] ) << 8 | buf[I2C_CP_M_CNTTH_L];
sayzyas 0:f8373bf202a6 279 DEBUG_PRINT_L1("Bd4> motor th (LBTFM R) : %d\r\n", mcc.mc_th_LBTFM_r);
sayzyas 0:f8373bf202a6 280 }
sayzyas 0:f8373bf202a6 281 }
sayzyas 0:f8373bf202a6 282 // RF TFM
sayzyas 0:f8373bf202a6 283 else if( buf[I2C_CP_MOTORNO] == MOTOR_RFTFM )
sayzyas 0:f8373bf202a6 284 {
sayzyas 0:f8373bf202a6 285 if( buf[I2C_CP_M_DIR] == MOTOR_DIR_FWD )
sayzyas 0:f8373bf202a6 286 {
sayzyas 0:f8373bf202a6 287 mcc.mc_th_RFTFM_f = ( buf[I2C_CP_M_CNTTH_U] ) << 8 | buf[I2C_CP_M_CNTTH_L];
sayzyas 0:f8373bf202a6 288 DEBUG_PRINT_L1("Bd4> motor th (RFTFM F) : %d\r\n", mcc.mc_th_RFTFM_f);
sayzyas 0:f8373bf202a6 289 }
sayzyas 0:f8373bf202a6 290 else
sayzyas 0:f8373bf202a6 291 {
sayzyas 0:f8373bf202a6 292 mcc.mc_th_RFTFM_r = ( buf[I2C_CP_M_CNTTH_U] ) << 8 | buf[I2C_CP_M_CNTTH_L];
sayzyas 0:f8373bf202a6 293 DEBUG_PRINT_L1("Bd4> motor th (RFTFM R) : %d\r\n", mcc.mc_th_RFTFM_r);
sayzyas 0:f8373bf202a6 294 }
sayzyas 0:f8373bf202a6 295 }
sayzyas 0:f8373bf202a6 296 // WINCH
sayzyas 0:f8373bf202a6 297 else if( buf[I2C_CP_MOTORNO] == MOTOR_WINCH )
sayzyas 0:f8373bf202a6 298 {
sayzyas 0:f8373bf202a6 299 if( buf[I2C_CP_M_DIR] == MOTOR_DIR_FWD )
sayzyas 0:f8373bf202a6 300 {
sayzyas 0:f8373bf202a6 301 mcc.mc_th_WINCH_f = ( buf[I2C_CP_M_CNTTH_U] ) << 8 | buf[I2C_CP_M_CNTTH_L];
sayzyas 0:f8373bf202a6 302 DEBUG_PRINT_L1("Bd4> motor th (WINCH F) : %d\r\n", mcc.mc_th_WINCH_f);
sayzyas 0:f8373bf202a6 303 }
sayzyas 0:f8373bf202a6 304 else
sayzyas 0:f8373bf202a6 305 {
sayzyas 0:f8373bf202a6 306 mcc.mc_th_WINCH_r = ( buf[I2C_CP_M_CNTTH_U] ) << 8 | buf[I2C_CP_M_CNTTH_L];
sayzyas 0:f8373bf202a6 307 DEBUG_PRINT_L1("Bd4> motor th (WINCH R) : %d\r\n", mcc.mc_th_WINCH_r);
sayzyas 0:f8373bf202a6 308 }
sayzyas 0:f8373bf202a6 309 }
sayzyas 0:f8373bf202a6 310 // PAN
sayzyas 0:f8373bf202a6 311 else if( buf[I2C_CP_MOTORNO] == MOTOR_CMPAN )
sayzyas 0:f8373bf202a6 312 {
sayzyas 0:f8373bf202a6 313 if( buf[I2C_CP_M_DIR] == MOTOR_DIR_FWD )
sayzyas 0:f8373bf202a6 314 {
sayzyas 0:f8373bf202a6 315 mcc.mc_th_CMPAN_f = ( buf[I2C_CP_M_CNTTH_U] ) << 8 | buf[I2C_CP_M_CNTTH_L];
sayzyas 0:f8373bf202a6 316 DEBUG_PRINT_L1("Bd4> motor th (CPAN F) : %d\r\n", mcc.mc_th_CMPAN_f);
sayzyas 0:f8373bf202a6 317 }
sayzyas 0:f8373bf202a6 318 else
sayzyas 0:f8373bf202a6 319 {
sayzyas 0:f8373bf202a6 320 mcc.mc_th_CMPAN_r = ( buf[I2C_CP_M_CNTTH_U] ) << 8 | buf[I2C_CP_M_CNTTH_L];
sayzyas 0:f8373bf202a6 321 DEBUG_PRINT_L1("Bd4> motor th (CPAN R) : %d\r\n", mcc.mc_th_CMPAN_r);
sayzyas 0:f8373bf202a6 322 }
sayzyas 0:f8373bf202a6 323 }
sayzyas 0:f8373bf202a6 324 // TILT
sayzyas 0:f8373bf202a6 325 else if( buf[I2C_CP_MOTORNO] == MOTOR_CTILT )
sayzyas 0:f8373bf202a6 326 {
sayzyas 0:f8373bf202a6 327 if( buf[I2C_CP_M_DIR] == MOTOR_DIR_FWD )
sayzyas 0:f8373bf202a6 328 {
sayzyas 0:f8373bf202a6 329 mcc.mc_th_CTILT_f = ( buf[I2C_CP_M_CNTTH_U] ) << 8 | buf[I2C_CP_M_CNTTH_L];
sayzyas 0:f8373bf202a6 330 DEBUG_PRINT_L1("Bd4> motor th (CTILT F) : %d\r\n", mcc.mc_th_CTILT_f);
sayzyas 0:f8373bf202a6 331 }
sayzyas 0:f8373bf202a6 332 else
sayzyas 0:f8373bf202a6 333 {
sayzyas 0:f8373bf202a6 334 mcc.mc_th_CTILT_r = ( buf[I2C_CP_M_CNTTH_U] ) << 8 | buf[I2C_CP_M_CNTTH_L];
sayzyas 0:f8373bf202a6 335 DEBUG_PRINT_L1("Bd4> motor th (CTILT R) : %d\r\n", mcc.mc_th_CTILT_r);
sayzyas 0:f8373bf202a6 336 }
sayzyas 0:f8373bf202a6 337 }
sayzyas 0:f8373bf202a6 338 }
sayzyas 0:f8373bf202a6 339 else if( buf[I2C_CP_COMMAND] == 0x4f ){ // Clear motor current over counters
sayzyas 0:f8373bf202a6 340 DEBUG_PRINT_L1("Bd4> i2c Clear motor current over counters\r\n");
sayzyas 0:f8373bf202a6 341 mcc.cnt_mclock_LBCRW_f = 0; // motor lock counter
sayzyas 0:f8373bf202a6 342 mcc.cnt_mclock_LBCRW_r = 0; // motor lock counter
sayzyas 0:f8373bf202a6 343 mcc.cnt_mclock_RFCRW_f = 0; // motor lock counter
sayzyas 0:f8373bf202a6 344 mcc.cnt_mclock_RFCRW_r = 0; // motor lock counter
sayzyas 0:f8373bf202a6 345 mcc.cnt_mclock_LBTFM_f = 0; // motor lock counter
sayzyas 0:f8373bf202a6 346 mcc.cnt_mclock_LBTFM_r = 0; // motor lock counter
sayzyas 0:f8373bf202a6 347 mcc.cnt_mclock_RFTFM_f = 0; // motor lock counter
sayzyas 0:f8373bf202a6 348 mcc.cnt_mclock_RFTFM_r = 0; // motor lock counter
sayzyas 0:f8373bf202a6 349 mcc.cnt_mclock_CMPAN_f = 0; // motor lock counter
sayzyas 0:f8373bf202a6 350 mcc.cnt_mclock_CMPAN_r = 0; // motor lock counter
sayzyas 0:f8373bf202a6 351 mcc.cnt_mclock_CTILT_f = 0; // motor lock counter
sayzyas 0:f8373bf202a6 352 mcc.cnt_mclock_CTILT_r = 0; // motor lock counter
sayzyas 0:f8373bf202a6 353 mcc.cnt_mclock_WINCH_f = 0; // motor lock counter
sayzyas 0:f8373bf202a6 354 mcc.cnt_mclock_WINCH_r = 0; // motor lock counter
sayzyas 0:f8373bf202a6 355 }
sayzyas 0:f8373bf202a6 356 else if( buf[I2C_CP_COMMAND] == 0x43 ){ // 'C': Set motor current base
sayzyas 0:f8373bf202a6 357 DEBUG_PRINT_L1("Bd4> i2c Set motor current base value\r\n");
sayzyas 0:f8373bf202a6 358 mcc.set_init_mc( 300 );
sayzyas 0:f8373bf202a6 359 }
sayzyas 0:f8373bf202a6 360 else if( buf[I2C_CP_COMMAND] == 'Z'){ // Position reset
sayzyas 0:f8373bf202a6 361 DEBUG_PRINT_L1("Bd4> i2c Winch Position reset\r\n");
sayzyas 0:f8373bf202a6 362 wheel.reset();
sayzyas 0:f8373bf202a6 363 position_offset = 0;
sayzyas 0:f8373bf202a6 364 }
sayzyas 0:f8373bf202a6 365 // New command 2016.11.17 for customer request
sayzyas 0:f8373bf202a6 366 else if( buf[I2C_CP_COMMAND] == 'S'){ // Preset winch pisition <--- B2Demo isn't use this.
sayzyas 0:f8373bf202a6 367 DEBUG_PRINT_L1("Bd4> i2c Winch Position preset\r\n");
sayzyas 0:f8373bf202a6 368 wheel.reset();
sayzyas 0:f8373bf202a6 369 position_offset = 0;
sayzyas 0:f8373bf202a6 370 for( int j = 0; j < NumberOfI2CCommand; j++)
sayzyas 0:f8373bf202a6 371 pc.printf("%02x ", buf[j]);
sayzyas 0:f8373bf202a6 372 pc.printf( "\r\n" );
sayzyas 0:f8373bf202a6 373 position_offset = ( buf[ I2C_CP_PRESET_CPOS_UPPER] << 8 );
sayzyas 0:f8373bf202a6 374 position_offset |= buf[I2C_CP_PRESET_CPOS_LOWER];
sayzyas 0:f8373bf202a6 375 pc.printf("POSITION OFFSET = %d\r\n", position_offset);
sayzyas 0:f8373bf202a6 376 }
sayzyas 0:f8373bf202a6 377 else if( buf[I2C_CP_COMMAND] == 'R'){ // Set resolver calculation base parameter.
sayzyas 0:f8373bf202a6 378 DEBUG_PRINT_L1("Bd4> i2c Set resolver calc base data\r\n");
sayzyas 0:f8373bf202a6 379 for( int j = 0; j < NumberOfI2CCommand; j++)
sayzyas 0:f8373bf202a6 380 {
sayzyas 0:f8373bf202a6 381 pc.printf("%02x ", buf[j]);
sayzyas 0:f8373bf202a6 382 }
sayzyas 0:f8373bf202a6 383 pc.printf( "\r\n" );
sayzyas 0:f8373bf202a6 384
sayzyas 0:f8373bf202a6 385 dram_diameter = ( buf[I2C_CP_WDRAM_DIA_UPPER] << 8 );
sayzyas 0:f8373bf202a6 386 dram_diameter |= buf[I2C_CP_WDRAM_DIA_LOWER];
sayzyas 0:f8373bf202a6 387 dis_correct_value = ( buf[I2C_CP_CCABLE_DIA_UPPER] << 8 );
sayzyas 0:f8373bf202a6 388 dis_correct_value |= buf[I2C_CP_CCABLE_DIA_LOWER];
sayzyas 0:f8373bf202a6 389 rresolution = buf[I2C_CP_RESOLVER_RESO];
sayzyas 0:f8373bf202a6 390
sayzyas 0:f8373bf202a6 391 DEBUG_PRINT_L1("Bd4> ===========================================\r\n");
sayzyas 0:f8373bf202a6 392 DEBUG_PRINT_L1("Bd4> Dram Diameter : %d\t(mm)\r\n", dram_diameter);
sayzyas 0:f8373bf202a6 393 DEBUG_PRINT_L1("Bd4> CCable Diameter : %d\t(mm)\r\n", dis_correct_value);
sayzyas 0:f8373bf202a6 394 DEBUG_PRINT_L1("Bd4> Resolver Resolution : %d\t(bit)\r\n", rresolution);
sayzyas 0:f8373bf202a6 395 DEBUG_PRINT_L1("Bd4> -------------------------------------------\r\n", rresolution);
sayzyas 0:f8373bf202a6 396 DEBUG_PRINT_L1("Bd4> Real Diameter : %d\t(mm)\r\n", (dram_diameter+dis_correct_value));
sayzyas 0:f8373bf202a6 397 DEBUG_PRINT_L1("Bd4> Rotation Pulse / 1round : %d\t(pulse)\r\n", (int)(pow(2.0, (double)rresolution)*4));
sayzyas 0:f8373bf202a6 398 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 ) ) );
sayzyas 0:f8373bf202a6 399 DEBUG_PRINT_L1("Bd4> ===========================================\r\n");
sayzyas 0:f8373bf202a6 400
sayzyas 0:f8373bf202a6 401 pulse = wheel.getPulses();
sayzyas 0:f8373bf202a6 402 //pulse += 1;
sayzyas 0:f8373bf202a6 403
sayzyas 0:f8373bf202a6 404 DEBUG_PRINT_L1("Bd4> PULSE: %07d, ", pulse);
sayzyas 0:f8373bf202a6 405 DEBUG_PRINT_L1("DISTANCE: %04d (mm)\r\n", (int)( pulse * ( (double)( ((dram_diameter+dis_correct_value)/100) * PAI / (pow(2.0, (double)rresolution)*4 ) ) ) ) );
sayzyas 0:f8373bf202a6 406 DEBUG_PRINT_L1("Bd4> ---------------------------------------\r\n");
sayzyas 0:f8373bf202a6 407
sayzyas 0:f8373bf202a6 408 led2 = LED_ON;
sayzyas 0:f8373bf202a6 409 }
sayzyas 0:f8373bf202a6 410 break;
sayzyas 0:f8373bf202a6 411 }
sayzyas 0:f8373bf202a6 412
sayzyas 0:f8373bf202a6 413 // Following instruction should do on thread, because this time of operation is lottle longer. !!!
sayzyas 0:f8373bf202a6 414
sayzyas 0:f8373bf202a6 415 /*
sayzyas 0:f8373bf202a6 416 DEBUG_PRINT_L0("Bd4> %05f", mcc.rd_motorCurrent(11)); DEBUG_PRINT_L0("\t");
sayzyas 0:f8373bf202a6 417 DEBUG_PRINT_L0("%05f", mcc.rd_motorCurrent(12)); DEBUG_PRINT_L0("\t");
sayzyas 0:f8373bf202a6 418 DEBUG_PRINT_L0("%05f", mcc.rd_motorCurrent(21)); DEBUG_PRINT_L0("\t");
sayzyas 0:f8373bf202a6 419 DEBUG_PRINT_L0("%05f", mcc.rd_motorCurrent(22)); DEBUG_PRINT_L0("\t");
sayzyas 0:f8373bf202a6 420 DEBUG_PRINT_L0("%05f", mcc.rd_motorCurrent(31)); DEBUG_PRINT_L0("\t");
sayzyas 0:f8373bf202a6 421 DEBUG_PRINT_L0("%05f", mcc.rd_motorCurrent(32)); DEBUG_PRINT_L0("\r\n");
sayzyas 0:f8373bf202a6 422 */
sayzyas 0:f8373bf202a6 423
sayzyas 0:f8373bf202a6 424
sayzyas 0:f8373bf202a6 425
sayzyas 0:f8373bf202a6 426
sayzyas 0:f8373bf202a6 427
sayzyas 0:f8373bf202a6 428 //Thread::wait(1);
sayzyas 0:f8373bf202a6 429 wait_ms(1);
sayzyas 0:f8373bf202a6 430
sayzyas 0:f8373bf202a6 431 }
sayzyas 0:f8373bf202a6 432 }