SD21.cpp

00001 /*
00002     SD21 - 21 Channel Servo Driver Module Library
00003     Copyright (c) 2011 Jim Herd
00004     
00005     This program is free software: you can redistribute it and/or modify
00006     it under the terms of the GNU General Public License as published by
00007     the Free Software Foundation, either version 3 of the License, or
00008     (at your option) any later version.
00009 
00010     This program is distributed in the hope that it will be useful,
00011     but WITHOUT ANY WARRANTY; without even the implied warranty of
00012     MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
00013     GNU General Public License for more details.
00014 
00015     You should have received a copy of the GNU General Public License
00016     along with this program.  If not, see <http://www.gnu.org/licenses/>.
00017 */    
00018 
00019 #include "SD21.h"
00020 #include "mbed.h"
00021 
00022 /*-----------------------------------------------------------------------------
00023  *
00024  */
00025 SD21::SD21(PinName sda, PinName scl)  : _i2c(sda, scl) {
00026     reset();                              // initialise chip to power-on condition
00027 };
00028 
00029 void SD21::reset() {
00030 }
00031 
00032 void SD21::set_servo(int servo_num, int angle, int speed) {
00033 
00034 char buffer[4];
00035 //
00036 // calculate servo pulse width from angle
00037 //
00038     SD21_tmp_data.value32 = 1000 + ((angle * 1000)/90);
00039 //
00040 // load i2c buffer with necessary data
00041 //
00042     buffer[0] = (char)((servo_num - 1) * 3);   // convert servo number to register address
00043     buffer[1] = (char)speed;
00044     buffer[2] = SD21_tmp_data.value8[0];
00045     buffer[3] = SD21_tmp_data.value8[1];
00046 //
00047 // send buffer to SD21 device
00048 //    
00049     _i2c.write(SD21_I2C_ADDRESS, buffer, 4);
00050 //
00051 // log these servo values
00052 //    
00053     servo_state[(servo_num - 1)].angle = angle;
00054     servo_state[(servo_num - 1)].speed = speed;
00055 }
00056