z ysaito / Mbed 2 deprecated Pro_B2_4ZNR_B2DS_ResolverCtrl_824MAX

2018.07.26

Dependencies:   QEI mbed-rtos mbed

4_main.cpp@2:c62dc496b79a, 2016-04-14 (annotated)

Committer:
sayzyas
Date:
Thu Apr 14 10:27:21 2016 +0000
Revision:
2:c62dc496b79a
Parent:
1:86705c6e0ab7
Child:
3:85eb7e954bfa
2016.04.14

Who changed what in which revision?

UserRevisionLine numberNew contents of line
sayzyas 0:b1136bf48c38 1 /**********************************************************
sayzyas 0:b1136bf48c38 2 * Project: B2 (1F-1)
sayzyas 0:b1136bf48c38 3 * Title: CrExp B2 Motor Ctrl Main
sayzyas 0:b1136bf48c38 4 * Target: LPCXpresso824-Max
sayzyas 0:b1136bf48c38 5 * Author: zinsor
sayzyas 0:b1136bf48c38 6 * Date(Latest update) 2015.12.21(Mon)
sayzyas 0:b1136bf48c38 7 * --------------------------------------------------------
sayzyas 0:b1136bf48c38 8 * Article
sayzyas 0:b1136bf48c38 9 *
sayzyas 0:b1136bf48c38 10 * --------------------------------------------------------
sayzyas 0:b1136bf48c38 11 *
sayzyas 0:b1136bf48c38 12 * LPCXpresso 824-MAX
sayzyas 0:b1136bf48c38 13 * +---------USB---------+
sayzyas 0:b1136bf48c38 14 * | |
sayzyas 0:b1136bf48c38 15 * | |
sayzyas 0:b1136bf48c38 16 * | |
sayzyas 0:b1136bf48c38 17 * | |
sayzyas 0:b1136bf48c38 18 * | | SCL P0_10 D15 -> IIC SCL
sayzyas 0:b1136bf48c38 19 * | ## ### ## | SDA P0_11 D14 -> IIC SDA
sayzyas 0:b1136bf48c38 20 * | # # # # # # | AVDD
sayzyas 0:b1136bf48c38 21 * N/A | # # # # # | GND
sayzyas 0:b1136bf48c38 22 * +3V3 | ## # # # | SCK P0_24 D13
sayzyas 0:b1136bf48c38 23 * NRST | # # # # # | MISO P0_25 D12
sayzyas 0:b1136bf48c38 24 * +3V3 | # # # # ###### | MOSI P0_26 D11 -> Resolver B
sayzyas 0:b1136bf48c38 25 * +5V | ## ##### # | SSEL P0_15 D10 -> Resolver A
sayzyas 0:b1136bf48c38 26 * GND | | P0_27 D9
sayzyas 0:b1136bf48c38 27 * GND | | P0_13 D8
sayzyas 0:b1136bf48c38 28 * N/A | | P0_17 D7
sayzyas 0:b1136bf48c38 29 * | | P0_16 D6
sayzyas 0:b1136bf48c38 30 * A0 P0_6 | | PWM P0_28 D5
sayzyas 0:b1136bf48c38 31 * A1 P0_14 | | PWM P0_18 D4
sayzyas 0:b1136bf48c38 32 * A2 P0_23 | | PWM P0_12 D3
sayzyas 0:b1136bf48c38 33 * A3 P0_22 | | PWM P0_19 D2
sayzyas 0:b1136bf48c38 34 * A4 P0_21 | | TX P0_4 D1
sayzyas 0:b1136bf48c38 35 * A5 P0_20 | | RX P0_0 D0
sayzyas 0:b1136bf48c38 36 * +---------------------+
sayzyas 0:b1136bf48c38 37 *
sayzyas 0:b1136bf48c38 38 ***************************************/
sayzyas 0:b1136bf48c38 39
sayzyas 0:b1136bf48c38 40 #include "mbed.h"
sayzyas 0:b1136bf48c38 41 #include "rtos.h"
sayzyas 0:b1136bf48c38 42 #include "QEI.h"
sayzyas 0:b1136bf48c38 43 #include "common.h"
sayzyas 0:b1136bf48c38 44 #include <math.h>
sayzyas 0:b1136bf48c38 45
sayzyas 0:b1136bf48c38 46 // Hardware setting
sayzyas 0:b1136bf48c38 47
sayzyas 0:b1136bf48c38 48 Serial pc(USBTX, USBRX); // tx, rx
sayzyas 0:b1136bf48c38 49
sayzyas 0:b1136bf48c38 50 QEI wheel( P0_15, P0_26, NC, ROTATE_PER_RESOLUTION, QEI::X2_ENCODING );
sayzyas 0:b1136bf48c38 51 /*
sayzyas 0:b1136bf48c38 52 Dram r = 60mm
sayzyas 0:b1136bf48c38 53 Cable fai = 3
sayzyas 0:b1136bf48c38 54
sayzyas 0:b1136bf48c38 55 (60+3)*3.14 = 197.82mm ==> 2^12*4 = 4096 * 4 = 16384 pulse
sayzyas 0:b1136bf48c38 56 1(mm) = 82.82(pulse)
sayzyas 0:b1136bf48c38 57 0.01208(mm) = 1(pulse)
sayzyas 0:b1136bf48c38 58
sayzyas 0:b1136bf48c38 59 */
sayzyas 0:b1136bf48c38 60 DigitalOut led1(LED1); // Red
sayzyas 0:b1136bf48c38 61 DigitalOut led2(LED2); // Green
sayzyas 0:b1136bf48c38 62 DigitalOut led3(LED3); // Blue
sayzyas 0:b1136bf48c38 63
sayzyas 0:b1136bf48c38 64 I2CSlave slave(P0_11, P0_10); //I2C SDA SCL
sayzyas 0:b1136bf48c38 65
sayzyas 0:b1136bf48c38 66 int i2c_saddress = 0;
sayzyas 0:b1136bf48c38 67
sayzyas 0:b1136bf48c38 68 void led_demo(){
sayzyas 0:b1136bf48c38 69 int i;
sayzyas 0:b1136bf48c38 70 for( i = 0; i < 20; i++ ) {
sayzyas 0:b1136bf48c38 71 led1 = LED_ON;
sayzyas 0:b1136bf48c38 72 led2 = LED_OFF;
sayzyas 0:b1136bf48c38 73 led3 = LED_OFF;
sayzyas 0:b1136bf48c38 74 wait_ms(20);
sayzyas 0:b1136bf48c38 75 led1 = LED_OFF;
sayzyas 0:b1136bf48c38 76 led2 = LED_OFF;
sayzyas 0:b1136bf48c38 77 led3 = LED_OFF;
sayzyas 0:b1136bf48c38 78 wait_ms(20);
sayzyas 0:b1136bf48c38 79 led1 = LED_OFF;
sayzyas 0:b1136bf48c38 80 led2 = LED_ON;
sayzyas 0:b1136bf48c38 81 led3 = LED_OFF;
sayzyas 0:b1136bf48c38 82 wait_ms(20);
sayzyas 0:b1136bf48c38 83 led1 = LED_OFF;
sayzyas 0:b1136bf48c38 84 led2 = LED_OFF;
sayzyas 0:b1136bf48c38 85 led3 = LED_OFF;
sayzyas 0:b1136bf48c38 86 wait_ms(20);
sayzyas 0:b1136bf48c38 87 led1 = LED_OFF;
sayzyas 0:b1136bf48c38 88 led2 = LED_OFF;
sayzyas 0:b1136bf48c38 89 led3 = LED_ON;
sayzyas 0:b1136bf48c38 90 wait_ms(20);
sayzyas 0:b1136bf48c38 91 led1 = LED_OFF;
sayzyas 0:b1136bf48c38 92 led2 = LED_OFF;
sayzyas 0:b1136bf48c38 93 led3 = LED_OFF;
sayzyas 0:b1136bf48c38 94 wait_ms(20);
sayzyas 0:b1136bf48c38 95 }
sayzyas 0:b1136bf48c38 96 // wait_ms(3000);
sayzyas 0:b1136bf48c38 97 }
sayzyas 0:b1136bf48c38 98
sayzyas 0:b1136bf48c38 99 // ========================================================================
sayzyas 0:b1136bf48c38 100 // Thread: Resolver Control Task
sayzyas 0:b1136bf48c38 101 // ========================================================================
sayzyas 0:b1136bf48c38 102 void ResolverCtrl_task(void const *) {
sayzyas 0:b1136bf48c38 103
sayzyas 0:b1136bf48c38 104
sayzyas 0:b1136bf48c38 105 }
sayzyas 0:b1136bf48c38 106
sayzyas 0:b1136bf48c38 107 // ========================================================================
sayzyas 0:b1136bf48c38 108 // Thread: Main Controller Interface Task
sayzyas 0:b1136bf48c38 109 // Get Command from Main controller and do the task.
sayzyas 0:b1136bf48c38 110 // ========================================================================
sayzyas 0:b1136bf48c38 111 void MainCtrl_Interface_task(void const *) {
sayzyas 0:b1136bf48c38 112
sayzyas 2:c62dc496b79a 113 char buf[14]; // command buffer
sayzyas 0:b1136bf48c38 114 char msg[] = "C000000";
sayzyas 0:b1136bf48c38 115 char res_msg[] = "Slave!";
sayzyas 0:b1136bf48c38 116 char res_msg2[] = "Bravo!";
sayzyas 0:b1136bf48c38 117
sayzyas 0:b1136bf48c38 118 int flg_readCurrent = 0;
sayzyas 0:b1136bf48c38 119
sayzyas 0:b1136bf48c38 120 int pulse;
sayzyas 0:b1136bf48c38 121 int16_t distance;
sayzyas 0:b1136bf48c38 122
sayzyas 0:b1136bf48c38 123 uint16_t dram_diameter;
sayzyas 0:b1136bf48c38 124 uint16_t ccable_diameter;
sayzyas 0:b1136bf48c38 125 uint8_t rresolution;
sayzyas 0:b1136bf48c38 126
sayzyas 0:b1136bf48c38 127 int tmp;
sayzyas 0:b1136bf48c38 128
sayzyas 0:b1136bf48c38 129 slave.address(i2c_saddress);
sayzyas 0:b1136bf48c38 130
sayzyas 0:b1136bf48c38 131 DEBUG_PRINT_L1("*** Start Resolver thread ***\n");
sayzyas 0:b1136bf48c38 132
sayzyas 0:b1136bf48c38 133 /*
sayzyas 0:b1136bf48c38 134 * I2C Access
sayzyas 0:b1136bf48c38 135 */
sayzyas 0:b1136bf48c38 136 while(1) {
sayzyas 0:b1136bf48c38 137 // DEBUG_PRINT_L1(".");
sayzyas 0:b1136bf48c38 138 int i = slave.receive();
sayzyas 0:b1136bf48c38 139 slave.read(buf, NumberOfI2CCommand);
sayzyas 0:b1136bf48c38 140 // DEBUG_PRINT_L1("?");
sayzyas 0:b1136bf48c38 141 // DEBUG_PRINT_L1("Slave Received\r\n");
sayzyas 0:b1136bf48c38 142 switch (i) {
sayzyas 0:b1136bf48c38 143 case I2CSlave::NoData:
sayzyas 0:b1136bf48c38 144 // DEBUG_PRINT_L1("the slave has not been addressed\r\n");
sayzyas 0:b1136bf48c38 145 // read_motor_current(2);
sayzyas 0:b1136bf48c38 146 break;
sayzyas 0:b1136bf48c38 147 case I2CSlave::ReadAddressed:
sayzyas 0:b1136bf48c38 148 /* Master is reading winch current position here */
sayzyas 0:b1136bf48c38 149 // DEBUG_PRINT_L1("the master has requested a read from this slave\r\n");
sayzyas 0:b1136bf48c38 150 pulse = wheel.getPulses();
sayzyas 0:b1136bf48c38 151
sayzyas 0:b1136bf48c38 152 distance = (int16_t)( pulse * ( (double)( ((dram_diameter+ccable_diameter)/100) * PAI / (pow(2.0, (double)rresolution)*4 ) ) ) );
sayzyas 0:b1136bf48c38 153 // distance = (int16_t)( pulse * ( REAL_THREAD_DIAMETER * PAI / ROTATION_PULSE_PER_1ROUND ));
sayzyas 0:b1136bf48c38 154
sayzyas 0:b1136bf48c38 155 DEBUG_PRINT_L1("Bd4> PULSE: %07d, ", pulse);
sayzyas 0:b1136bf48c38 156 DEBUG_PRINT_L1("DISTANCE: %04d (mm)", distance);
sayzyas 0:b1136bf48c38 157 res_msg2[0] = distance & 0xFF;
sayzyas 0:b1136bf48c38 158 res_msg2[1] = (distance >> 8)&0xFF;
sayzyas 0:b1136bf48c38 159 slave.write(res_msg2, 2); // Includes null char
sayzyas 0:b1136bf48c38 160 tmp = (res_msg2[1] < 8)&0xFF00 | res_msg2[0]&0xFF;
sayzyas 0:b1136bf48c38 161 DEBUG_PRINT_L1("\tSend data : %04d(%02x,%02x)\n", tmp, res_msg2[1], res_msg2[0]);
sayzyas 0:b1136bf48c38 162 break;
sayzyas 0:b1136bf48c38 163 case I2CSlave::WriteGeneral:
sayzyas 0:b1136bf48c38 164 DEBUG_PRINT_L1("Bd4> the master is writing to all slave\r\n");
sayzyas 0:b1136bf48c38 165 slave.read(buf, NumberOfI2CCommand);
sayzyas 0:b1136bf48c38 166 DEBUG_PRINT_L1("Bd4> Read G: %s\r\n", buf);
sayzyas 0:b1136bf48c38 167 break;
sayzyas 0:b1136bf48c38 168 case I2CSlave::WriteAddressed:
sayzyas 0:b1136bf48c38 169 /* ******************************************* */
sayzyas 0:b1136bf48c38 170 /* Add setting data set operation in this area */
sayzyas 0:b1136bf48c38 171 /* ******************************************* */
sayzyas 2:c62dc496b79a 172 if( buf[I2C_CP_COMMAND_R] == 'Z'){
sayzyas 0:b1136bf48c38 173 wheel.reset();
sayzyas 0:b1136bf48c38 174 }
sayzyas 0:b1136bf48c38 175 else{
sayzyas 2:c62dc496b79a 176 for( int j = 0; j < NumberOfI2CCommand; j++)
sayzyas 2:c62dc496b79a 177 pc,printf("%02x ", buf[j]);
sayzyas 2:c62dc496b79a 178 pc.printf( "\r\n" );
sayzyas 2:c62dc496b79a 179
sayzyas 2:c62dc496b79a 180 dram_diameter = ( buf[I2C_CP_WDRAM_DIA_UPPER] << 8 );
sayzyas 2:c62dc496b79a 181 dram_diameter |= buf[I2C_CP_WDRAM_DIA_LOWER];
sayzyas 2:c62dc496b79a 182 ccable_diameter = ( buf[I2C_CP_CCABLE_DIA_UPPER] << 8 );
sayzyas 2:c62dc496b79a 183 ccable_diameter |= buf[I2C_CP_CCABLE_DIA_LOWER];
sayzyas 2:c62dc496b79a 184 rresolution = buf[I2C_CP_RESOLVER_RESO];
sayzyas 0:b1136bf48c38 185
sayzyas 0:b1136bf48c38 186 DEBUG_PRINT_L1("Bd4> ===========================================\n");
sayzyas 0:b1136bf48c38 187 DEBUG_PRINT_L1("Bd4> Dram Diameter : %d\t(mm)\n", dram_diameter/100);
sayzyas 0:b1136bf48c38 188 DEBUG_PRINT_L1("Bd4> CCable Diameter : %d\t(mm)\n", ccable_diameter/100);
sayzyas 0:b1136bf48c38 189 DEBUG_PRINT_L1("Bd4> Resolver Resolution : %d\t(bit)\n", rresolution);
sayzyas 0:b1136bf48c38 190 DEBUG_PRINT_L1("Bd4> -------------------------------------------\n", rresolution);
sayzyas 0:b1136bf48c38 191 DEBUG_PRINT_L1("Bd4> Real Diameter : %d\t(mm)\n", (dram_diameter+ccable_diameter)/100);
sayzyas 0:b1136bf48c38 192 DEBUG_PRINT_L1("Bd4> Rotation Pulse / 1round : %d\t(pulse)\n", (int)(pow(2.0, (double)rresolution)*4));
sayzyas 0:b1136bf48c38 193 DEBUG_PRINT_L1("Bd4> Distance / 1pulse : %lf\t(mm)\n", (double)( ((dram_diameter+ccable_diameter)/100) * PAI / (pow(2.0, (double)rresolution)*4 ) ) );
sayzyas 0:b1136bf48c38 194 DEBUG_PRINT_L1("Bd4> ===========================================\n");
sayzyas 0:b1136bf48c38 195
sayzyas 0:b1136bf48c38 196 pulse = wheel.getPulses();
sayzyas 0:b1136bf48c38 197 DEBUG_PRINT_L1("Bd4> PULSE: %07d, ", pulse);
sayzyas 0:b1136bf48c38 198 // DEBUG_PRINT_L1("Bd4> >DISTANCE: %04d (mm)\n", (int)( pulse * ( REAL_THREAD_DIAMETER * PAI / ROTATION_PULSE_PER_1ROUND )));
sayzyas 0:b1136bf48c38 199 DEBUG_PRINT_L1("DISTANCE: %04d (mm)\n", (int)( pulse * ( (double)( ((dram_diameter+ccable_diameter)/100) * PAI / (pow(2.0, (double)rresolution)*4 ) ) ) ) );
sayzyas 0:b1136bf48c38 200 DEBUG_PRINT_L1("Bd4> ---------------------------------------\n");
sayzyas 0:b1136bf48c38 201 }
sayzyas 0:b1136bf48c38 202 break;
sayzyas 0:b1136bf48c38 203 }
sayzyas 0:b1136bf48c38 204 Thread::wait(1);
sayzyas 0:b1136bf48c38 205 }
sayzyas 0:b1136bf48c38 206 }
sayzyas 0:b1136bf48c38 207 // ========================================================================
sayzyas 0:b1136bf48c38 208 // Main Function
sayzyas 0:b1136bf48c38 209 // ========================================================================
sayzyas 0:b1136bf48c38 210 int main() {
sayzyas 0:b1136bf48c38 211 int32_t counter = 0;
sayzyas 0:b1136bf48c38 212
sayzyas 0:b1136bf48c38 213 pc.baud(115200);
sayzyas 0:b1136bf48c38 214
sayzyas 0:b1136bf48c38 215 i2c_saddress = Target_IIC_ADDR; // defined at header
sayzyas 0:b1136bf48c38 216
sayzyas 0:b1136bf48c38 217 DEBUG_PRINT_L0("> Board type: Resolver Counter controller(0x%02x)\r\n",i2c_saddress );
sayzyas 0:b1136bf48c38 218
sayzyas 1:86705c6e0ab7 219 i2c_saddress = I2C_ADDRESS_RESOLVER;
sayzyas 0:b1136bf48c38 220
sayzyas 0:b1136bf48c38 221 led_demo();
sayzyas 0:b1136bf48c38 222
sayzyas 0:b1136bf48c38 223 DEBUG_PRINT_L0("\r\n");
sayzyas 0:b1136bf48c38 224 DEBUG_PRINT_L0("Bd4> +--------------------------------------\r\n");
sayzyas 0:b1136bf48c38 225 DEBUG_PRINT_L0("Bd4> | B2 CrExp Resolver Pulse Counter Main \r\n");
sayzyas 0:b1136bf48c38 226 DEBUG_PRINT_L0("Bd4> | Letest update: %s\r\n", LatestUpDate);
sayzyas 0:b1136bf48c38 227 DEBUG_PRINT_L0("Bd4> | Program Revision: %s\r\n", ProgramRevision);
sayzyas 0:b1136bf48c38 228 DEBUG_PRINT_L0("Bd4> | Author: %s\r\n", Author);
sayzyas 0:b1136bf48c38 229 DEBUG_PRINT_L0("Bd4> | Copyright(C) 2015 %s Allright Reserved\r\n", Company);
sayzyas 0:b1136bf48c38 230 DEBUG_PRINT_L0("Bd4> +--------------------------------------\r\n");
sayzyas 0:b1136bf48c38 231
sayzyas 0:b1136bf48c38 232 Thread resolverControlTask1(MainCtrl_Interface_task, NULL, osPriorityNormal, 128 * 4);
sayzyas 0:b1136bf48c38 233 wheel.reset();
sayzyas 0:b1136bf48c38 234
sayzyas 0:b1136bf48c38 235 while(1){
sayzyas 0:b1136bf48c38 236
sayzyas 0:b1136bf48c38 237 counter++;
sayzyas 0:b1136bf48c38 238 if( counter >= 50 ){
sayzyas 0:b1136bf48c38 239 led3 = !led3; // Blue
sayzyas 0:b1136bf48c38 240 counter = 0;
sayzyas 0:b1136bf48c38 241 }
sayzyas 0:b1136bf48c38 242 Thread::wait(5);
sayzyas 0:b1136bf48c38 243 }
sayzyas 0:b1136bf48c38 244 }