Diff: eDES2WD.cpp

Revision:

1:3c8a6bd50779

Parent:

0:141734449aeb

--- a/eDES2WD.cpp	Thu Feb 23 01:33:12 2012 +0000
+++ b/eDES2WD.cpp	Thu Feb 23 05:05:23 2012 +0000
@@ -6,7 +6,7 @@
 void eDES2WD_reset (int id) {
   char cmd[] = {1, I2C_WCMD_RESET};
   int adrs;
-	
+    
   if (0 < id){
     adrs = SUB_ADRS + (id - 1) * 2;
   } else {
@@ -18,7 +18,7 @@
 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){
@@ -32,7 +32,7 @@
 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){
@@ -42,3 +42,46 @@
   }
   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];
+}