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 
32 using namespace upm;
33 
adafruitss(int bus,int i2c_address)34 adafruitss::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 
setPWMFreq(float freq)65 void 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 
update(void)114 int adafruitss::update(void)
115 {
116     return MRAA_SUCCESS;
117 }
118 
servo(uint8_t port,uint8_t servo_type,float degrees)119 void 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