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 
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 
91 
92  once = 0;
93  }
94 
95  return HAL_OK;
96 }
97 
99  printf("ReInit_0000000200000001\n");
100  return HAL_OK;
101 }
102 
104  printf("Finalize_0000000200000001\n");
105  if (once == 0) {
106  close(i2c);
107  once = 1;
108  }
109  return HAL_OK;
110 }
111 
113  return HAL_OK;
114 };
115 
117  return HAL_OK;
118 };
119 
121  return HAL_OK;
122 };
123 
125  return HAL_OK;
126 };
127 
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 
141  printf("GetPosition_0000000200000001\n");
142  *position = &(*halComponent)->positionControlActuator.actualPosition;
143  return HAL_OK;
144 }
145 
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,
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 
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 */
enum ReturnCode ReInit_0000000200000001()
enum ReturnCode Dummy()
enum ReturnCode AddObserver_0000000200000001()
uint8_t PCA9685_read(uint8_t adr)
#define PCA9685_MODE1
OpenEL Common Header File 3.0.0.
#define PCA9685_PRESCALE
enum ReturnCode GetProperty_0000000200000001()
enum ReturnCode GetTime_0000000200000001()
ReturnCode
Definition: openEL.h:44
enum ReturnCode SetPosition_0000000200000001(HALCOMPONENT_T **halComponent, halfloat position)
float32_t halfloat
Definition: openEL.h:53
void PCA9685_pwmWrite(uint8_t ch, double pulseWidth_usec)
void PCA9685_init(float freq)
void PCA9685_setPWM(uint8_t ch, uint16_t onTime, uint16_t offTime)
#define PWM_FREQUENCY
enum ReturnCode Init_0000000200000001()
int once
#define LED0_ON_L
enum ReturnCode GetPosition_0000000200000001(HALCOMPONENT_T **halComponent, halfloat **position)
void PCA9685_write(uint8_t adr, uint8_t dat)
const EL_CMN_FNC_TBL_T component_0000000200000001
Definition: openEL.h:45
OpenEL UTRX-17-1(PCA9685)(0000000020000001) header file 3.0.0.
enum ReturnCode RemoveObserver_0000000200000001()
enum ReturnCode Finalize_0000000200000001()