eDES2WD.cpp

00001 #include "mbed.h"
00002 #include "eDES2WD.h"
00003 
00004 I2C i2c(p28, p27);
00005 
00006 void eDES2WD_reset (int id) {
00007   char cmd[] = {1, I2C_WCMD_RESET};
00008   int adrs;
00009     
00010   if (0 < id){
00011     adrs = SUB_ADRS + (id - 1) * 2;
00012   } else {
00013     adrs = 0;
00014   }
00015   i2c.write(adrs, cmd, 2);
00016 }
00017 
00018 void eDES2WD_motor (int id, int l_speed, int r_speed) {
00019   char cmd[] = {3, I2C_WCMD_MOTOR, 0, 0};
00020   int adrs;
00021     
00022   cmd[2] = l_speed;
00023   cmd[3] = r_speed;
00024   if (0 < id){
00025     adrs = SUB_ADRS + (id - 1) * 2;
00026   } else {
00027     adrs = 0;
00028   }
00029   i2c.write(adrs, cmd, 4);
00030 }
00031 
00032 void eDES2WD_set_led (int id, int led, int sw) {
00033   char cmd[] = {3, I2C_WCMD_LED, 0, 0};
00034   int adrs;
00035     
00036   cmd[2] = led;
00037   cmd[3] = sw;
00038   if (0 < id){
00039     adrs = SUB_ADRS + (id - 1) * 2;
00040   } else {
00041     adrs = 0;
00042   }
00043   i2c.write(adrs, cmd, 4);
00044 }
00045 
00046 unsigned int eDES2WD_get_sensor(int id, int cn, unsigned int *dat) {
00047     char cmd[] = {2, I2C_RCMD_SENSOR, 0};
00048     char recv[MAX_CN];
00049     unsigned int d;
00050     int adrs;
00051     int nmax, n;
00052     
00053     if (id < 1 || 9 < id) return(0);
00054     adrs = SUB_ADRS + (id - 1) * 2;
00055     nmax = cn < MAX_CN ? 2 : MAX_CN * 2 + 2;
00056     
00057     cmd[2] = cn;
00058     i2c.write(adrs, cmd, 3);
00059     i2c.read(adrs | 1, recv, nmax);
00060     if (nmax == 2){
00061         d = recv[0] | (recv[1] << 8);
00062         return d;
00063     } else {
00064         for (n = 0; n < MAX_CN; n++){
00065             d = recv[0] | (recv[1] << 8);
00066             dat[n] = d;
00067         }
00068 //      gSUB_LED_G = recv[20];
00069 //      gSUB_LED_R = recv[21];
00070     }
00071     return 0;
00072 }
00073 
00074 int eDES2WD_get_lat(int id, int cno) {
00075     char cmd[] = {2, I2C_RCMD_LAT, 0};
00076     char recv[MAX_CN];
00077     int adrs;
00078     
00079     if (id < 1 || 9 < id) return(false);
00080     if (MAX_CN - 1 < cno) return(false);
00081     adrs = SUB_ADRS + (id - 1) * 2;
00082 
00083     cmd[2] = cno;
00084     i2c.write(adrs, cmd, 3);
00085     i2c.read(adrs | 1, recv, 1);
00086     return recv[0];
00087 }