close
8-bit A/D Converter 原理
今天要說明的是 馬達制動 (Mortor Brake) ; Mortor Brake, brake 這個字不是我拼錯了, 它中文就是 "制動" 的意思. 在此我必須先說明 8-bit A/D Converter. 為什麼馬達制動會與 AD Converter 有關呢? 原因是馬達 正轉/反轉 轉到我們所希望控制的位置停下來, 這停下來的位置必須有一個參考, 否則馬達怎麼知道轉向以及何時停止轉動? 這個參考在本文中使用 類比輸入 (Analog Input) 來做參考. 馬達在不同位置時 Analog Input 會不同, 這裡我們假設示 0.372~2.77 V (伏特) .
如本文最上方的示意圖所示: 8-bit AD Converter 原理是 Digital OutPut (圖示 Digital OutPut 4條線, 8-bit 應為 8條線) 至 D/A 線路, 這 D/A 線路可能會需要 OP放大 (運算放大器) , 再與 類比輸入 (Analog Input) 來比較 (COMPARATOR) . 8-bit 定址至 255, 著 Digital OutPut 由 0 往 255 遞增, D/A 輸出之 Analog Output 也會遞增. 起出 Analog Input 與 Analog Output 比較 LOGIC 為 0, 遞增至臨界值, 這比較器 (COMPARATOR) 回突然 LOGIC 為 1; LOGIC 由 0 變為 1 的時機, Digital OutPut 所輸出的 binary 數值, 就是 8-bit A/D 值.
PCA9555 I2C to GPIO (IO Extender)
這裡我所示範的 IO 是使用 PCA9555 IO Extender, 下方圖示是它的方塊圖:
PCA9555 Host 介面是 I2C BUS, 多個 I2C DEVICE 可以並接, 但每一 DEVICE 的 SLAVE ADDRESS (BUS ADDRESS) 必須不同;
因此 PCA_9555_SLAVE_1 / PCA_9555_SLAVE_2 我們設為 0x20 與 0x21 .
#define PCA_9555_SLAVE_1 0X20 #define PCA_9555_SLAVE_2 0X21
上圖 A2, A1, A0 即是 I2C SLAVE ADDRESS 最低 3個位元. PCA_9555_SLAVE_1=0x20 最低 3個位元是 b000; 也就是 A2=0, A1=0, A0=0 ; PCA_9555_SLAVE_2=0x21 最低 3個位元是 b001; 也就是 A2=0, A1=0, A0=1 .
#define PCA_9555_SLAVE_1 0X20 #define PCA_9555_SLAVE_2 0X21
PCA9555 Registers 是固定的: 詳見 datasheet .
#define PCA_9555_INPUT_0 0 #define PCA_9555_INPUT_1 1 #define PCA_9555_OUTPUT_0 2 #define PCA_9555_OUTPUT_1 3 #define PCA_9555_POLARITY_0 4 #define PCA_9555_POLARITY_1 5 #define PCA_9555_CONFIG_0 6 #define PCA_9555_CONFIG_1 7
I2C_WRITE_DATA / I2C_READ_DATA
這裡我們需要 I2C READ / WRITE 函式.
int i2c_write_data(int fd_i2c, int bus_adr, int sub_adr, const unsigned char *data, int length) {
int res;
char buf[256];
int i;
if (0 == data || 0 >= fd_i2c) return -1;
res = ioctl(fd_i2c, I2C_SLAVE_FORCE, bus_adr);
if(res < 0) return -1;
buf[0] = sub_adr;
for(i = 1; i <= length; i++) buf[i] = data[i -1];
if(write(fd_i2c, buf, length + 1) != (length + 1)) return -1;
return length;
}
int i2c_read_data(int fd_i2c, int bus_adr, int sub_adr, unsigned char *data, int length) {
int res;
if (0 == data || 0 >= fd_i2c) return -1;
res = ioctl(fd_i2c, I2C_SLAVE_FORCE, bus_adr);
if(res < 0) return -1;
if(write(fd_i2c, &sub_adr, 1) != 1) return -1;
return read(fd_i2c, data, length);
}
8-bit A/D Converter - ReadI2cAdc()
來看一下 ReadI2cAdc() 這個函式:
// 返回 0:失敗 1:成功 ; 參數傳回 value
static int ReadI2cAdc(int fd, unsigned char *value) {
int i;
int probe = 0;
unsigned char X = 0;
unsigned char Y = 0;
static unsigned char O = 0; // last X
if (-1 == fd) return 0;
for (i = 0; i < 256; i++) {
if (-1 == i2c_write_data(fd, PCA_9555_SLAVE_2, PCA_9555_OUTPUT_1, &X, 1)) {
printf("(%s %d) i2c_write_data FAIL \n", __FILE__, __LINE__);
return 0;
}
usleep(1000);
if (-1 == i2c_read_data(fd, PCA_9555_SLAVE_2, PCA_9555_INPUT_0, &Y, 1)) {
printf("(%s %d) i2c_read_data FAIL \n", __FILE__, __LINE__);
return 0;
}
usleep(1000);
// COMPARATOR
if ((Y & BIT7) == BIT7) {
probe = 1;
break;
}
O = X;
if (X >= 255) X = 0;
else X++;
}
if (0 == probe) return 0;
*value = O; // 傳回上一次的 DIGITAL OUTPUT
return 1;
}
Digital OutPut 接線至 PCA_9555_SLAVE_2 之 PORT 1 :
i2c_write_data(fd, PCA_9555_SLAVE_2, PCA_9555_OUTPUT_1, &X, 1);
COMPARATOR LOGIC 接線至 PCA_9555_SLAVE_2 PORT-0 裡面的 IO7 也就是 P0_7
i2c_read_data(fd, PCA_9555_SLAVE_2, PCA_9555_INPUT_0, &Y, 1); if ((Y & BIT7) == BIT7) LOGIC=1;
函式中:
for (i = 0; i < 256; i++) ...
DIGITAL OUTPUT 由 0 往 255 遞增. 我們要找的是 PCA_9555_SLAVE_2 P0_7 (COMPARATOR LOGIC) 由 0 轉變到 1 的時機; 當 COMPARATOR LOGIC 由 0 轉變到 1, 函式返回, 由參數 value 傳回上一次的 DIGITAL OUTPUT.
馬達正轉 / 反轉 / 停止
馬達的 IO 是 PCA_9555_SLAVE_1 之 P0_2 以及 P0_3 , 因此:
正轉:
// motor+ v &= ~BIT2; v |= BIT3;
反轉:
// motor- v &= ~BIT3; v |= BIT2;
停止:
// motor stop v |= BIT2; v |= BIT3;
IO Input / Output Settings
1. 馬達 IO 設為 OUTPUT:
// motor IO
res = i2c_read_data(i2c0, PCA_9555_SLAVE_1, PCA_9555_CONFIG_0, &v, 1);
if (-1 == res)
printf("(%s %d) i2c() FAIL, EXIT \n", __FILE__, __LINE__), close(i2c0), exit(1);
v &= ~BIT2; v &= ~BIT3;
res = i2c_write_data(i2c0, PCA_9555_SLAVE_1, PCA_9555_CONFIG_0, &v, 1);
if (-1 == res)
printf("(%s %d) i2c() FAIL, EXIT \n", __FILE__, __LINE__), close(i2c0), exit(1);
2. DIGITAL OUTPUT:
// X output
v = 0X00; res = i2c_write_data(i2c0, PCA_9555_SLAVE_2, PCA_9555_CONFIG_1, &v, 1);
if (-1 == res)
printf("(%s %d) i2c_write_data() FAIL, EXIT \n", __FILE__, __LINE__), close(i2c0), exit(1);
3. LOGIC INPUT:
// Y input
res = i2c_read_data(i2c0, PCA_9555_SLAVE_2, PCA_9555_CONFIG_0, &v, 1);
if (-1 == res)
printf("(%s %d) i2c() FAIL, EXIT \n", __FILE__, __LINE__), close(i2c0), exit(1);
v |= BIT7; res = i2c_write_data(i2c0, PCA_9555_SLAVE_2, PCA_9555_CONFIG_0, &v, 1);
if (-1 == res)
printf("(%s %d) i2c_write_data() FAIL, EXIT \n", __FILE__, __LINE__), close(i2c0), exit(1);
A/D Converter 伺服馬達制動 - mbrake.c
把上述綜合起來, 筆者寫了這個伺服馬達制動的程式 mbrake.c ; 讀者請別小看它, 我已經看過兩個軟體工程師掛點 (GG了) 在這 A/D Converter 伺服馬達制動程式; 在當時好像只有我做出來.
#include <stdio.h>
#include <unistd.h>
#include <string.h>
#include <sys/types.h>
#include <sys/stat.h>
#include <errno.h>
#include <sys/socket.h>
#include <netinet/in.h>
#include <netdb.h>
#include <stdarg.h>
#include <stdlib.h>
#include <time.h>
#include <fcntl.h>
#include <signal.h>
#include <termios.h>
#include <netinet/tcp.h>
#include <string.h>
#include <stdint.h>
#include <inttypes.h>
#include <asm/ioctl.h>
#include <asm/errno.h>
#include <linux/i2c.h>
#include <linux/i2c-dev.h>
//////////////////////////////////////////////////////////////////////
// define
//////////////////////////////////////////////////////////////////////
#define BIT0 (1<<0)
#define BIT1 (1<<1)
#define BIT2 (1<<2)
#define BIT3 (1<<3)
#define BIT4 (1<<4)
#define BIT5 (1<<5)
#define BIT6 (1<<6)
#define BIT7 (1<<7)
#define PCA_9555_SLAVE_0 0X24
#define PCA_9555_SLAVE_1 0X20
#define PCA_9555_SLAVE_2 0X21
#define PCA_9555_INPUT_0 0
#define PCA_9555_INPUT_1 1
#define PCA_9555_OUTPUT_0 2
#define PCA_9555_OUTPUT_1 3
#define PCA_9555_POLARITY_0 4
#define PCA_9555_POLARITY_1 5
#define PCA_9555_CONFIG_0 6
#define PCA_9555_CONFIG_1 7
#ifdef USE_SOC_ADC
#define GPIO_IOCTL_ID 'G'
#define GPIO_IOCTL_SET_VALUE _IOW(GPIO_IOCTL_ID, 0, int)
#define GPIO_IOCTL_GET_VALUE _IOR(GPIO_IOCTL_ID, 1, int)
#define GPIO_IOCTL_SET_INPUT _IOW(GPIO_IOCTL_ID, 2, int)
#define GPIO_IOCTL_SET_PROPERTY _IOW(GPIO_IOCTL_ID, 3, int)
#define GPIO_IOCTL_SET_INT _IOW(GPIO_IOCTL_ID, 4, int)
#define GPIO_FUNC_ORG 0
#define GPIO_FUNC_GPIO 1
#define GPIO_DIR_OUTPUT 0
#define GPIO_DIR_INPUT 1
#define GPIO_PULL_LOW 0
#define GPIO_PULL_HIGH 1
#define GPIO_PULL_FLOATING 2
#define GPIO_NO_PULL 3
#define GPIO_IRQ_DISABLE 0
#define GPIO_IRQ_ENABLE 1
#define GP_ADC_MAGIC 'G'
#define IOCTL_GP_ADC_START _IOW(GP_ADC_MAGIC,0x01,unsigned long)
#define IOCTL_GP_ADC_STOP _IO(GP_ADC_MAGIC,0x02)
#define MK_GPIO_INDEX(ch, func, gid, pin) \
(((ch) << 24) | ((func) << 16) | ((gid) << 8) | (pin))
typedef struct gpio_content_s {
unsigned int pin_index;
unsigned int value;
unsigned int debounce;
} gpio_content_t;
#endif
//////////////////////////////////////////////////////////////////////
//////////////////////////////////////////////////////////////////////
static int i2c_write_data(int fd_i2c, int bus_adr, int sub_adr, const unsigned char *data, int length) {
int res;
char buf[256];
int i;
if (0 == data || 0 >= fd_i2c) return -1;
res = ioctl(fd_i2c, I2C_SLAVE_FORCE, bus_adr);
if(res < 0) return -1;
buf[0] = sub_adr;
for(i = 1; i <= length; i++) buf[i] = data[i -1];
if(write(fd_i2c, buf, length + 1) != (length + 1)) return -1;
return length;
}
static int i2c_read_data(int fd_i2c, int bus_adr, int sub_adr, unsigned char *data, int length) {
int res;
if (0 == data || 0 >= fd_i2c) return -1;
res = ioctl(fd_i2c, I2C_SLAVE_FORCE, bus_adr);
if(res < 0) return -1;
if(write(fd_i2c, &sub_adr, 1) != 1) return -1;
return read(fd_i2c, data, length);
}
#ifndef USE_SOC_ADC
static int ReadI2cAdc(int fd, unsigned char *value) {
int i;
int probe = 0;
unsigned char X = 0;
unsigned char Y = 0;
static unsigned char O = 0; // last X
if (-1 == fd) return 0;
// DIGITAL OUTPUT 由 0 往 255 遞增
for (i = 0; i < 256; i++) {
if (-1 == i2c_write_data(fd, PCA_9555_SLAVE_2, PCA_9555_OUTPUT_1, &X, 1)) {
printf("(%s %d) i2c_write_data FAIL \n", __FILE__, __LINE__);
return 0;
}
usleep(1000);
if (-1 == i2c_read_data(fd, PCA_9555_SLAVE_2, PCA_9555_INPUT_0, &Y, 1)) {
printf("(%s %d) i2c_read_data FAIL \n", __FILE__, __LINE__);
return 0;
}
usleep(1000);
// COMPARATOR
if ((Y & BIT7) == BIT7) {
// COMPARATOR LOGIC=1
probe = 1;
break;
}
// COMPARATOR LOGIC=0
O = X;
if (X >= 255) X = 0;
else X++;
}
if (0 == probe) return 0;
*value = O;
return 1;
}
#endif
void PrintUsage(void) {
char file[128];
int i = 0;
memset(file, 0, sizeof(file));
strcpy(file, __FILE__);
for (; i < 128; i++) if ('.' == file[i]) file[i] = 0;
printf("Usage: %s -L [BRAKE-LEVEL] -T [TIMEOUT]\n", file);
printf("Example: %s -L 6 -T 6\n", file), exit(1);
}
int main(int argc, char *argv[]) {
int adc1 = -1;
int t;
time_t timeout = 6;
int i2c0 = -1;
int level = 6;
static unsigned int adc = 0;
time_t now = 0;
int D = 0;
int res;
int o;
int len = 0;
static unsigned char v;
int i;
int ok = 0;
setbuf(stdout, 0);
if (argc < 2) PrintUsage();
while ((o = getopt(argc, argv, "L:l:T:t:")) != -1) {
switch (o) {
default:
break;
case '?':
PrintUsage();
break;
case 't':
case 'T':
len = strlen(optarg);
for (i = 0; i < len; i++) {
if ('0' != optarg[i] && '1' != optarg[i] && '2' != optarg[i] && '3' != optarg[i] && '4' != optarg[i] && '5' != optarg[i] && '6' != optarg[i] && '7' != optarg[i] && '8' != optarg[i] && '9' != optarg[i]) {
PrintUsage();
}
}
t = atoi(optarg);
if (t <= 0) PrintUsage();
timeout = (time_t)t;
timeout &= 0XFF;
break;
case 'l':
case 'L':
len = strlen(optarg);
for (i = 0; i < len; i++) {
if ('0' != optarg[i] && '1' != optarg[i] && '2' != optarg[i] && '3' != optarg[i] && '4' != optarg[i] && '5' != optarg[i] && '6' != optarg[i] && '7' != optarg[i] && '8' != optarg[i] && '9' != optarg[i]) {
PrintUsage();
}
}
level = atoi(optarg);
if (level <= 0) PrintUsage();
level &= 0XFF;
break;
}
}
i2c0 = open("/dev/i2c-0", O_RDWR);
if (-1 == i2c0)
printf("(%s %d) OPEN(/dev/i2c-0) FAIL, EXIT \n", __FILE__, __LINE__), exit(1);
// motor IO output
res = i2c_read_data(i2c0, PCA_9555_SLAVE_1, PCA_9555_CONFIG_0, &v, 1);
if (-1 == res)
printf("(%s %d) i2c() FAIL, EXIT \n", __FILE__, __LINE__), close(i2c0), exit(1);
v &= ~BIT2; v &= ~BIT3;
res = i2c_write_data(i2c0, PCA_9555_SLAVE_1, PCA_9555_CONFIG_0, &v, 1);
if (-1 == res)
printf("(%s %d) i2c() FAIL, EXIT \n", __FILE__, __LINE__), close(i2c0), exit(1);
#ifdef USE_SOC_ADC
adc1 = open("/dev/adc", O_RDWR);
if (-1 == adc1)
printf("(%s %d) open('/dev/adc') Fail \n", __FILE__, __LINE__), exit(1);
#else
// DIGITAL OUTPUT
v = 0X00; res = i2c_write_data(i2c0, PCA_9555_SLAVE_2, PCA_9555_CONFIG_1, &v, 1);
if (-1 == res)
printf("(%s %d) i2c_write_data() FAIL, EXIT \n", __FILE__, __LINE__), close(i2c0), exit(1);
// LOGIC INPUT
res = i2c_read_data(i2c0, PCA_9555_SLAVE_2, PCA_9555_CONFIG_0, &v, 1);
if (-1 == res)
printf("(%s %d) i2c() FAIL, EXIT \n", __FILE__, __LINE__), close(i2c0), exit(1);
v |= BIT7; res = i2c_write_data(i2c0, PCA_9555_SLAVE_2, PCA_9555_CONFIG_0, &v, 1);
if (-1 == res)
printf("(%s %d) i2c_write_data() FAIL, EXIT \n", __FILE__, __LINE__), close(i2c0), exit(1);
#endif
time(&now);
timeout += now;
for(;;) {
int gpio;
#if 1
if (-1 == i2c_read_data(i2c0, PCA_9555_SLAVE_1, PCA_9555_INPUT_0, &v, 1)) {
printf("\n(%s %d) i2c_read_data() FAIL, EXIT \n", __FILE__, __LINE__);
goto BAITOUT;
}
// motor stop
v |= BIT2; v |= BIT3;
if (-1 == i2c_write_data(i2c0, PCA_9555_SLAVE_1, PCA_9555_OUTPUT_0, &v, 1)) {
printf("\n(%s %d) i2c_write_data() FAIL, EXIT \n", __FILE__, __LINE__);
goto BAITOUT;
}
#endif
time(&now);
if (now > timeout) {
printf("\n(%s %d) TIMEOUT, EXIT \n", __FILE__, __LINE__);
goto BAITOUT;
}
#ifdef USE_SOC_ADC
gpio = open("/dev/gpio", O_RDWR);
if(-1 == gpio) {
printf("GPIO OPEN ERROR\n"); break;
} else {
gpio_content_t ctx;
ctx.pin_index = MK_GPIO_INDEX(0, 0, 9, 20);
ctx.value = 0;
ctx.value = GPIO_PULL_FLOATING; ctx.debounce = 0;
ioctl(gpio, GPIO_IOCTL_SET_INPUT, &ctx);
close(gpio);
if (-1 == ioctl(adc1, IOCTL_GP_ADC_START, 1)) { printf("ADC_START ERROR\n"); break; }
if (-1 == read(adc1, &adc, sizeof(int))) { printf("ADC_READ ERROR\n"); break; }
if (-1 == ioctl(adc1, IOCTL_GP_ADC_STOP, 1)) { printf("ADC_STOP ERROR\n"); break; }
// 10-bit ; hence we need shift right 2 bits
adc >>= 2;
}
#else
if(0 != ReadI2cAdc(i2c0, &adc)) printf("L=%d A=%d\n", level, adc);
else { printf("ADC ERROR\n"); break; }
#endif
D = (int)level - (int)adc;
if (D <= 1 && D >= -1) {
ok = 1;
break;
} else {
if (D >= 0) {
// motor+
v &= ~BIT2; v |= BIT3;
} else {
// motor-
v &= ~BIT3; v |= BIT2;
}
if (-1 == i2c_write_data(i2c0, PCA_9555_SLAVE_1, PCA_9555_OUTPUT_0, &v, 1)) {
printf("\n(%s %d) i2c_write_data() FAIL, EXIT \n", __FILE__, __LINE__);
goto BAITOUT;
}
usleep(20 * 1000);
}
}
if (ok) printf("\nMORTOR BRAKE, LEVEL=%d ADC=%d\n", level, adc);
BAITOUT:
// motor stop
v |= BIT2; v |= BIT3;
i2c_write_data(i2c0, PCA_9555_SLAVE_1, PCA_9555_OUTPUT_0, &v, 1);
close(i2c0);
close(adc1);
return 0;
}
使用方式 (轉動馬達至 Level 99 的位置)
# mbrake -L 99 -T 6
-L 面是 level ; -T 後面是 time out
Treadmill 的 incline 或是 bike 的 workload 原理可能是這樣的.
Email: jasonc@mail2000.com.tw . 請尊重原創, 使用文件時載明出處. 謝謝.
文章標籤
全站熱搜
留言列表
{{ article.title }}
