Suga koubou
/
eDES2WD_test
http://mbed.org/users/okini3939/notebook/daisen-edes/
eDES2WD.cpp
- Committer:
- okini3939
- Date:
- 2012-02-23
- Revision:
- 1:3c8a6bd50779
- Parent:
- 0:141734449aeb
File content as of revision 1:3c8a6bd50779:
#include "mbed.h" #include "eDES2WD.h" I2C i2c(p28, p27); void eDES2WD_reset (int id) { char cmd[] = {1, I2C_WCMD_RESET}; int adrs; if (0 < id){ adrs = SUB_ADRS + (id - 1) * 2; } else { adrs = 0; } i2c.write(adrs, cmd, 2); } void eDES2WD_motor (int id, int l_speed, int r_speed) { char cmd[] = {3, I2C_WCMD_MOTOR, 0, 0}; int adrs; cmd[2] = l_speed; cmd[3] = r_speed; if (0 < id){ adrs = SUB_ADRS + (id - 1) * 2; } else { adrs = 0; } i2c.write(adrs, cmd, 4); } void eDES2WD_set_led (int id, int led, int sw) { char cmd[] = {3, I2C_WCMD_LED, 0, 0}; int adrs; cmd[2] = led; cmd[3] = sw; if (0 < id){ adrs = SUB_ADRS + (id - 1) * 2; } else { adrs = 0; } i2c.write(adrs, cmd, 4); } unsigned int eDES2WD_get_sensor(int id, int cn, unsigned int *dat) { char cmd[] = {2, I2C_RCMD_SENSOR, 0}; char recv[MAX_CN]; unsigned int d; int adrs; int nmax, n; if (id < 1 || 9 < id) return(0); adrs = SUB_ADRS + (id - 1) * 2; nmax = cn < MAX_CN ? 2 : MAX_CN * 2 + 2; cmd[2] = cn; i2c.write(adrs, cmd, 3); i2c.read(adrs | 1, recv, nmax); if (nmax == 2){ d = recv[0] | (recv[1] << 8); return d; } else { for (n = 0; n < MAX_CN; n++){ d = recv[0] | (recv[1] << 8); dat[n] = d; } // gSUB_LED_G = recv[20]; // gSUB_LED_R = recv[21]; } return 0; } int eDES2WD_get_lat(int id, int cno) { char cmd[] = {2, I2C_RCMD_LAT, 0}; char recv[MAX_CN]; int adrs; if (id < 1 || 9 < id) return(false); if (MAX_CN - 1 < cno) return(false); adrs = SUB_ADRS + (id - 1) * 2; cmd[2] = cno; i2c.write(adrs, cmd, 3); i2c.read(adrs | 1, recv, 1); return recv[0]; }