Masayoshi Abe / Mbed 2 deprecated WireFSHandControl

For Wire Hand

Dependencies:   mbed mbed-rtos EthernetInterface

fdc.h@0:88bfb16a6ad5, 2020-04-24 (annotated)

Committer:
Masayoshi
Date:
Fri Apr 24 03:11:11 2020 +0000
Revision:
0:88bfb16a6ad5
KYOSO edition

Who changed what in which revision?

UserRevisionLine numberNew contents of line
Masayoshi 0:88bfb16a6ad5 1 /*-----------------------------------------------------------------
Masayoshi 0:88bfb16a6ad5 2 * fdc.h
Masayoshi 0:88bfb16a6ad5 3 */
Masayoshi 0:88bfb16a6ad5 4 // debug port
Masayoshi 0:88bfb16a6ad5 5 DigitalOut d19(p19);
Masayoshi 0:88bfb16a6ad5 6 DigitalOut d20(p20);
Masayoshi 0:88bfb16a6ad5 7 DigitalOut d21(p21);
Masayoshi 0:88bfb16a6ad5 8 DigitalOut d22(p22);
Masayoshi 0:88bfb16a6ad5 9 DigitalOut d23(p23);
Masayoshi 0:88bfb16a6ad5 10 DigitalOut d24(p24);
Masayoshi 0:88bfb16a6ad5 11 DigitalOut d25(p25);
Masayoshi 0:88bfb16a6ad5 12 DigitalOut d26(p26);
Masayoshi 0:88bfb16a6ad5 13
Masayoshi 0:88bfb16a6ad5 14 //static const char * DST_ADDRESS = "192.168.140.2";
Masayoshi 0:88bfb16a6ad5 15 //static const char * MY_ADDRESS = "192.168.140.8";
Masayoshi 0:88bfb16a6ad5 16 //static const int DST_PORT = 6008;
Masayoshi 0:88bfb16a6ad5 17 //static const int MY_PORT = 6008;
Masayoshi 0:88bfb16a6ad5 18 //static const char * NETMASK = "255.255.0.0";
Masayoshi 0:88bfb16a6ad5 19 //static const char * GATEWAY = "0.0.0.0";
Masayoshi 0:88bfb16a6ad5 20
Masayoshi 0:88bfb16a6ad5 21 //static const int USB_BAUDRATE = 9600;
Masayoshi 0:88bfb16a6ad5 22
Masayoshi 0:88bfb16a6ad5 23 static const uint8_t ADDR_I2C = 0xFF; //Address on FDC1004 for measurement register
Masayoshi 0:88bfb16a6ad5 24 static const uint8_t REG_FDC = 0xA0; //int REG_FDC = 80; //Address on FDC1004 for measurement register b1010000
Masayoshi 0:88bfb16a6ad5 25 static const uint8_t REG_MUX = 0xE0; //int REG_MEX =112; //Address on PCA9548 for measurement register b1110000
Masayoshi 0:88bfb16a6ad5 26 static char FDC_CONFIG[] = {0x08, 0x09, 0x0A, 0x0B};
Masayoshi 0:88bfb16a6ad5 27 static char FDC_MSB[] = {0x00, 0x02, 0x04, 0x06};
Masayoshi 0:88bfb16a6ad5 28 static char FDC_LSB[] = {0x01, 0x03, 0x05, 0x07};
Masayoshi 0:88bfb16a6ad5 29
Masayoshi 0:88bfb16a6ad5 30 static const int FDC_NUM1 = 2;
Masayoshi 0:88bfb16a6ad5 31 static const int FDC_NUM2 = 1;
Masayoshi 0:88bfb16a6ad5 32 static const int FDC_NUM = FDC_NUM1 + FDC_NUM2; //MUXに接続しているFDC1004の数、ch0から順に接続
Masayoshi 0:88bfb16a6ad5 33 //static const int NUM_AXES = 4;
Masayoshi 0:88bfb16a6ad5 34 static const int NUM_DEN = 4; // DENKYOKU
Masayoshi 0:88bfb16a6ad5 35 //static const int UDP_TX_SIZE = sizeof(float) * FDC_NUM * NUM_AXES;
Masayoshi 0:88bfb16a6ad5 36
Masayoshi 0:88bfb16a6ad5 37 //union UdpTx {
Masayoshi 0:88bfb16a6ad5 38 // float data[FDC_NUM][NUM_AXES];
Masayoshi 0:88bfb16a6ad5 39 // char bytes[4*FDC_NUM*NUM_AXES];
Masayoshi 0:88bfb16a6ad5 40 //};
Masayoshi 0:88bfb16a6ad5 41
Masayoshi 0:88bfb16a6ad5 42 //RawSerial pc(USBTX, USBRX);
Masayoshi 0:88bfb16a6ad5 43 //I2C i2c(p9, p10);
Masayoshi 0:88bfb16a6ad5 44 //DigitalOut led1(LED1);
Masayoshi 0:88bfb16a6ad5 45
Masayoshi 0:88bfb16a6ad5 46 /*static void setupUdp(UDPSocket& sock) {
Masayoshi 0:88bfb16a6ad5 47 EthernetInterface eth;
Masayoshi 0:88bfb16a6ad5 48
Masayoshi 0:88bfb16a6ad5 49 pc.printf("Setup UDP\r\n");
Masayoshi 0:88bfb16a6ad5 50
Masayoshi 0:88bfb16a6ad5 51 eth.init(MY_ADDRESS, NETMASK, GATEWAY);
Masayoshi 0:88bfb16a6ad5 52 pc.printf("eth.init()\r\n");
Masayoshi 0:88bfb16a6ad5 53 eth.connect(15000);
Masayoshi 0:88bfb16a6ad5 54 pc.printf("eth.connect()\r\n");
Masayoshi 0:88bfb16a6ad5 55
Masayoshi 0:88bfb16a6ad5 56 const char *ip = eth.getIPAddress();
Masayoshi 0:88bfb16a6ad5 57 pc.printf("IP: %s\r\n", ip ? ip : "No IP");
Masayoshi 0:88bfb16a6ad5 58
Masayoshi 0:88bfb16a6ad5 59 sock.bind(MY_PORT);
Masayoshi 0:88bfb16a6ad5 60 pc.printf("Port: %d\r\n", MY_PORT);
Masayoshi 0:88bfb16a6ad5 61 }*/
Masayoshi 0:88bfb16a6ad5 62
Masayoshi 0:88bfb16a6ad5 63 /* PCA9548を使ったch切替　 */
Masayoshi 0:88bfb16a6ad5 64 static inline bool switchI2c(I2C *i2c, uint8_t ch) {
Masayoshi 0:88bfb16a6ad5 65 bool ret = true;
Masayoshi 0:88bfb16a6ad5 66 char cmd[1] = {1 << ch};
Masayoshi 0:88bfb16a6ad5 67 int result = i2c->write(REG_MUX, cmd, 1, false);
Masayoshi 0:88bfb16a6ad5 68 if (result != 0) { // Error
Masayoshi 0:88bfb16a6ad5 69 //pc.printf("switchI2c error \r\n");
Masayoshi 0:88bfb16a6ad5 70 //ERROR
Masayoshi 0:88bfb16a6ad5 71 d22 = !d22;
Masayoshi 0:88bfb16a6ad5 72 //wait(0.2);
Masayoshi 0:88bfb16a6ad5 73 d22 = !d22;
Masayoshi 0:88bfb16a6ad5 74 ret = false;
Masayoshi 0:88bfb16a6ad5 75 }
Masayoshi 0:88bfb16a6ad5 76 return ret;
Masayoshi 0:88bfb16a6ad5 77 }
Masayoshi 0:88bfb16a6ad5 78
Masayoshi 0:88bfb16a6ad5 79 /* FDC1004の設定をする関数　 */
Masayoshi 0:88bfb16a6ad5 80 static inline bool initFdc(I2C *i2c){
Masayoshi 0:88bfb16a6ad5 81 char cmd1[3] = {0x0C, 0x80, 0x00};
Masayoshi 0:88bfb16a6ad5 82 char cmd2[1] = {ADDR_I2C};
Masayoshi 0:88bfb16a6ad5 83 i2c->write(REG_FDC, cmd1, 3, false);
Masayoshi 0:88bfb16a6ad5 84 i2c->write(REG_FDC, cmd2, 1, false); // DeviceIDの確認
Masayoshi 0:88bfb16a6ad5 85
Masayoshi 0:88bfb16a6ad5 86 //idチェック　16,4 と出たらOK。
Masayoshi 0:88bfb16a6ad5 87 char res[2];
Masayoshi 0:88bfb16a6ad5 88 i2c->read(REG_FDC, res, 2);
Masayoshi 0:88bfb16a6ad5 89
Masayoshi 0:88bfb16a6ad5 90 return res[0] == 16 && res[1] == 4;
Masayoshi 0:88bfb16a6ad5 91 }
Masayoshi 0:88bfb16a6ad5 92
Masayoshi 0:88bfb16a6ad5 93 static inline uint8_t highByte(uint16_t x) {
Masayoshi 0:88bfb16a6ad5 94 return (x >> 8) & 0xFF;
Masayoshi 0:88bfb16a6ad5 95 }
Masayoshi 0:88bfb16a6ad5 96
Masayoshi 0:88bfb16a6ad5 97 static inline uint8_t lowByte(uint16_t x) {
Masayoshi 0:88bfb16a6ad5 98 return (x >> 0) & 0xFF;
Masayoshi 0:88bfb16a6ad5 99 }
Masayoshi 0:88bfb16a6ad5 100
Masayoshi 0:88bfb16a6ad5 101 static inline void config(I2C *i2c, int rate, int (&capdac)[NUM_DEN]){
Masayoshi 0:88bfb16a6ad5 102 // rate設定 1:100S/s, 2:200S/s, 3:400S/s
Masayoshi 0:88bfb16a6ad5 103
Masayoshi 0:88bfb16a6ad5 104 uint16_t conf_fdc, conf_meas;
Masayoshi 0:88bfb16a6ad5 105
Masayoshi 0:88bfb16a6ad5 106 // conf_fdc 測定頻度と使うチャンネルを設定
Masayoshi 0:88bfb16a6ad5 107 conf_fdc = (rate << 10) + (1 << 8) + (0b1111 << 4);
Masayoshi 0:88bfb16a6ad5 108 char cmd[3] = {0x0C, highByte(conf_fdc), lowByte(conf_fdc)};
Masayoshi 0:88bfb16a6ad5 109 i2c->write(REG_FDC, cmd, 3, false);
Masayoshi 0:88bfb16a6ad5 110
Masayoshi 0:88bfb16a6ad5 111 // conf_meas CAPDACの設定
Masayoshi 0:88bfb16a6ad5 112 for(int i=0; i<4;i++){
Masayoshi 0:88bfb16a6ad5 113 if(capdac[i] >= 0){
Masayoshi 0:88bfb16a6ad5 114 conf_meas = (i << 13) + (0b100 << 10) + (capdac[i] << 5);
Masayoshi 0:88bfb16a6ad5 115 } else if (capdac[i] == -1){
Masayoshi 0:88bfb16a6ad5 116 conf_meas = (i << 13) + (0b111 << 10);
Masayoshi 0:88bfb16a6ad5 117 }
Masayoshi 0:88bfb16a6ad5 118 cmd[0] = FDC_CONFIG[i];
Masayoshi 0:88bfb16a6ad5 119 cmd[1] = highByte(conf_meas);
Masayoshi 0:88bfb16a6ad5 120 cmd[2] = lowByte(conf_meas);
Masayoshi 0:88bfb16a6ad5 121 int result = i2c->write(REG_FDC, cmd, 3, false);
Masayoshi 0:88bfb16a6ad5 122 }
Masayoshi 0:88bfb16a6ad5 123 }
Masayoshi 0:88bfb16a6ad5 124
Masayoshi 0:88bfb16a6ad5 125 static inline int readRegister(I2C *i2c, char address) {
Masayoshi 0:88bfb16a6ad5 126 char cmd[1] = {address};
Masayoshi 0:88bfb16a6ad5 127 char res[2];
Masayoshi 0:88bfb16a6ad5 128
Masayoshi 0:88bfb16a6ad5 129 i2c->write(REG_FDC, cmd, 1, true); // no stop
Masayoshi 0:88bfb16a6ad5 130 i2c->read (REG_FDC, res, 2);
Masayoshi 0:88bfb16a6ad5 131 return (int)((int)res[0] << 8) | res[1];
Masayoshi 0:88bfb16a6ad5 132 }
Masayoshi 0:88bfb16a6ad5 133
Masayoshi 0:88bfb16a6ad5 134 static inline int readMeas(I2C *i2c, int i) {
Masayoshi 0:88bfb16a6ad5 135 int msb = readRegister(i2c, FDC_MSB[i]);
Masayoshi 0:88bfb16a6ad5 136 return (msb << 8);
Masayoshi 0:88bfb16a6ad5 137 }
Masayoshi 0:88bfb16a6ad5 138
Masayoshi 0:88bfb16a6ad5 139 static inline void senseCapacitance(I2C *i2c, float (&c)[NUM_DEN], int (&capdac)[NUM_DEN]){
Masayoshi 0:88bfb16a6ad5 140 for (int i=0; i<NUM_DEN; i++) {
Masayoshi 0:88bfb16a6ad5 141 c[i] = ((float)readMeas(i2c, i)/0x80000);
Masayoshi 0:88bfb16a6ad5 142 if (c[i] >= 16) {
Masayoshi 0:88bfb16a6ad5 143 c[i] -= 32;
Masayoshi 0:88bfb16a6ad5 144 }
Masayoshi 0:88bfb16a6ad5 145 if (capdac[i] > 0) {
Masayoshi 0:88bfb16a6ad5 146 c[i] += float(capdac[i]) * 3.125f / 0x80000;
Masayoshi 0:88bfb16a6ad5 147 }
Masayoshi 0:88bfb16a6ad5 148 }
Masayoshi 0:88bfb16a6ad5 149 }
Masayoshi 0:88bfb16a6ad5 150
Masayoshi 0:88bfb16a6ad5 151 // TODO リファクタリング後の関数（senseCapacitance）が、元の関数と同じ値になるか要検証
Masayoshi 0:88bfb16a6ad5 152 /*static void senseCapacitance_orig(float (&c)[NUM_AXES], int (&capdac)[NUM_AXES]){
Masayoshi 0:88bfb16a6ad5 153 unsigned int mbb1, mbb2, mbb3; // variables to store 16-bit unsigned integers
Masayoshi 0:88bfb16a6ad5 154 char res1[2];
Masayoshi 0:88bfb16a6ad5 155 char res2[1];
Masayoshi 0:88bfb16a6ad5 156
Masayoshi 0:88bfb16a6ad5 157 for(int i=0; i<NUM_AXES; i++){
Masayoshi 0:88bfb16a6ad5 158 i2c.write(REG_FDC, FDC_MSB + i, 1, true);
Masayoshi 0:88bfb16a6ad5 159 i2c.read(REG_FDC, res1, 2);
Masayoshi 0:88bfb16a6ad5 160
Masayoshi 0:88bfb16a6ad5 161 i2c.write(REG_FDC, FDC_LSB + i, 1, true);
Masayoshi 0:88bfb16a6ad5 162 i2c.read(REG_FDC, res2, 1);
Masayoshi 0:88bfb16a6ad5 163
Masayoshi 0:88bfb16a6ad5 164 // Below are the calculations to take the 24-bit measurement and convert into capacitance for MEAS1
Masayoshi 0:88bfb16a6ad5 165 // * From datasheet: p.16 Section 8.6.1.1 shows the formula (all we need to do is shift the 24-bit value right by 19)
Masayoshi 0:88bfb16a6ad5 166 // ** Shifting by 19 takes a 24 bit number and makes it a 5 bit number with 19 bits after the decimal point
Masayoshi 0:88bfb16a6ad5 167 mbb1 = res1[0] * 256 + res1[1]; // Puts 16 most significant bits into one integer for left sensor measurement
Masayoshi 0:88bfb16a6ad5 168 mbb2 = mbb1 >> 11; // Sets mbb2 to the 5 bits that exist before the decimal point ｐFの整数部分5bit
Masayoshi 0:88bfb16a6ad5 169 mbb3 = 0b0000011111111111 & mbb1; // Sets mbb3 to the 11 bits that exist after the decimal point　ｐFの小数点以下の11bit
Masayoshi 0:88bfb16a6ad5 170
Masayoshi 0:88bfb16a6ad5 171 c[i] = (float)mbb2 + (float)mbb3 / 2048 + (float)res2[0] / 524288; // 単位pF
Masayoshi 0:88bfb16a6ad5 172 if (capdac[i] > 0) {
Masayoshi 0:88bfb16a6ad5 173 c[i] += float(capdac[i]) * float(3.125); //CAPDACによるoffset追加
Masayoshi 0:88bfb16a6ad5 174 }
Masayoshi 0:88bfb16a6ad5 175 }
Masayoshi 0:88bfb16a6ad5 176 }*/
Masayoshi 0:88bfb16a6ad5 177
Masayoshi 0:88bfb16a6ad5 178 /*static void displayValues(float (&values)[FDC_NUM][NUM_AXES]) {
Masayoshi 0:88bfb16a6ad5 179 for (int j=0; j<FDC_NUM; j++) {
Masayoshi 0:88bfb16a6ad5 180 for(int i=0; i<NUM_AXES; i++) {
Masayoshi 0:88bfb16a6ad5 181 pc.printf("|%5.2f|", values[j][i]);
Masayoshi 0:88bfb16a6ad5 182 }
Masayoshi 0:88bfb16a6ad5 183 }
Masayoshi 0:88bfb16a6ad5 184 pc.printf("\r\n");
Masayoshi 0:88bfb16a6ad5 185 }*/