Diff: SD21.cpp

Revision:

0:0f81e14bfae9

Child:

3:f8b3023564af

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/SD21.cpp	Sat Mar 19 23:51:57 2011 +0000
@@ -0,0 +1,56 @@
+/*
+    SD21 - 21 Channel Servo Driver Module Library
+    Copyright (c) 2011 Jim Herd
+    
+    This program is free software: you can redistribute it and/or modify
+    it under the terms of the GNU General Public License as published by
+    the Free Software Foundation, either version 3 of the License, or
+    (at your option) any later version.
+
+    This program is distributed in the hope that it will be useful,
+    but WITHOUT ANY WARRANTY; without even the implied warranty of
+    MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+    GNU General Public License for more details.
+
+    You should have received a copy of the GNU General Public License
+    along with this program.  If not, see <http://www.gnu.org/licenses/>.
+*/    
+
+#include "SD21.h"
+#include "mbed.h"
+
+/*-----------------------------------------------------------------------------
+ *
+ */
+SD21::SD21(PinName sda, PinName scl)  : _i2c(sda, scl) {
+    reset();                              // initialise chip to power-on condition
+};
+
+void SD21::reset() {
+}
+
+void SD21::set_servo(int servo_num, int angle, int speed) {
+
+char buffer[4];
+//
+// calculate servo pulse width from angle
+//
+    SD21_tmp_data.value32 = 1000 + ((angle * 1000)/90);
+//
+// load i2c buffer with necessary data
+//
+    buffer[0] = (char)((servo_num - 1) * 3);   // convert servo number to register address
+    buffer[1] = (char)speed;
+    buffer[2] = SD21_tmp_data.value8[0];
+    buffer[3] = SD21_tmp_data.value8[1];
+//
+// send buffer to SD21 device
+//    
+    _i2c.write(SD21_I2C_ADDRESS, buffer, 4);
+//
+// log these servo values
+//    
+    servo_state[servo_num].angle = angle;
+    servo_state[servo_num].speed = speed;
+}
+