Diff: eDES2WD.cpp

Revision:

1:2a38c9027bd1

Parent:

0:04367044b55e

Child:

2:ef1c45486022

--- a/eDES2WD.cpp	Fri Jun 08 16:10:50 2012 +0000
+++ b/eDES2WD.cpp	Sat Dec 15 15:35:30 2012 +0000
@@ -1,97 +1,98 @@
-/**
- * DAISEN eDES 2WD driver for mbed
- * Copyright (c) 2012 Suga
- * Released under the MIT License: http://mbed.org/license/mit
- */
-
-/** @file
- * @brief DAISEN eDES 2WD driver for mbed
- */
-
-#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];
-}
+/**
+ * DAISEN eDES 2WD driver for mbed
+ * Copyright (c) 2012 Suga
+ * Released under the MIT License: http://mbed.org/license/mit
+ */
+
+/** @file
+ * @brief DAISEN eDES 2WD driver for mbed
+ */
+
+#include "mbed.h"
+#include "eDES2WD.h"
+
+eDES2WD::eDES2WD (I2C &i2c) : _i2c(i2c) {
+}
+
+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, eDES2WD_LED 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];
+}