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