Access SD21 21 channel servo controller
SD21.cpp@0:0f81e14bfae9, 2011-03-19 (annotated)
- Committer:
- jimherd
- Date:
- Sat Mar 19 23:51:57 2011 +0000
- Revision:
- 0:0f81e14bfae9
- Child:
- 3:f8b3023564af
Who changed what in which revision?
User | Revision | Line number | New 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 | 0:0f81e14bfae9 | 53 | servo_state[servo_num].angle = angle; |
jimherd | 0:0f81e14bfae9 | 54 | servo_state[servo_num].speed = speed; |
jimherd | 0:0f81e14bfae9 | 55 | } |
jimherd | 0:0f81e14bfae9 | 56 |