1/* 2 * Author: Stan Gifford <stan@gifford.id.au> 3 * Copyright (c) 2015 Intel Corporation. 4 * 5 * Permission is hereby granted, free of charge, to any person obtaining 6 * a copy of this software and associated documentation files (the 7 * "Software"), to deal in the Software without restriction, including 8 * without limitation the rights to use, copy, modify, merge, publish, 9 * distribute, sublicense, and/or sell copies of the Software, and to 10 * permit persons to whom the Software is furnished to do so, subject to 11 * the following conditions: 12 * 13 * The above copyright notice and this permission notice shall be 14 * included in all copies or substantial portions of the Software. 15 * 16 * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, 17 * EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF 18 * MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND 19 * NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE 20 * LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION 21 * OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION 22 * WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. 23 */ 24 25#include <string> 26#include <stdexcept> 27 28#include "adafruitss.h" 29#include <unistd.h> 30#include <math.h> 31 32using namespace upm; 33 34adafruitss::adafruitss(int bus,int i2c_address) 35{ 36 if ( !(m_i2c = mraa_i2c_init(bus)) ) 37 { 38 throw std::invalid_argument(std::string(__FUNCTION__) + 39 ": mraa_i2c_init() failed"); 40 return; 41 } 42 43 pca9685_addr = i2c_address; 44 if (mraa_i2c_address(m_i2c, pca9685_addr) != MRAA_SUCCESS) 45 { 46 throw std::invalid_argument(std::string(__FUNCTION__) + 47 ": mraa_i2c_address() failed"); 48 return; 49 } 50 51 m_rx_tx_buf[0]=PCA9685_MODE1; 52 m_rx_tx_buf[1]=0; 53 if (mraa_i2c_write(m_i2c,m_rx_tx_buf,2) != MRAA_SUCCESS) 54 { 55 throw std::invalid_argument(std::string(__FUNCTION__) + 56 ": mraa_i2c_write() failed"); 57 return; 58 } 59 60 adafruitss::setPWMFreq(60); 61 62 adafruitss::update(); 63} 64 65void adafruitss::setPWMFreq(float freq) { 66 float afreq= freq * 0.899683334F; // Correct for overshoot in the frequency setting (see issue #11). (Tested at 60hz with Logic 4 for 50hz and 60hz) 67 float prescaleval = 25000000; 68 prescaleval /= 4096; 69 prescaleval /= afreq; 70 prescaleval -= 1; 71 float pwm_frequency = freq; // Use actual requested frequency gives the correct pulse width 72 73 _duration_1ms = ((4096*pwm_frequency)/1000); // This is 1ms duration 74 75 uint8_t prescale = roundf(prescaleval); 76 77 78 79 mraa_i2c_address(m_i2c, pca9685_addr); 80 mraa_i2c_read_byte_data(m_i2c,PCA9685_MODE1); 81 82 83 m_rx_tx_buf[0]=PCA9685_MODE1; 84 m_rx_tx_buf[1]=0x10; // sleep 85 mraa_i2c_address(m_i2c, pca9685_addr); 86 mraa_i2c_write(m_i2c,m_rx_tx_buf,2); 87 88 89 90 m_rx_tx_buf[0]=PCA9685_PRESCALE; 91 m_rx_tx_buf[1]=prescale; 92 mraa_i2c_address(m_i2c, pca9685_addr); 93 mraa_i2c_write(m_i2c,m_rx_tx_buf,2); 94 95 96 97 98 m_rx_tx_buf[0]=PCA9685_MODE1; 99 m_rx_tx_buf[1]=0x00; 100 mraa_i2c_address(m_i2c, pca9685_addr); 101 mraa_i2c_write(m_i2c,m_rx_tx_buf,2); 102 103 // mraa_i2c_write_byte_data(m_i2c,0x00,PCA9685_MODE1); 104 105 usleep(5000); 106 107 108 m_rx_tx_buf[0]=PCA9685_MODE1; 109 m_rx_tx_buf[1]=0xa1; 110 mraa_i2c_address(m_i2c, pca9685_addr); 111 mraa_i2c_write(m_i2c,m_rx_tx_buf,2); 112} 113 114int adafruitss::update(void) 115{ 116 return MRAA_SUCCESS; 117} 118 119void adafruitss::servo(uint8_t port, uint8_t servo_type, float degrees) { 120 // Set Servo values 121 // Degrees is from 0 to 180 122 // servo_type: 0 = standard 1ms to 2ms 123 // 1 = extended 0.6ms to 2.4ms 124 // 2 = extended 0.8ms to 2.2ms 125 126 float duration; 127 128 if(degrees>180) degrees=180; // Ensure within bounds 129 if (degrees<0) degrees=0; 130 switch (servo_type) { 131 default: 132 case 0: // Standard Servo 1ms to 2ms 133 duration = _duration_1ms + ((_duration_1ms*degrees)/180); 134 break; 135 136 case 1: // Extended Servo 0.6ms to 2.4ms, i.e. 1.8ms from 0 to 180 137 //duration = (_duration_1ms*0.6) + ((_duration_1ms*1.8*degrees)/180); simplified to.. 138 duration = (_duration_1ms*0.6) + ((_duration_1ms*degrees)/100); 139 break; 140 141 case 2: // Extended Servo 0.8ms to 2.2ms, i.e. 1.4ms from 0 to 180 142 //duration = (_duration_1ms*0.8) + ((_duration_1ms*1.4*degrees)/180); simplified to.. 143 duration = (_duration_1ms*0.8) + ((_duration_1ms*degrees)/128); 144 break; 145 case 3: // Extended Servo 0.9ms to 2.1ms, - GWS Mini STD BB servo 146 //duration = (_duration_1ms*0.9) + ((_duration_1ms*1.4*degrees)/180); simplified to.. 147 duration = (_duration_1ms*0.9) + ((_duration_1ms*degrees)/120); 148 break; 149 } 150 151 uint16_t d= roundf(duration); 152 mraa_i2c_address(m_i2c, pca9685_addr); 153 m_rx_tx_buf[0]=LED0_REG+4*port; 154 m_rx_tx_buf[1]=0; 155 m_rx_tx_buf[2]=0; 156 m_rx_tx_buf[3]=d; 157 m_rx_tx_buf[4]=d>>8; 158 159 mraa_i2c_write(m_i2c,m_rx_tx_buf,5); 160 } 161 162