Diff: eDES2WD.cpp

Revision:

0:141734449aeb

Child:

1:3c8a6bd50779

diff -r 000000000000 -r 141734449aeb eDES2WD.cpp
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/eDES2WD.cpp	Thu Feb 23 01:33:12 2012 +0000
@@ -0,0 +1,44 @@
+#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);
+}