z ysaito / Mbed 2 deprecated APro_B2DTst_1_ResAnaCtrl_LPC824

https://os.mbed.com/users/sayzyas

Dependencies:   QEI TextLCD mbed

P1_1Y_main.cpp@0:73dd48be5ca6, 2018-07-26 (annotated)

Committer:
sayzyas
Date:
Thu Jul 26 00:20:15 2018 +0000
Revision:
0:73dd48be5ca6
2018.07.26

Who changed what in which revision?

UserRevisionLine numberNew contents of line
sayzyas 0:73dd48be5ca6 1 /**********************************************************
sayzyas 0:73dd48be5ca6 2 * Project: B2Dash (1F-1)
sayzyas 0:73dd48be5ca6 3 * Title: CrExp B2D Resolver Ctrl Main
sayzyas 0:73dd48be5ca6 4 * Target: LPCXpresso824-Max
sayzyas 0:73dd48be5ca6 5 * Author: zStranger
sayzyas 0:73dd48be5ca6 6 * Date(Latest update) 2017.8.23
sayzyas 0:73dd48be5ca6 7 * --------------------------------------------------------
sayzyas 0:73dd48be5ca6 8 * Article
sayzyas 0:73dd48be5ca6 9 *
sayzyas 0:73dd48be5ca6 10 *
sayzyas 0:73dd48be5ca6 11 * --------------------------------------------------------
sayzyas 0:73dd48be5ca6 12 *
sayzyas 0:73dd48be5ca6 13 * LPCXpresso 824-MAX
sayzyas 0:73dd48be5ca6 14 * +---------USB---------+
sayzyas 0:73dd48be5ca6 15 * | |
sayzyas 0:73dd48be5ca6 16 * | |
sayzyas 0:73dd48be5ca6 17 * | |
sayzyas 0:73dd48be5ca6 18 * | |
sayzyas 0:73dd48be5ca6 19 * | | SCL P0_10 D15 -> IIC SCL
sayzyas 0:73dd48be5ca6 20 * | ## ### ## | SDA P0_11 D14 -> IIC SDA
sayzyas 0:73dd48be5ca6 21 * | # # # # # # | AVDD
sayzyas 0:73dd48be5ca6 22 * N/A | # # # # # | GND
sayzyas 0:73dd48be5ca6 23 * +3V3 | ## # # # | SCK P0_24 D13
sayzyas 0:73dd48be5ca6 24 * NRST | # # # # # | MISO P0_25 D12
sayzyas 0:73dd48be5ca6 25 * +3V3 | # # # # ###### | MOSI P0_26 D11 -> Res B
sayzyas 0:73dd48be5ca6 26 * +5V | ## ##### # | SSEL P0_15 D10 -> Res A
sayzyas 0:73dd48be5ca6 27 * GND | | P0_27 D9 --> SW ZERO
sayzyas 0:73dd48be5ca6 28 * GND | | P0_13 D8 --> Int2Main
sayzyas 0:73dd48be5ca6 29 * N/A | | P0_17 D7
sayzyas 0:73dd48be5ca6 30 * | | P0_16 D6
sayzyas 0:73dd48be5ca6 31 * mtrcnt (Dram) --> A0 P0_6 | | PWM P0_28 D5 --> LCD DB7
sayzyas 0:73dd48be5ca6 32 * mtrcnt (Cable) --> A1 P0_14 | | PWM P0_18 D4 --> LCD DB6
sayzyas 0:73dd48be5ca6 33 * mtrspd (Dram CW) -> A2 P0_23 | | PWM P0_12 D3 --> LCD DB5
sayzyas 0:73dd48be5ca6 34 * mtrspd (Dram CCW) -> A3 P0_22 | | PWM P0_19 D2 --> LCD DB4
sayzyas 0:73dd48be5ca6 35 * mtrspd (Cable CW) -> A4 P0_21 | | TX P0_4 D1 --> LCD E
sayzyas 0:73dd48be5ca6 36 * mtrspd (Cable CCW)-> A5 P0_20 | | RX P0_0 D0 --> LCD RS
sayzyas 0:73dd48be5ca6 37 * +---------------------+
sayzyas 0:73dd48be5ca6 38 *
sayzyas 0:73dd48be5ca6 39 ***************************************/
sayzyas 0:73dd48be5ca6 40
sayzyas 0:73dd48be5ca6 41 #include "mbed.h"
sayzyas 0:73dd48be5ca6 42 //#include "rtos.h"
sayzyas 0:73dd48be5ca6 43 #include "QEI.h"
sayzyas 0:73dd48be5ca6 44 #include "common.h"
sayzyas 0:73dd48be5ca6 45 #include "debugprint.h"
sayzyas 0:73dd48be5ca6 46 #include <math.h>
sayzyas 0:73dd48be5ca6 47 #include "mcchk.h"
sayzyas 0:73dd48be5ca6 48 #include "TextLCD.h"
sayzyas 0:73dd48be5ca6 49
sayzyas 0:73dd48be5ca6 50 // Hardware setting
sayzyas 0:73dd48be5ca6 51
sayzyas 0:73dd48be5ca6 52 Serial pc(USBTX, USBRX); // tx, rx
sayzyas 0:73dd48be5ca6 53
sayzyas 0:73dd48be5ca6 54 /*
sayzyas 0:73dd48be5ca6 55 QEI wheel
sayzyas 0:73dd48be5ca6 56 PinName channelA,
sayzyas 0:73dd48be5ca6 57 PinName channelB,
sayzyas 0:73dd48be5ca6 58 PinName index,
sayzyas 0:73dd48be5ca6 59 int pulsesPerRev,
sayzyas 0:73dd48be5ca6 60 Encoding encoding) : channelA_(channelA), channelB_(channelB),
sayzyas 0:73dd48be5ca6 61 index_(index)
sayzyas 0:73dd48be5ca6 62 */
sayzyas 0:73dd48be5ca6 63 #ifdef ResolverDirection_f // For Fix Fallong Winch
sayzyas 0:73dd48be5ca6 64 //QEI wheel( P0_26, P0_15, NC, ROTATE_PER_RESOLUTION, QEI::X2_ENCODING );
sayzyas 0:73dd48be5ca6 65 QEI wheel( D11, D10, NC, ROTATE_PER_RESOLUTION, QEI::X2_ENCODING );
sayzyas 0:73dd48be5ca6 66 #else //For Falling Position Moving Winch
sayzyas 0:73dd48be5ca6 67 //QEI wheel( P0_15, P0_26, NC, ROTATE_PER_RESOLUTION, QEI::X2_ENCODING );
sayzyas 0:73dd48be5ca6 68 QEI wheel( D10, D11, NC, ROTATE_PER_RESOLUTION, QEI::X2_ENCODING );
sayzyas 0:73dd48be5ca6 69
sayzyas 0:73dd48be5ca6 70 #endif // FFWinchPhaseSetting
sayzyas 0:73dd48be5ca6 71
sayzyas 0:73dd48be5ca6 72 //TextLCD lcd(P0_28, P0_18, P0_12, P0_19, P0_4, P0_0); // rs, e, d4-d7
sayzyas 0:73dd48be5ca6 73 TextLCD lcd(D0,D1,D2,D5,D12,D13); // rs, e, d4-d7
sayzyas 0:73dd48be5ca6 74
sayzyas 0:73dd48be5ca6 75 /*
sayzyas 0:73dd48be5ca6 76 Dram r = 60mm
sayzyas 0:73dd48be5ca6 77 Cable fai = 3
sayzyas 0:73dd48be5ca6 78
sayzyas 0:73dd48be5ca6 79 (60+3)*3.14 = 197.82mm ==> 2^12*4 = 4096 * 4 = 16384 pulse
sayzyas 0:73dd48be5ca6 80 1(mm) = 82.82(pulse)
sayzyas 0:73dd48be5ca6 81 0.01208(mm) = 1(pulse)
sayzyas 0:73dd48be5ca6 82
sayzyas 0:73dd48be5ca6 83 */
sayzyas 0:73dd48be5ca6 84 DigitalOut led1(LED1); // Red
sayzyas 0:73dd48be5ca6 85 DigitalOut led2(LED2); // Green
sayzyas 0:73dd48be5ca6 86 DigitalOut led3(LED3); // Blue
sayzyas 0:73dd48be5ca6 87
sayzyas 0:73dd48be5ca6 88 DigitalOut int2hst(D8);
sayzyas 0:73dd48be5ca6 89 DigitalIn sw_zero(D7);
sayzyas 0:73dd48be5ca6 90
sayzyas 0:73dd48be5ca6 91 AnalogIn mtcnt_wdram(A0);
sayzyas 0:73dd48be5ca6 92 AnalogIn mtcnt_wcabl(A1);
sayzyas 0:73dd48be5ca6 93 AnalogIn mtspd_wdram_cw(A2);
sayzyas 0:73dd48be5ca6 94 AnalogIn mtspd_wdram_ccw(A3);
sayzyas 0:73dd48be5ca6 95 AnalogIn mtspd_wcabl_cw(A4);
sayzyas 0:73dd48be5ca6 96 AnalogIn mtspd_wcabl_ccw(A5);
sayzyas 0:73dd48be5ca6 97
sayzyas 0:73dd48be5ca6 98 I2CSlave slave(D14, D15); //I2C SDA SCL
sayzyas 0:73dd48be5ca6 99
sayzyas 0:73dd48be5ca6 100 //Mutex lcdMutex;
sayzyas 0:73dd48be5ca6 101
sayzyas 0:73dd48be5ca6 102 void led_demo(){
sayzyas 0:73dd48be5ca6 103 int i;
sayzyas 0:73dd48be5ca6 104 for( i = 0; i < 20; i++ ) {
sayzyas 0:73dd48be5ca6 105 led1 = LED_ON;
sayzyas 0:73dd48be5ca6 106 led2 = LED_OFF;
sayzyas 0:73dd48be5ca6 107 led3 = LED_OFF;
sayzyas 0:73dd48be5ca6 108 wait_ms(20);
sayzyas 0:73dd48be5ca6 109 led1 = LED_OFF;
sayzyas 0:73dd48be5ca6 110 led2 = LED_OFF;
sayzyas 0:73dd48be5ca6 111 led3 = LED_OFF;
sayzyas 0:73dd48be5ca6 112 wait_ms(20);
sayzyas 0:73dd48be5ca6 113 led1 = LED_OFF;
sayzyas 0:73dd48be5ca6 114 led2 = LED_ON;
sayzyas 0:73dd48be5ca6 115 led3 = LED_OFF;
sayzyas 0:73dd48be5ca6 116 wait_ms(20);
sayzyas 0:73dd48be5ca6 117 led1 = LED_OFF;
sayzyas 0:73dd48be5ca6 118 led2 = LED_OFF;
sayzyas 0:73dd48be5ca6 119 led3 = LED_OFF;
sayzyas 0:73dd48be5ca6 120 wait_ms(20);
sayzyas 0:73dd48be5ca6 121 led1 = LED_OFF;
sayzyas 0:73dd48be5ca6 122 led2 = LED_OFF;
sayzyas 0:73dd48be5ca6 123 led3 = LED_ON;
sayzyas 0:73dd48be5ca6 124 wait_ms(20);
sayzyas 0:73dd48be5ca6 125 led1 = LED_OFF;
sayzyas 0:73dd48be5ca6 126 led2 = LED_OFF;
sayzyas 0:73dd48be5ca6 127 led3 = LED_OFF;
sayzyas 0:73dd48be5ca6 128 wait_ms(20);
sayzyas 0:73dd48be5ca6 129 }
sayzyas 0:73dd48be5ca6 130 // wait_ms(3000);
sayzyas 0:73dd48be5ca6 131 }
sayzyas 0:73dd48be5ca6 132
sayzyas 0:73dd48be5ca6 133 void lcd_dsp( int column, int row, char* msg, int cnt)
sayzyas 0:73dd48be5ca6 134 {
sayzyas 0:73dd48be5ca6 135 for( int i = 0; i < cnt; i++ )
sayzyas 0:73dd48be5ca6 136 {
sayzyas 0:73dd48be5ca6 137 lcd.locate(column+i,row);
sayzyas 0:73dd48be5ca6 138 lcd.putc(*msg++);
sayzyas 0:73dd48be5ca6 139 }
sayzyas 0:73dd48be5ca6 140 wait_ms(1);
sayzyas 0:73dd48be5ca6 141 // Thread::wait(10);
sayzyas 0:73dd48be5ca6 142 }
sayzyas 0:73dd48be5ca6 143
sayzyas 0:73dd48be5ca6 144 void lcd_out( char* line1, char* line2 ) {
sayzyas 0:73dd48be5ca6 145 lcd.cls();
sayzyas 0:73dd48be5ca6 146 lcd.locate(0, 0);
sayzyas 0:73dd48be5ca6 147 lcd.printf(line1);
sayzyas 0:73dd48be5ca6 148 lcd.locate(0, 1);
sayzyas 0:73dd48be5ca6 149 lcd.printf(line2);
sayzyas 0:73dd48be5ca6 150 }
sayzyas 0:73dd48be5ca6 151
sayzyas 0:73dd48be5ca6 152
sayzyas 0:73dd48be5ca6 153
sayzyas 0:73dd48be5ca6 154 // ========================================================================
sayzyas 0:73dd48be5ca6 155 // Main Function
sayzyas 0:73dd48be5ca6 156 // ========================================================================
sayzyas 0:73dd48be5ca6 157 int main() {
sayzyas 0:73dd48be5ca6 158 int i = 0;
sayzyas 0:73dd48be5ca6 159 int ii;
sayzyas 0:73dd48be5ca6 160 char msg[16] = "";
sayzyas 0:73dd48be5ca6 161 char buf[12]; // command buffer
sayzyas 0:73dd48be5ca6 162
sayzyas 0:73dd48be5ca6 163 int pulse;
sayzyas 0:73dd48be5ca6 164 double dropAmt_d = 0.0f;
sayzyas 0:73dd48be5ca6 165 int16_t dropAmt;
sayzyas 0:73dd48be5ca6 166 int16_t pos_offset = 0;;
sayzyas 0:73dd48be5ca6 167
sayzyas 0:73dd48be5ca6 168 uint16_t dram_diameter;
sayzyas 0:73dd48be5ca6 169 uint16_t dis_correct_value;
sayzyas 0:73dd48be5ca6 170 uint8_t rresolution;
sayzyas 0:73dd48be5ca6 171
sayzyas 0:73dd48be5ca6 172 bool flg_mtrEStop = false;
sayzyas 0:73dd48be5ca6 173
sayzyas 0:73dd48be5ca6 174 float mc_wdram; // motor current
sayzyas 0:73dd48be5ca6 175 float mc_wcabl; // motor current
sayzyas 0:73dd48be5ca6 176 int16_t sp_wdram_f; // motor speed
sayzyas 0:73dd48be5ca6 177 int16_t sp_wdram_r; // motor speed
sayzyas 0:73dd48be5ca6 178 int16_t sp_wcabl_f; // motor speed
sayzyas 0:73dd48be5ca6 179 int16_t sp_wcabl_r; // motor speed
sayzyas 0:73dd48be5ca6 180
sayzyas 0:73dd48be5ca6 181 mcchk mcc;
sayzyas 0:73dd48be5ca6 182 sw_zero.mode(PullUp);
sayzyas 0:73dd48be5ca6 183 pc.baud(115200);
sayzyas 0:73dd48be5ca6 184 slave.address(I2C_ADDRESS_RESOLVER);
sayzyas 0:73dd48be5ca6 185 //led_demo();
sayzyas 0:73dd48be5ca6 186
sayzyas 0:73dd48be5ca6 187 // 1234567890123456 1234567890123456
sayzyas 0:73dd48be5ca6 188 lcd_dsp(0,0,"B2' FFWinchProto",16);
sayzyas 0:73dd48be5ca6 189 lcd_dsp(0,1,"Revast Co.,Ltd. ",16);
sayzyas 0:73dd48be5ca6 190 wait_ms(1000);
sayzyas 0:73dd48be5ca6 191 lcd_dsp(0,1,"Now Booting ... ",16);
sayzyas 0:73dd48be5ca6 192
sayzyas 0:73dd48be5ca6 193 DEBUG_PRINT_L0("\r\n");
sayzyas 0:73dd48be5ca6 194 DEBUG_PRINT_L0("LPC824> +-------------------------------------------------------------\r\n");
sayzyas 0:73dd48be5ca6 195 DEBUG_PRINT_L0("LPC824> | Project: B2Dash Debris Explorer Winch test machine\r\n");
sayzyas 0:73dd48be5ca6 196 DEBUG_PRINT_L0("LPC824> |-------------------------------------------------------------\r\n");
sayzyas 0:73dd48be5ca6 197 DEBUG_PRINT_L0("LPC824> | This is: Resolver pulse counter Main\r\n");
sayzyas 0:73dd48be5ca6 198 DEBUG_PRINT_L0("LPC824> | Target MCU: mbed LPC824MAX\r\n");
sayzyas 0:73dd48be5ca6 199 DEBUG_PRINT_L0("LPC824> | Letest update: %s\r\n", LatestUpDate);
sayzyas 0:73dd48be5ca6 200 DEBUG_PRINT_L0("LPC824> | Program Revision: %s\r\n", ProgramRevision);
sayzyas 0:73dd48be5ca6 201 DEBUG_PRINT_L0("LPC824> | Author: %s\r\n", Author);
sayzyas 0:73dd48be5ca6 202 DEBUG_PRINT_L0("LPC824> | Copyright(C) 2017 %s Allright Reserved\r\n", Company);
sayzyas 0:73dd48be5ca6 203 DEBUG_PRINT_L0("LPC824> +-------------------------------------------------------------\r\n");
sayzyas 0:73dd48be5ca6 204
sayzyas 0:73dd48be5ca6 205 wheel.reset();
sayzyas 0:73dd48be5ca6 206
sayzyas 0:73dd48be5ca6 207 wait_ms(1000);
sayzyas 0:73dd48be5ca6 208 lcd_dsp(0,1,"Wait net connect",16);
sayzyas 0:73dd48be5ca6 209 DEBUG_PRINT_L0("LPC824> wait ... \r\n");
sayzyas 0:73dd48be5ca6 210
sayzyas 0:73dd48be5ca6 211 // Thread roop
sayzyas 0:73dd48be5ca6 212 while(1){
sayzyas 0:73dd48be5ca6 213 DEBUG_PRINT_L0("LPC824> x");
sayzyas 0:73dd48be5ca6 214
sayzyas 0:73dd48be5ca6 215 // When push winch position zero reset button then whell reset
sayzyas 0:73dd48be5ca6 216 if( sw_zero == 0 )
sayzyas 0:73dd48be5ca6 217 {
sayzyas 0:73dd48be5ca6 218 wheel.reset();
sayzyas 0:73dd48be5ca6 219 wait_ms(1);
sayzyas 0:73dd48be5ca6 220 }
sayzyas 0:73dd48be5ca6 221
sayzyas 0:73dd48be5ca6 222 /* [ Automatic stop function control ] **************************************
sayzyas 0:73dd48be5ca6 223 * Down(CW)
sayzyas 0:73dd48be5ca6 224 * When camera head reached to MAX_DROP_AMOUNT (= 3500 mm)
sayzyas 0:73dd48be5ca6 225 * then the winch will stop owing to protect system breake.
sayzyas 0:73dd48be5ca6 226 * UP(CCW) Master (= host) shuld always send any IIC packet to client
sayzyas 0:73dd48be5ca6 227 * When camera head reached to home position (= 0000 mm)
sayzyas 0:73dd48be5ca6 228 * then the winch will stop owing to protect system breake.
sayzyas 0:73dd48be5ca6 229 * DROP_AMOUNT_ADJ value is adjustment value for over and under shooting.
sayzyas 0:73dd48be5ca6 230 * ************************************************************************* */
sayzyas 0:73dd48be5ca6 231 if(
sayzyas 0:73dd48be5ca6 232 (dropAmt>=( MAX_DROP_AMOUNT - DROP_AMOUNT_ADJ ))&&(flg_mtrEStop==false) ||
sayzyas 0:73dd48be5ca6 233 (dropAmt<= DROP_AMOUNT_ADJ )&&(flg_mtrEStop==false)
sayzyas 0:73dd48be5ca6 234 ){
sayzyas 0:73dd48be5ca6 235 // DEBUG_PRINT_L1("#### Interuppt to host ####\r\n");
sayzyas 0:73dd48be5ca6 236 flg_mtrEStop = true;
sayzyas 0:73dd48be5ca6 237 int2hst = 1; // interrupt to host controller
sayzyas 0:73dd48be5ca6 238 }
sayzyas 0:73dd48be5ca6 239 else
sayzyas 0:73dd48be5ca6 240 {
sayzyas 0:73dd48be5ca6 241 // DEBUG_PRINT_L1("#### Interuppt off ####\r\n");
sayzyas 0:73dd48be5ca6 242 int2hst = 0; // interrupt off
sayzyas 0:73dd48be5ca6 243 }
sayzyas 0:73dd48be5ca6 244
sayzyas 0:73dd48be5ca6 245 if(
sayzyas 0:73dd48be5ca6 246 ( dropAmt < ( MAX_DROP_AMOUNT - DROP_AMOUNT_ADJ ) ) &&
sayzyas 0:73dd48be5ca6 247 ( dropAmt > DROP_AMOUNT_ADJ )
sayzyas 0:73dd48be5ca6 248 ){
sayzyas 0:73dd48be5ca6 249 flg_mtrEStop = false;
sayzyas 0:73dd48be5ca6 250 }
sayzyas 0:73dd48be5ca6 251
sayzyas 0:73dd48be5ca6 252 /* ** Note ** ****************************************************** */
sayzyas 0:73dd48be5ca6 253 /* This is Slave (= Client) */
sayzyas 0:73dd48be5ca6 254 /* Master (= host) shuld always send any IIC packet to client */
sayzyas 0:73dd48be5ca6 255 /* Because slave is waiting packet here ! */
sayzyas 0:73dd48be5ca6 256 /* ***************************************************************** */
sayzyas 0:73dd48be5ca6 257 i = slave.receive();
sayzyas 0:73dd48be5ca6 258 slave.read(buf, NumberOfI2CCommand);
sayzyas 0:73dd48be5ca6 259
sayzyas 0:73dd48be5ca6 260 switch (i) {
sayzyas 0:73dd48be5ca6 261 case I2CSlave::NoData:
sayzyas 0:73dd48be5ca6 262 // DEBUG_PRINT_L1("the slave has not been addressed\r\n");
sayzyas 0:73dd48be5ca6 263 // read_motor_current(2);
sayzyas 0:73dd48be5ca6 264 break;
sayzyas 0:73dd48be5ca6 265
sayzyas 0:73dd48be5ca6 266 case I2CSlave::ReadAddressed:
sayzyas 0:73dd48be5ca6 267
sayzyas 0:73dd48be5ca6 268 pulse = wheel.getPulses(); /* Master read the current position of winch here! */
sayzyas 0:73dd48be5ca6 269 /* dropAmt calculation */
sayzyas 0:73dd48be5ca6 270 dropAmt_d = ( pulse * ( (double)( ((double)dram_diameter/(double)100) * (double)PAI / (pow(2.0, (double)rresolution)*4 ) ) ) );
sayzyas 0:73dd48be5ca6 271 dropAmt = (int16_t)(dropAmt_d * (double)((double)dis_correct_value/(double)dis_correct_value));
sayzyas 0:73dd48be5ca6 272 dropAmt += pos_offset;
sayzyas 0:73dd48be5ca6 273 dropAmt *= -1;
sayzyas 0:73dd48be5ca6 274 DEBUG_PRINT_L2("LPC824> Pulse: %07d, DropAmount(Offset): %04d(%4d) mm\t (%d, %d)", pulse, dropAmt, pos_offset, dram_diameter, rresolution);
sayzyas 0:73dd48be5ca6 275
sayzyas 0:73dd48be5ca6 276 mc_wdram = mtcnt_wdram.read();
sayzyas 0:73dd48be5ca6 277 mc_wcabl = mtcnt_wcabl.read();
sayzyas 0:73dd48be5ca6 278 sp_wdram_f = (int16_t)(mtspd_wdram_cw.read()*100.0f);
sayzyas 0:73dd48be5ca6 279 sp_wdram_r = (int16_t)(mtspd_wdram_ccw.read()*100.0f);
sayzyas 0:73dd48be5ca6 280 sp_wcabl_f = (int16_t)(mtspd_wcabl_cw.read()*100.0f);
sayzyas 0:73dd48be5ca6 281 sp_wcabl_r = (int16_t)(mtspd_wcabl_ccw.read()*100.0f);
sayzyas 0:73dd48be5ca6 282 // DEBUG_PRINT_L2("Motor speed: Dram CW=%03d CCW=%03d Cable CW=%03d CCW=%03d \t", sp_wdram_f, sp_wdram_r, sp_wcabl_f, sp_wcabl_r);
sayzyas 0:73dd48be5ca6 283 // DEBUG_PRINT_L2("Motor current: Dram=%f Cable=%f \r\n", mc_wdram, mc_wcabl);
sayzyas 0:73dd48be5ca6 284
sayzyas 0:73dd48be5ca6 285 sprintf(msg,"[%04d mm]*", dropAmt);
sayzyas 0:73dd48be5ca6 286 lcd_dsp(3,1," ",4);
sayzyas 0:73dd48be5ca6 287 lcd_dsp(7,1,msg,9);
sayzyas 0:73dd48be5ca6 288
sayzyas 0:73dd48be5ca6 289 msg[0] = 0x12; // Dummy data for data check
sayzyas 0:73dd48be5ca6 290 msg[1] = sp_wdram_f & 0xFF;
sayzyas 0:73dd48be5ca6 291 msg[2] = (sp_wdram_f >> 8) & 0xFF;
sayzyas 0:73dd48be5ca6 292 msg[3] = sp_wdram_r & 0xFF;
sayzyas 0:73dd48be5ca6 293 msg[4] = (sp_wdram_r >> 8) & 0xFF;
sayzyas 0:73dd48be5ca6 294 msg[5] = sp_wcabl_f & 0xFF;
sayzyas 0:73dd48be5ca6 295 msg[6] = (sp_wcabl_f >> 8) & 0xFF;
sayzyas 0:73dd48be5ca6 296 msg[7] = sp_wcabl_r & 0xFF;
sayzyas 0:73dd48be5ca6 297 msg[8] = (sp_wcabl_r >> 8) & 0xFF;
sayzyas 0:73dd48be5ca6 298 msg[9] = dropAmt & 0xFF;
sayzyas 0:73dd48be5ca6 299 msg[10] = (dropAmt >> 8) & 0xFF;
sayzyas 0:73dd48be5ca6 300 msg[11] = 0x34; // Dummy data
sayzyas 0:73dd48be5ca6 301 slave.write(msg, 12); // Includes null char
sayzyas 0:73dd48be5ca6 302 //DEBUG_PRINT_L2("\tGot data : %02x,%02x,%02x,%02x,%02x,%02x,%02x,%02x\r\n", msg[1],msg[2],msg[3],msg[4],msg[5],msg[6],msg[7],msg[8]);
sayzyas 0:73dd48be5ca6 303 // lcd_dsp(3,1,">",1);
sayzyas 0:73dd48be5ca6 304 break;
sayzyas 0:73dd48be5ca6 305
sayzyas 0:73dd48be5ca6 306 case I2CSlave::WriteAddressed:
sayzyas 0:73dd48be5ca6 307 if( buf[I2C_CP_COMMAND] == 'Z'){
sayzyas 0:73dd48be5ca6 308 // DEBUG_PRINT_L1("Z got\r\n");
sayzyas 0:73dd48be5ca6 309 sprintf(msg," %03d", sp_wcabl_f);
sayzyas 0:73dd48be5ca6 310 lcd_dsp(0,0,msg,4);
sayzyas 0:73dd48be5ca6 311 sprintf(msg," %03d", sp_wcabl_r);
sayzyas 0:73dd48be5ca6 312 lcd_dsp(4,0,msg,4);
sayzyas 0:73dd48be5ca6 313 sprintf(msg," %03d", sp_wdram_r);
sayzyas 0:73dd48be5ca6 314 lcd_dsp(8,0,msg,4);
sayzyas 0:73dd48be5ca6 315 sprintf(msg," %03d", sp_wdram_f);
sayzyas 0:73dd48be5ca6 316 lcd_dsp(12,0,msg,4);
sayzyas 0:73dd48be5ca6 317 dram_diameter = ( buf[I2C_CP_WDRAM_DIA_X100_UPPER] << 8 );
sayzyas 0:73dd48be5ca6 318 dram_diameter |= buf[I2C_CP_WDRAM_DIA_X100_LOWER];
sayzyas 0:73dd48be5ca6 319 dis_correct_value = ( buf[I2C_CP_ADJUST_VALUE_X10000_UPPER] << 8 );
sayzyas 0:73dd48be5ca6 320 dis_correct_value |= buf[I2C_CP_ADJUST_VALUE_X10000_LOWER];
sayzyas 0:73dd48be5ca6 321 rresolution = buf[I2C_CP_RESOLVER_RESO];
sayzyas 0:73dd48be5ca6 322 /*
sayzyas 0:73dd48be5ca6 323 DEBUG_PRINT_L1("Bd4> ===========================================\r\n");
sayzyas 0:73dd48be5ca6 324 DEBUG_PRINT_L1("Bd4> Dram Diameter : %d\t(mm)\r\n", dram_diameter);
sayzyas 0:73dd48be5ca6 325 DEBUG_PRINT_L1("Bd4> CCable Diameter : %d\t(mm)\r\n", dis_correct_value);
sayzyas 0:73dd48be5ca6 326 DEBUG_PRINT_L1("Bd4> Resolver Resolution : %d\t(bit)\r\n", rresolution);
sayzyas 0:73dd48be5ca6 327 DEBUG_PRINT_L1("Bd4> -------------------------------------------\r\n", rresolution);
sayzyas 0:73dd48be5ca6 328 DEBUG_PRINT_L1("Bd4> Real Diameter : %d\t(mm)\r\n", (dram_diameter+dis_correct_value));
sayzyas 0:73dd48be5ca6 329 DEBUG_PRINT_L1("Bd4> Rotation Pulse / 1round : %d\t(pulse)\r\n", (int)(pow(2.0, (double)rresolution)*4));
sayzyas 0:73dd48be5ca6 330 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:73dd48be5ca6 331 DEBUG_PRINT_L1("Bd4> ===========================================\r\n");
sayzyas 0:73dd48be5ca6 332 */
sayzyas 0:73dd48be5ca6 333 }
sayzyas 0:73dd48be5ca6 334 else if( buf[I2C_CP_COMMAND] == 'N'){
sayzyas 0:73dd48be5ca6 335 sprintf(msg," %03d", sp_wcabl_f);
sayzyas 0:73dd48be5ca6 336 lcd_dsp(0,0,msg,4);
sayzyas 0:73dd48be5ca6 337 sprintf(msg," %03d", sp_wcabl_r);
sayzyas 0:73dd48be5ca6 338 lcd_dsp(4,0,msg,4);
sayzyas 0:73dd48be5ca6 339 sprintf(msg," %03d", sp_wdram_r);
sayzyas 0:73dd48be5ca6 340 lcd_dsp(8,0,msg,4);
sayzyas 0:73dd48be5ca6 341 sprintf(msg," %03d", sp_wdram_f);
sayzyas 0:73dd48be5ca6 342 lcd_dsp(12,0,msg,4);
sayzyas 0:73dd48be5ca6 343 }
sayzyas 0:73dd48be5ca6 344 // else if( buf[I2C_CP_COMMAND] == 'M'){
sayzyas 0:73dd48be5ca6 345 // lcd_dsp(4,0,"Winch manual",12);
sayzyas 0:73dd48be5ca6 346 // }
sayzyas 0:73dd48be5ca6 347 break;
sayzyas 0:73dd48be5ca6 348 }
sayzyas 0:73dd48be5ca6 349 // Animation
sayzyas 0:73dd48be5ca6 350 if( ii < 5 ){
sayzyas 0:73dd48be5ca6 351 lcd_dsp(0,1,"[|]",3);
sayzyas 0:73dd48be5ca6 352 }
sayzyas 0:73dd48be5ca6 353 else if( ii < 10 ){
sayzyas 0:73dd48be5ca6 354 lcd_dsp(0,1,"[/]",3);
sayzyas 0:73dd48be5ca6 355 }
sayzyas 0:73dd48be5ca6 356 else if ( ii < 15 ){
sayzyas 0:73dd48be5ca6 357 lcd_dsp(0,1,"[-]",3);
sayzyas 0:73dd48be5ca6 358 }
sayzyas 0:73dd48be5ca6 359 ii++;
sayzyas 0:73dd48be5ca6 360 if( ii >= 15 ) ii = 0;
sayzyas 0:73dd48be5ca6 361 wait_ms(5);
sayzyas 0:73dd48be5ca6 362 }
sayzyas 0:73dd48be5ca6 363 }
sayzyas 0:73dd48be5ca6 364