OpenEL 3.0.0

openEL_utrx-17-1.c
Go to the documentation of this file.
1
12 /*
13
14 Copyright (c) 2017,2018 Japan Embedded Systems Technology Association(JASA)
15 All rights reserved.
16
17 Redistribution and use in source and binary forms, with or without modification, are permitted provided that the following conditions are met:
18
19 Redistributions of source code must retain the above copyright notice, this list of conditions and the following disclaimer.
20 Redistributions in binary form must reproduce the above copyright notice, this list of conditions and the following disclaimer in the documentation and/or other materials provided with the distribution.
21 Neither the name of the Association nor the names of its contributors may be used to endorse or promote products derived from this software without specific prior written permission.
22
23 THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
24 */
25
26 #ifdef __cplusplus
27 namespace el {
28 extern "C" {
29 #endif /* __cplusplus */
30 /*
31 #define OPENEL_SW_SURFACE_FRIEND 0
32 */
33 #include "openEL.h"
34 #include "openEL_utrx-17-1.h"
35 #include <stdio.h>
36 #include <stdint.h>
37 #include <unistd.h>
38 #include <math.h>
39 #include <linux/i2c-dev.h>
40 #include <fcntl.h>
41 #include <sys/ioctl.h>
42
43 #define PCA9685_SUBADR1 0x2
44 #define PCA9685_SUBADR2 0x3
45 #define PCA9685_SUBADR3 0x4
46
47 #define PCA9685_MODE1 0x0
48 #define PCA9685_PRESCALE 0xFE
49
50 #define LED0_ON_L 0x6
51 #define LED0_ON_H 0x7
52 #define LED0_OFF_L 0x8
53 #define LED0_OFF_H 0x9
54
55 #define ALLLED_ON_L 0xFA
56 #define ALLLED_ON_H 0xFB
57 #define ALLLED_OFF_L 0xFC
58 #define ALLLED_OFF_H 0xFD
59
60 #define PWM_FREQUENCY 60 //60Hz 16.7ms
61 #define PWM_PULSE_WIDTH_MAX 12000 //12ms
62
63 void PCA9685_init(float freq);
64 uint8_t PCA9685_read(uint8_t adr);
65 void PCA9685_write(uint8_t adr, uint8_t dat);
66 void PCA9685_pwmWrite(uint8_t ch, double pulseWidth_usec);
67 void PCA9685_setPWM(uint8_t ch, uint16_t onTime, uint16_t offTime);
68
69 static int i2c;
70 int once = 1;
71
72 enum ReturnCode Init_0000000200000001(){
73 printf("Init_0000000200000001\n");
74 if (once) {
75 char i2cFileName[] = "/dev/i2c-1";
76 int driverAddress = 0x40;
77 int i;
78 double dfVal;
79
80 if((i2c = open(i2cFileName, O_RDWR)) < 0){
81 printf("I2C open err\n");
82 return -1;
83 }
84
85 if(ioctl(i2c, I2C_SLAVE, driverAddress) < 0){
86 printf("ioctl err\n");
87 return -1;
88 }
89
90 PCA9685_init(PWM_FREQUENCY);
91
92 once = 0;
93 }
94
95 return HAL_OK;
96 }
97
98 enum ReturnCode ReInit_0000000200000001(){
99 printf("ReInit_0000000200000001\n");
100 return HAL_OK;
101 }
102
103 enum ReturnCode Finalize_0000000200000001(){
104 printf("Finalize_0000000200000001\n");
105 if (once == 0) {
106 close(i2c);
107 once = 1;
108 }
109 return HAL_OK;
110 }
111
112 enum ReturnCode AddObserver_0000000200000001(){
113 return HAL_OK;
114 };
115
116 enum ReturnCode RemoveObserver_0000000200000001(){
117 return HAL_OK;
118 };
119
120 enum ReturnCode GetProperty_0000000200000001(){
121 return HAL_OK;
122 };
123
124 enum ReturnCode GetTime_0000000200000001(){
125 return HAL_OK;
126 };
127
128 enum ReturnCode SetPosition_0000000200000001(HALCOMPONENT_T **halComponent, halfloat position){
129 printf("SetPosition_0000000200000001, position=%f\n", position);
130 (*halComponent)->positionControlActuator.actualPosition = position;
131 int16_t _minAngle = 0;
132 int16_t _maxAngle = 180;
133 int16_t _minServoPL = 143;
134 int16_t _maxServoPL = 471;
135 uint16_t pulseLen = (position-_minAngle)*(_maxServoPL-_minServoPL)/(_maxAngle-_minAngle) + _minServoPL;
136 PCA9685_setPWM(Device_ID - 1, 0, pulseLen);
137 return HAL_OK;
138 }
139
140 enum ReturnCode GetPosition_0000000200000001(HALCOMPONENT_T **halComponent, halfloat **position){
141 printf("GetPosition_0000000200000001\n");
142 *position = &(*halComponent)->positionControlActuator.actualPosition;
143 return HAL_OK;
144 }
145
146 enum ReturnCode Dummy(){
147 return HAL_OK;
148 };
149
150 /* Constant Table (Global scope) */
151 /*---------------------------------------------------------------------------*/
152 static const ELMOTOR_FNC_TBL_T motorFncTbl_0000000200000001 = {
153 /* 0x00 */ Init_0000000200000001,
154 /* 0x01 */ ReInit_0000000200000001,
155 /* 0x02 */ Finalize_0000000200000001,
156 /* 0x03 */ AddObserver_0000000200000001,
157 /* 0x04 */ RemoveObserver_0000000200000001,
158 /* 0x05 */ GetProperty_0000000200000001,
159 /* 0x06 */ GetTime_0000000200000001,
160 /* 0x07 */ Dummy,
162 /* 0x08 */ SetPosition_0000000200000001,
163 /* 0x09 */ GetPosition_0000000200000001,
164 /* 0x0A */ Dummy,
165 /* 0x0B */ Dummy,
166 /* 0x0C */ Dummy,
167 /* 0x0D */ Dummy,
168 /* 0x0E */ Dummy,
169 /* 0x0F */ Dummy,
171 /* 0x10 */ Dummy,
172 /* 0x11 */ Dummy,
173 /* 0x12 */ Dummy,
174 /* 0x13 */ Dummy,
175 /* 0x14 */ Dummy,
176 /* 0x15 */ Dummy,
177 /* 0x16 */ Dummy,
178 /* 0x17 */ Dummy,
180 /* 0x18 */ Dummy,
181 /* 0x19 */ Dummy,
182 /* 0x1A */ Dummy,
183 /* 0x1B */ Dummy,
184 /* 0x1C */ Dummy,
185 /* 0x1D */ Dummy,
186 /* 0x1E */ Dummy,
187 /* 0x1F */ Dummy,
188 };
189
190 const EL_CMN_FNC_TBL_T component_0000000200000001 = {(void *)(&motorFncTbl_0000000200000001)};
191
192 void PCA9685_init(float freq)
193 {
194 float prescaleval = 25000000;
195
196 PCA9685_write(PCA9685_MODE1, 0x0);
197 usleep(100000);//100ms
198 uint8_t prescale = 101;
199 uint8_t oldmode = PCA9685_read(PCA9685_MODE1);
200 uint8_t newmode = (oldmode&0x7F) | 0x10;
201 PCA9685_write(PCA9685_MODE1, newmode);
202 PCA9685_write(PCA9685_PRESCALE, prescale);
203 PCA9685_write(PCA9685_MODE1, oldmode);
204 sleep(5);
205 PCA9685_write(PCA9685_MODE1, oldmode | 0xa1);
206 }
207
208 void PCA9685_pwmWrite(uint8_t ch, double pulseWidth_usec)
209 {
210 double pulselength;
211 double pulseWidth;
212
213 pulselength = 1000000 / PWM_FREQUENCY;
214 pulselength /= 4096;
215 pulseWidth = pulseWidth_usec / pulselength;
216
217 PCA9685_setPWM(ch, 0, pulseWidth);
218 }
219
220 void PCA9685_setPWM(uint8_t ch, uint16_t onTime, uint16_t offTime)
221 {
222 uint8_t sendData[5];
223
224 sendData[0] = LED0_ON_L + 4 * ch;
225 sendData[1] = (uint8_t)(0x00ff & onTime);
226 sendData[2] = (uint8_t)((0xff00 & onTime) >> 8);
227 sendData[3] = (uint8_t)(0x00ff & offTime);
228 sendData[4] = (uint8_t)((0xff00 & offTime) >> 8);
229
230 if(write(i2c, sendData, 5) != 5){
231 printf("PCA9685_setPWM() err\n");
232 }
233 }
234
235 uint8_t PCA9685_read(uint8_t adr)
236 {
237 uint8_t sendData;
238 uint8_t readData;
239
240 sendData = adr;
241 if(write(i2c, &sendData, 1) != 1){
242 printf("PCA9685_read() err1\n");
243 }
244 else{
245 if(read(i2c, &readData, 1) != 1){
246 printf("PCA9685_read() err2\n");
247 }
248 }
249
250 return readData;
251 }
252
253 void PCA9685_write(uint8_t adr, uint8_t dat)
254 {
255 uint8_t buf[2];
256
257 buf[0] = adr;
258 buf[1] = dat;
259 if(write(i2c, buf, 2) != 2){
260 printf("PCA9685_write() err\n");
261 }
262 }
263
264 #ifdef __cplusplus
265 } /* extern "C" */
266 } /* namespace el */
267 #endif /* __cplusplus */
ReInit_0000000200000001
enum ReturnCode ReInit_0000000200000001()
Definition: openEL_utrx-17-1.c:98
Dummy
enum ReturnCode Dummy()
Definition: openEL_utrx-17-1.c:146
AddObserver_0000000200000001
enum ReturnCode AddObserver_0000000200000001()
Definition: openEL_utrx-17-1.c:112
PCA9685_read
uint8_t PCA9685_read(uint8_t adr)
Definition: openEL_utrx-17-1.c:235
PCA9685_MODE1
#define PCA9685_MODE1
Definition: openEL_utrx-17-1.c:47
openEL.h
OpenEL Common Header File 3.0.0.
PCA9685_PRESCALE
#define PCA9685_PRESCALE
Definition: openEL_utrx-17-1.c:48
GetProperty_0000000200000001
enum ReturnCode GetProperty_0000000200000001()
Definition: openEL_utrx-17-1.c:120
GetTime_0000000200000001
enum ReturnCode GetTime_0000000200000001()
Definition: openEL_utrx-17-1.c:124
elCommonFncTbl
Definition: openEL.h:214
ReturnCode
ReturnCode
Definition: openEL.h:44
elMotorFncTblSt
Definition: openEL_motor.h:112
SetPosition_0000000200000001
enum ReturnCode SetPosition_0000000200000001(HALCOMPONENT_T **halComponent, halfloat position)
Definition: openEL_utrx-17-1.c:128
halfloat
float32_t halfloat
Definition: openEL.h:53
PCA9685_pwmWrite
void PCA9685_pwmWrite(uint8_t ch, double pulseWidth_usec)
Definition: openEL_utrx-17-1.c:208
PCA9685_init
void PCA9685_init(float freq)
Definition: openEL_utrx-17-1.c:192
PCA9685_setPWM
void PCA9685_setPWM(uint8_t ch, uint16_t onTime, uint16_t offTime)
Definition: openEL_utrx-17-1.c:220
PWM_FREQUENCY
#define PWM_FREQUENCY
Definition: openEL_utrx-17-1.c:60
HalComponent
Definition: openEL.h:162
Init_0000000200000001
enum ReturnCode Init_0000000200000001()
Definition: openEL_utrx-17-1.c:72
once
int once
Definition: openEL_utrx-17-1.c:70
LED0_ON_L
#define LED0_ON_L
Definition: openEL_utrx-17-1.c:50
GetPosition_0000000200000001
enum ReturnCode GetPosition_0000000200000001(HALCOMPONENT_T **halComponent, halfloat **position)
Definition: openEL_utrx-17-1.c:140
PCA9685_write
void PCA9685_write(uint8_t adr, uint8_t dat)
Definition: openEL_utrx-17-1.c:253
component_0000000200000001
const EL_CMN_FNC_TBL_T component_0000000200000001
Definition: openEL_utrx-17-1.c:190
HAL_OK
Definition: openEL.h:45
openEL_utrx-17-1.h
OpenEL UTRX-17-1(PCA9685)(0000000020000001) header file 3.0.0.
RemoveObserver_0000000200000001
enum ReturnCode RemoveObserver_0000000200000001()
Definition: openEL_utrx-17-1.c:116
Finalize_0000000200000001
enum ReturnCode Finalize_0000000200000001()
Definition: openEL_utrx-17-1.c:103