jim herd / SD21

Access SD21 21 channel servo controller

SD21.cpp@3:f8b3023564af, 2012-02-09 (annotated)

Committer:
jimherd
Date:
Thu Feb 09 23:41:43 2012 +0000
Revision:
3:f8b3023564af
Parent:
0:0f81e14bfae9 
Minor fix. Index issue.

Who changed what in which revision?

UserRevisionLine numberNew contents of line
jimherd 0:0f81e14bfae9 1 /*
jimherd 0:0f81e14bfae9 2 SD21 - 21 Channel Servo Driver Module Library
jimherd 0:0f81e14bfae9 3 Copyright (c) 2011 Jim Herd
jimherd 0:0f81e14bfae9 4
jimherd 0:0f81e14bfae9 5 This program is free software: you can redistribute it and/or modify
jimherd 0:0f81e14bfae9 6 it under the terms of the GNU General Public License as published by
jimherd 0:0f81e14bfae9 7 the Free Software Foundation, either version 3 of the License, or
jimherd 0:0f81e14bfae9 8 (at your option) any later version.
jimherd 0:0f81e14bfae9 9
jimherd 0:0f81e14bfae9 10 This program is distributed in the hope that it will be useful,
jimherd 0:0f81e14bfae9 11 but WITHOUT ANY WARRANTY; without even the implied warranty of
jimherd 0:0f81e14bfae9 12 MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
jimherd 0:0f81e14bfae9 13 GNU General Public License for more details.
jimherd 0:0f81e14bfae9 14
jimherd 0:0f81e14bfae9 15 You should have received a copy of the GNU General Public License
jimherd 0:0f81e14bfae9 16 along with this program. If not, see <http://www.gnu.org/licenses/>.
jimherd 0:0f81e14bfae9 17 */
jimherd 0:0f81e14bfae9 18
jimherd 0:0f81e14bfae9 19 #include "SD21.h"
jimherd 0:0f81e14bfae9 20 #include "mbed.h"
jimherd 0:0f81e14bfae9 21
jimherd 0:0f81e14bfae9 22 /*-----------------------------------------------------------------------------
jimherd 0:0f81e14bfae9 23 *
jimherd 0:0f81e14bfae9 24 */
jimherd 0:0f81e14bfae9 25 SD21::SD21(PinName sda, PinName scl) : _i2c(sda, scl) {
jimherd 0:0f81e14bfae9 26 reset(); // initialise chip to power-on condition
jimherd 0:0f81e14bfae9 27 };
jimherd 0:0f81e14bfae9 28
jimherd 0:0f81e14bfae9 29 void SD21::reset() {
jimherd 0:0f81e14bfae9 30 }
jimherd 0:0f81e14bfae9 31
jimherd 0:0f81e14bfae9 32 void SD21::set_servo(int servo_num, int angle, int speed) {
jimherd 0:0f81e14bfae9 33
jimherd 0:0f81e14bfae9 34 char buffer[4];
jimherd 0:0f81e14bfae9 35 //
jimherd 0:0f81e14bfae9 36 // calculate servo pulse width from angle
jimherd 0:0f81e14bfae9 37 //
jimherd 0:0f81e14bfae9 38 SD21_tmp_data.value32 = 1000 + ((angle * 1000)/90);
jimherd 0:0f81e14bfae9 39 //
jimherd 0:0f81e14bfae9 40 // load i2c buffer with necessary data
jimherd 0:0f81e14bfae9 41 //
jimherd 0:0f81e14bfae9 42 buffer[0] = (char)((servo_num - 1) * 3); // convert servo number to register address
jimherd 0:0f81e14bfae9 43 buffer[1] = (char)speed;
jimherd 0:0f81e14bfae9 44 buffer[2] = SD21_tmp_data.value8[0];
jimherd 0:0f81e14bfae9 45 buffer[3] = SD21_tmp_data.value8[1];
jimherd 0:0f81e14bfae9 46 //
jimherd 0:0f81e14bfae9 47 // send buffer to SD21 device
jimherd 0:0f81e14bfae9 48 //
jimherd 0:0f81e14bfae9 49 _i2c.write(SD21_I2C_ADDRESS, buffer, 4);
jimherd 0:0f81e14bfae9 50 //
jimherd 0:0f81e14bfae9 51 // log these servo values
jimherd 0:0f81e14bfae9 52 //
jimherd 3:f8b3023564af 53 servo_state[(servo_num - 1)].angle = angle;
jimherd 3:f8b3023564af 54 servo_state[(servo_num - 1)].speed = speed;
jimherd 0:0f81e14bfae9 55 }
jimherd 0:0f81e14bfae9 56