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@0:f8373bf202a6, 2018-07-26 (annotated)

Committer:
sayzyas
Date:
Thu Jul 26 00:19:47 2018 +0000
Revision:
0:f8373bf202a6
Child:
1:85e8e2c8f283
2018.07.26

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
sayzyas 0:f8373bf202a6 223
sayzyas 0:f8373bf202a6 224 }
sayzyas 0:f8373bf202a6 225 else
sayzyas 0:f8373bf202a6 226 {
sayzyas 0:f8373bf202a6 227 msg[3] = 0;
sayzyas 0:f8373bf202a6 228 // 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 229
sayzyas 0:f8373bf202a6 230 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 231 mcc.cnt_mclock_LBTFM_f,
sayzyas 0:f8373bf202a6 232 mcc.cnt_mclock_LBTFM_r,
sayzyas 0:f8373bf202a6 233 mcc.cnt_mclock_RFTFM_f,
sayzyas 0:f8373bf202a6 234 mcc.cnt_mclock_RFTFM_r
sayzyas 0:f8373bf202a6 235
sayzyas 0:f8373bf202a6 236 );
sayzyas 0:f8373bf202a6 237
sayzyas 0:f8373bf202a6 238 }
sayzyas 0:f8373bf202a6 239 }
sayzyas 0:f8373bf202a6 240
sayzyas 0:f8373bf202a6 241 else if( buf[I2C_CP_COMMAND] == 'T' ){ // Set motor threshold
sayzyas 0:f8373bf202a6 242 DEBUG_PRINT_L1("Bd4> i2c Set motor threshold\r\n");
sayzyas 0:f8373bf202a6 243 // LB CRW
sayzyas 0:f8373bf202a6 244 if( buf[I2C_CP_MOTORNO] == MOTOR_LBCRW )
sayzyas 0:f8373bf202a6 245 {
sayzyas 0:f8373bf202a6 246 if( buf[I2C_CP_M_DIR] == MOTOR_DIR_FWD )
sayzyas 0:f8373bf202a6 247 {
sayzyas 0:f8373bf202a6 248 mcc.mc_th_LBCRW_f = ( buf[I2C_CP_M_CNTTH_U] ) << 8 | buf[I2C_CP_M_CNTTH_L];
sayzyas 0:f8373bf202a6 249 DEBUG_PRINT_L1("Bd4> motor th (LBCRW F) : %d\r\n", mcc.mc_th_LBCRW_f);
sayzyas 0:f8373bf202a6 250 }
sayzyas 0:f8373bf202a6 251 else
sayzyas 0:f8373bf202a6 252 {
sayzyas 0:f8373bf202a6 253 mcc.mc_th_LBCRW_r = ( buf[I2C_CP_M_CNTTH_U] ) << 8 | buf[I2C_CP_M_CNTTH_L];
sayzyas 0:f8373bf202a6 254 DEBUG_PRINT_L1("Bd4> motor th (LBCRW R) : %d\r\n", mcc.mc_th_LBCRW_r);
sayzyas 0:f8373bf202a6 255 }
sayzyas 0:f8373bf202a6 256 }
sayzyas 0:f8373bf202a6 257 // RF CRW
sayzyas 0:f8373bf202a6 258 else if( buf[I2C_CP_MOTORNO] == MOTOR_RFCRW )
sayzyas 0:f8373bf202a6 259 {
sayzyas 0:f8373bf202a6 260 if( buf[I2C_CP_M_DIR] == MOTOR_DIR_FWD )
sayzyas 0:f8373bf202a6 261 {
sayzyas 0:f8373bf202a6 262 mcc.mc_th_RFCRW_f = ( buf[I2C_CP_M_CNTTH_U] ) << 8 | buf[I2C_CP_M_CNTTH_L];
sayzyas 0:f8373bf202a6 263 DEBUG_PRINT_L1("Bd4> motor th (RFCRW F) : %d\r\n", mcc.mc_th_RFCRW_f);
sayzyas 0:f8373bf202a6 264 }
sayzyas 0:f8373bf202a6 265 else
sayzyas 0:f8373bf202a6 266 {
sayzyas 0:f8373bf202a6 267 mcc.mc_th_RFCRW_r = ( buf[I2C_CP_M_CNTTH_U] ) << 8 | buf[I2C_CP_M_CNTTH_L];
sayzyas 0:f8373bf202a6 268 DEBUG_PRINT_L1("Bd4> motor th (RFCRW R) : %d\r\n", mcc.mc_th_RFCRW_r);
sayzyas 0:f8373bf202a6 269 }
sayzyas 0:f8373bf202a6 270 }
sayzyas 0:f8373bf202a6 271 // LB TFM
sayzyas 0:f8373bf202a6 272 else if( buf[I2C_CP_MOTORNO] == MOTOR_LBTFM )
sayzyas 0:f8373bf202a6 273 {
sayzyas 0:f8373bf202a6 274 if( buf[I2C_CP_M_DIR] == MOTOR_DIR_FWD )
sayzyas 0:f8373bf202a6 275 {
sayzyas 0:f8373bf202a6 276 mcc.mc_th_LBTFM_f = ( buf[I2C_CP_M_CNTTH_U] ) << 8 | buf[I2C_CP_M_CNTTH_L];
sayzyas 0:f8373bf202a6 277 DEBUG_PRINT_L1("Bd4> motor th (LBTFM F) : %d\r\n", mcc.mc_th_LBTFM_f);
sayzyas 0:f8373bf202a6 278 }
sayzyas 0:f8373bf202a6 279 else
sayzyas 0:f8373bf202a6 280 {
sayzyas 0:f8373bf202a6 281 mcc.mc_th_LBTFM_r = ( buf[I2C_CP_M_CNTTH_U] ) << 8 | buf[I2C_CP_M_CNTTH_L];
sayzyas 0:f8373bf202a6 282 DEBUG_PRINT_L1("Bd4> motor th (LBTFM R) : %d\r\n", mcc.mc_th_LBTFM_r);
sayzyas 0:f8373bf202a6 283 }
sayzyas 0:f8373bf202a6 284 }
sayzyas 0:f8373bf202a6 285 // RF TFM
sayzyas 0:f8373bf202a6 286 else if( buf[I2C_CP_MOTORNO] == MOTOR_RFTFM )
sayzyas 0:f8373bf202a6 287 {
sayzyas 0:f8373bf202a6 288 if( buf[I2C_CP_M_DIR] == MOTOR_DIR_FWD )
sayzyas 0:f8373bf202a6 289 {
sayzyas 0:f8373bf202a6 290 mcc.mc_th_RFTFM_f = ( buf[I2C_CP_M_CNTTH_U] ) << 8 | buf[I2C_CP_M_CNTTH_L];
sayzyas 0:f8373bf202a6 291 DEBUG_PRINT_L1("Bd4> motor th (RFTFM F) : %d\r\n", mcc.mc_th_RFTFM_f);
sayzyas 0:f8373bf202a6 292 }
sayzyas 0:f8373bf202a6 293 else
sayzyas 0:f8373bf202a6 294 {
sayzyas 0:f8373bf202a6 295 mcc.mc_th_RFTFM_r = ( buf[I2C_CP_M_CNTTH_U] ) << 8 | buf[I2C_CP_M_CNTTH_L];
sayzyas 0:f8373bf202a6 296 DEBUG_PRINT_L1("Bd4> motor th (RFTFM R) : %d\r\n", mcc.mc_th_RFTFM_r);
sayzyas 0:f8373bf202a6 297 }
sayzyas 0:f8373bf202a6 298 }
sayzyas 0:f8373bf202a6 299 // WINCH
sayzyas 0:f8373bf202a6 300 else if( buf[I2C_CP_MOTORNO] == MOTOR_WINCH )
sayzyas 0:f8373bf202a6 301 {
sayzyas 0:f8373bf202a6 302 if( buf[I2C_CP_M_DIR] == MOTOR_DIR_FWD )
sayzyas 0:f8373bf202a6 303 {
sayzyas 0:f8373bf202a6 304 mcc.mc_th_WINCH_f = ( buf[I2C_CP_M_CNTTH_U] ) << 8 | buf[I2C_CP_M_CNTTH_L];
sayzyas 0:f8373bf202a6 305 DEBUG_PRINT_L1("Bd4> motor th (WINCH F) : %d\r\n", mcc.mc_th_WINCH_f);
sayzyas 0:f8373bf202a6 306 }
sayzyas 0:f8373bf202a6 307 else
sayzyas 0:f8373bf202a6 308 {
sayzyas 0:f8373bf202a6 309 mcc.mc_th_WINCH_r = ( buf[I2C_CP_M_CNTTH_U] ) << 8 | buf[I2C_CP_M_CNTTH_L];
sayzyas 0:f8373bf202a6 310 DEBUG_PRINT_L1("Bd4> motor th (WINCH R) : %d\r\n", mcc.mc_th_WINCH_r);
sayzyas 0:f8373bf202a6 311 }
sayzyas 0:f8373bf202a6 312 }
sayzyas 0:f8373bf202a6 313 // PAN
sayzyas 0:f8373bf202a6 314 else if( buf[I2C_CP_MOTORNO] == MOTOR_CMPAN )
sayzyas 0:f8373bf202a6 315 {
sayzyas 0:f8373bf202a6 316 if( buf[I2C_CP_M_DIR] == MOTOR_DIR_FWD )
sayzyas 0:f8373bf202a6 317 {
sayzyas 0:f8373bf202a6 318 mcc.mc_th_CMPAN_f = ( buf[I2C_CP_M_CNTTH_U] ) << 8 | buf[I2C_CP_M_CNTTH_L];
sayzyas 0:f8373bf202a6 319 DEBUG_PRINT_L1("Bd4> motor th (CPAN F) : %d\r\n", mcc.mc_th_CMPAN_f);
sayzyas 0:f8373bf202a6 320 }
sayzyas 0:f8373bf202a6 321 else
sayzyas 0:f8373bf202a6 322 {
sayzyas 0:f8373bf202a6 323 mcc.mc_th_CMPAN_r = ( buf[I2C_CP_M_CNTTH_U] ) << 8 | buf[I2C_CP_M_CNTTH_L];
sayzyas 0:f8373bf202a6 324 DEBUG_PRINT_L1("Bd4> motor th (CPAN R) : %d\r\n", mcc.mc_th_CMPAN_r);
sayzyas 0:f8373bf202a6 325 }
sayzyas 0:f8373bf202a6 326 }
sayzyas 0:f8373bf202a6 327 // TILT
sayzyas 0:f8373bf202a6 328 else if( buf[I2C_CP_MOTORNO] == MOTOR_CTILT )
sayzyas 0:f8373bf202a6 329 {
sayzyas 0:f8373bf202a6 330 if( buf[I2C_CP_M_DIR] == MOTOR_DIR_FWD )
sayzyas 0:f8373bf202a6 331 {
sayzyas 0:f8373bf202a6 332 mcc.mc_th_CTILT_f = ( buf[I2C_CP_M_CNTTH_U] ) << 8 | buf[I2C_CP_M_CNTTH_L];
sayzyas 0:f8373bf202a6 333 DEBUG_PRINT_L1("Bd4> motor th (CTILT F) : %d\r\n", mcc.mc_th_CTILT_f);
sayzyas 0:f8373bf202a6 334 }
sayzyas 0:f8373bf202a6 335 else
sayzyas 0:f8373bf202a6 336 {
sayzyas 0:f8373bf202a6 337 mcc.mc_th_CTILT_r = ( buf[I2C_CP_M_CNTTH_U] ) << 8 | buf[I2C_CP_M_CNTTH_L];
sayzyas 0:f8373bf202a6 338 DEBUG_PRINT_L1("Bd4> motor th (CTILT R) : %d\r\n", mcc.mc_th_CTILT_r);
sayzyas 0:f8373bf202a6 339 }
sayzyas 0:f8373bf202a6 340 }
sayzyas 0:f8373bf202a6 341 }
sayzyas 0:f8373bf202a6 342 else if( buf[I2C_CP_COMMAND] == 0x4f ){ // Clear motor current over counters
sayzyas 0:f8373bf202a6 343 DEBUG_PRINT_L1("Bd4> i2c Clear motor current over counters\r\n");
sayzyas 0:f8373bf202a6 344 mcc.cnt_mclock_LBCRW_f = 0; // motor lock counter
sayzyas 0:f8373bf202a6 345 mcc.cnt_mclock_LBCRW_r = 0; // motor lock counter
sayzyas 0:f8373bf202a6 346 mcc.cnt_mclock_RFCRW_f = 0; // motor lock counter
sayzyas 0:f8373bf202a6 347 mcc.cnt_mclock_RFCRW_r = 0; // motor lock counter
sayzyas 0:f8373bf202a6 348 mcc.cnt_mclock_LBTFM_f = 0; // motor lock counter
sayzyas 0:f8373bf202a6 349 mcc.cnt_mclock_LBTFM_r = 0; // motor lock counter
sayzyas 0:f8373bf202a6 350 mcc.cnt_mclock_RFTFM_f = 0; // motor lock counter
sayzyas 0:f8373bf202a6 351 mcc.cnt_mclock_RFTFM_r = 0; // motor lock counter
sayzyas 0:f8373bf202a6 352 mcc.cnt_mclock_CMPAN_f = 0; // motor lock counter
sayzyas 0:f8373bf202a6 353 mcc.cnt_mclock_CMPAN_r = 0; // motor lock counter
sayzyas 0:f8373bf202a6 354 mcc.cnt_mclock_CTILT_f = 0; // motor lock counter
sayzyas 0:f8373bf202a6 355 mcc.cnt_mclock_CTILT_r = 0; // motor lock counter
sayzyas 0:f8373bf202a6 356 mcc.cnt_mclock_WINCH_f = 0; // motor lock counter
sayzyas 0:f8373bf202a6 357 mcc.cnt_mclock_WINCH_r = 0; // motor lock counter
sayzyas 0:f8373bf202a6 358 }
sayzyas 0:f8373bf202a6 359 else if( buf[I2C_CP_COMMAND] == 0x43 ){ // 'C': Set motor current base
sayzyas 0:f8373bf202a6 360 DEBUG_PRINT_L1("Bd4> i2c Set motor current base value\r\n");
sayzyas 0:f8373bf202a6 361 mcc.set_init_mc( 300 );
sayzyas 0:f8373bf202a6 362 }
sayzyas 0:f8373bf202a6 363 else if( buf[I2C_CP_COMMAND] == 'Z'){ // Position reset
sayzyas 0:f8373bf202a6 364 DEBUG_PRINT_L1("Bd4> i2c Winch Position reset\r\n");
sayzyas 0:f8373bf202a6 365 wheel.reset();
sayzyas 0:f8373bf202a6 366 position_offset = 0;
sayzyas 0:f8373bf202a6 367 }
sayzyas 0:f8373bf202a6 368 // New command 2016.11.17 for customer request
sayzyas 0:f8373bf202a6 369 else if( buf[I2C_CP_COMMAND] == 'S'){ // Preset winch pisition <--- B2Demo isn't use this.
sayzyas 0:f8373bf202a6 370 DEBUG_PRINT_L1("Bd4> i2c Winch Position preset\r\n");
sayzyas 0:f8373bf202a6 371 wheel.reset();
sayzyas 0:f8373bf202a6 372 position_offset = 0;
sayzyas 0:f8373bf202a6 373 for( int j = 0; j < NumberOfI2CCommand; j++)
sayzyas 0:f8373bf202a6 374 pc.printf("%02x ", buf[j]);
sayzyas 0:f8373bf202a6 375 pc.printf( "\r\n" );
sayzyas 0:f8373bf202a6 376 position_offset = ( buf[ I2C_CP_PRESET_CPOS_UPPER] << 8 );
sayzyas 0:f8373bf202a6 377 position_offset |= buf[I2C_CP_PRESET_CPOS_LOWER];
sayzyas 0:f8373bf202a6 378 pc.printf("POSITION OFFSET = %d\r\n", position_offset);
sayzyas 0:f8373bf202a6 379 }
sayzyas 0:f8373bf202a6 380 else if( buf[I2C_CP_COMMAND] == 'R'){ // Set resolver calculation base parameter.
sayzyas 0:f8373bf202a6 381 DEBUG_PRINT_L1("Bd4> i2c Set resolver calc base data\r\n");
sayzyas 0:f8373bf202a6 382 for( int j = 0; j < NumberOfI2CCommand; j++)
sayzyas 0:f8373bf202a6 383 {
sayzyas 0:f8373bf202a6 384 pc.printf("%02x ", buf[j]);
sayzyas 0:f8373bf202a6 385 }
sayzyas 0:f8373bf202a6 386 pc.printf( "\r\n" );
sayzyas 0:f8373bf202a6 387
sayzyas 0:f8373bf202a6 388 dram_diameter = ( buf[I2C_CP_WDRAM_DIA_UPPER] << 8 );
sayzyas 0:f8373bf202a6 389 dram_diameter |= buf[I2C_CP_WDRAM_DIA_LOWER];
sayzyas 0:f8373bf202a6 390 dis_correct_value = ( buf[I2C_CP_CCABLE_DIA_UPPER] << 8 );
sayzyas 0:f8373bf202a6 391 dis_correct_value |= buf[I2C_CP_CCABLE_DIA_LOWER];
sayzyas 0:f8373bf202a6 392 rresolution = buf[I2C_CP_RESOLVER_RESO];
sayzyas 0:f8373bf202a6 393
sayzyas 0:f8373bf202a6 394 DEBUG_PRINT_L1("Bd4> ===========================================\r\n");
sayzyas 0:f8373bf202a6 395 DEBUG_PRINT_L1("Bd4> Dram Diameter : %d\t(mm)\r\n", dram_diameter);
sayzyas 0:f8373bf202a6 396 DEBUG_PRINT_L1("Bd4> CCable Diameter : %d\t(mm)\r\n", dis_correct_value);
sayzyas 0:f8373bf202a6 397 DEBUG_PRINT_L1("Bd4> Resolver Resolution : %d\t(bit)\r\n", rresolution);
sayzyas 0:f8373bf202a6 398 DEBUG_PRINT_L1("Bd4> -------------------------------------------\r\n", rresolution);
sayzyas 0:f8373bf202a6 399 DEBUG_PRINT_L1("Bd4> Real Diameter : %d\t(mm)\r\n", (dram_diameter+dis_correct_value));
sayzyas 0:f8373bf202a6 400 DEBUG_PRINT_L1("Bd4> Rotation Pulse / 1round : %d\t(pulse)\r\n", (int)(pow(2.0, (double)rresolution)*4));
sayzyas 0:f8373bf202a6 401 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 402 DEBUG_PRINT_L1("Bd4> ===========================================\r\n");
sayzyas 0:f8373bf202a6 403
sayzyas 0:f8373bf202a6 404 pulse = wheel.getPulses();
sayzyas 0:f8373bf202a6 405 //pulse += 1;
sayzyas 0:f8373bf202a6 406
sayzyas 0:f8373bf202a6 407 DEBUG_PRINT_L1("Bd4> PULSE: %07d, ", pulse);
sayzyas 0:f8373bf202a6 408 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 409 DEBUG_PRINT_L1("Bd4> ---------------------------------------\r\n");
sayzyas 0:f8373bf202a6 410
sayzyas 0:f8373bf202a6 411 led2 = LED_ON;
sayzyas 0:f8373bf202a6 412 }
sayzyas 0:f8373bf202a6 413 break;
sayzyas 0:f8373bf202a6 414 }
sayzyas 0:f8373bf202a6 415
sayzyas 0:f8373bf202a6 416 // Following instruction should do on thread, because this time of operation is lottle longer. !!!
sayzyas 0:f8373bf202a6 417
sayzyas 0:f8373bf202a6 418 /*
sayzyas 0:f8373bf202a6 419 DEBUG_PRINT_L0("Bd4> %05f", mcc.rd_motorCurrent(11)); DEBUG_PRINT_L0("\t");
sayzyas 0:f8373bf202a6 420 DEBUG_PRINT_L0("%05f", mcc.rd_motorCurrent(12)); DEBUG_PRINT_L0("\t");
sayzyas 0:f8373bf202a6 421 DEBUG_PRINT_L0("%05f", mcc.rd_motorCurrent(21)); DEBUG_PRINT_L0("\t");
sayzyas 0:f8373bf202a6 422 DEBUG_PRINT_L0("%05f", mcc.rd_motorCurrent(22)); DEBUG_PRINT_L0("\t");
sayzyas 0:f8373bf202a6 423 DEBUG_PRINT_L0("%05f", mcc.rd_motorCurrent(31)); DEBUG_PRINT_L0("\t");
sayzyas 0:f8373bf202a6 424 DEBUG_PRINT_L0("%05f", mcc.rd_motorCurrent(32)); DEBUG_PRINT_L0("\r\n");
sayzyas 0:f8373bf202a6 425 */
sayzyas 0:f8373bf202a6 426
sayzyas 0:f8373bf202a6 427
sayzyas 0:f8373bf202a6 428
sayzyas 0:f8373bf202a6 429
sayzyas 0:f8373bf202a6 430
sayzyas 0:f8373bf202a6 431 //Thread::wait(1);
sayzyas 0:f8373bf202a6 432 wait_ms(1);
sayzyas 0:f8373bf202a6 433
sayzyas 0:f8373bf202a6 434 }
sayzyas 0:f8373bf202a6 435 }