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