六軸機械臂

洪嵐峰

製作六軸機械臂需要以下零件：

PIC18F46K22 微控制器
伺服馬達：6個
馬達驅動模組：6個
陀螺儀：1個
加速度計：1個
串列通訊模組：1個
電源供應器
連接線、螺絲等材料

以下是 PIC18F46K22 的連結腳位：


             VSS               RA0/AN0          RA1/AN1          RA2/AN2          RA3/AN3          RA4/T0CKI        RA5              VDD
             RE0/AN5/CN6       RE1/AN6/CN7      RE2/AN7/CN8      VREF+/AN8/CN9     VREF-/AN9/CN10   RC0/T1OSO/CLKO   RC1/T1OSI/CCP2   RC2/CCP1/P1A   
             RC3/SCK/SCL       RC4/SDI/SDA      RC5/SDO          RC6/TX/CK         RC7/RX/DT         VSS               VDD              OSC1/CLKI      
             OSC2/CLKO         RC8               RC9               RC10              RC11              RC12              RC13             VSS            
             VDD               RD0/PSP0         RD1/PSP1         RD2/PSP2         RD3/PSP3         RD4/PSP4         RD5/PSP5         RD6/PSP6        
             RD7/PSP7         RE3/AN4/CN5      RE4               RE5               RE6               RE7               VSS              VDD            
             RB0/INT0          RB1/INT1         RB2/INT2         RB3/CCP2/PWM      RB4/PGM           RB5               RB6              RB7            
             RB8/CCP1/PWM      RB9/SDA          RB10/SCL         RB11/CCP4/P1B     RB12/CCP5/P1C     RB13/CTED1       RB14/CTED2       RB15            
以下是一份 PIC18F46K22 六軸機械臂程式碼
使用陀螺儀和加速度計控制機械臂的運動：


#include <xc.h>

// 定義陀螺儀和加速度計的地址
#define GYRO_ADDR 0xD0
#define ACCEL_ADDR 0x30

// 陀螺儀和加速度計的數據
unsigned char gyro_data[6];
unsigned char accel_data[6];

// 初始化 I2C 通訊
void init_i2c() {
    SSP1CON1 = 0b00101000; // 啟用 SSP1，I2C 主機模式，時鐘頻率為 Fosc/(4*(SSP1ADD+1))
    SSP1CON2 = 0b00000000; // 無需
    SSP1ADD = 0x13; // 設置時鐘頻率為 100kHz
    SSP1STAT = 0b10000000; // SDA 和 SCL 都使用標準模式
}

// 從 I2C 設備讀取數據
void i2c_read(unsigned char addr, unsigned char* data, unsigned char count) {
unsigned char i;
SSP1CON2bits.SEN = 1; // 起始位
while(SSP1CON2bits.SEN); // 等待起始位完成
SSP1BUF = addr | 0x01; // 發送讀取命令
while(SSP1STATbits.R_nW); // 等待命令發送完成
SSP1CON2bits.RCEN = 1; // 啟用接收模式
for(i = 0; i < count; i++) {
while(!SSP1STATbits.BF); // 等待數據接收完成
data[i] = SSP1BUF; // 保存數據
SSP1CON2bits.ACKDT = (i == count - 1); // 最後一個數據需要發送 NACK
SSP1CON2bits.ACKEN = 1; // 發送 ACK/NACK
while(SSP1CON2bits.ACKEN); // 等待 ACK/NACK 完成
}
SSP1CON2bits.PEN = 1; // 停止位
while(SSP1CON2bits.PEN); // 等待停止位完成
}

// 往 I2C 設備寫入數據
void i2c_write(unsigned char addr, unsigned char* data, unsigned char count) {
unsigned char i;
SSP1CON2bits.SEN = 1; // 起始位
while(SSP1CON2bits.SEN); // 等待起始位完成
SSP1BUF = addr & 0xFE; // 發送寫入命令
while(SSP1STATbits.R_nW); // 等待命令發送完成
for(i = 0; i < count; i++) {
SSP1BUF = data[i]; // 發送數據
while(SSP1STATbits.R_nW); // 等待數據發送完成
if(SSP1CON2bits.ACKSTAT) { // 檢查是否有設備沒有 ACK
SSP1CON2bits.PEN = 1; // 停止位
while(SSP1CON2bits.PEN); // 等待停止位完成
return;
}
}
SSP1CON2bits.PEN = 1; // 停止位
while(SSP1CON2bits.PEN); // 等待停止位完成
}

// 初始化陀螺儀和加速度計
void init_sensors() {
unsigned char config_data[2];
// 配置陀螺儀
config_data[0] = 0x20; // CTRL_REG1
config_data[1] = 0b00001111

i2c_write(GYRO_ADDR, config_data, 2);
config_data[0] = 0x21; // CTRL_REG2
config_data[1] = 0b00000000;
i2c_write(GYRO_ADDR, config_data, 2);
config_data[0] = 0x23; // CTRL_REG4
config_data[1] = 0b00110000;
i2c_write(GYRO_ADDR, config_data, 2);
// 配置加速度計
config_data[0] = 0x20; // CTRL_REG1
config_data[1] = 0b01100111;
i2c_write(ACCEL_ADDR, config_data, 2);
config_data[0] = 0x21; // CTRL_REG2
config_data[1] = 0b00000000;
i2c_write(ACCEL_ADDR, config_data, 2);
config_data[0] = 0x23; // CTRL_REG4
config_data[1] = 0b00001000;
i2c_write(ACCEL_ADDR, config_data, 2);
}

// 讀取陀螺儀和加速度計的數據
void read_sensors() {
unsigned char gyro_data[6], accel_data[6];
// 讀取陀螺儀數據
i2c_read(GYRO_ADDR, gyro_data, 6);
gyro_x = (gyro_data[1] << 8) | gyro_data[0];
gyro_y = (gyro_data[3] << 8) | gyro_data[2];
gyro_z = (gyro_data[5] << 8) | gyro_data[4];
// 讀取加速度計數據
i2c_read(ACCEL_ADDR, accel_data, 6);
accel_x = (accel_data[1] << 8) | accel_data[0];
accel_y = (accel_data[3] << 8) | accel_data[2];
accel_z = (accel_data[5] << 8) | accel_data[4];
}

// 初始化 PWM
void init_pwm() {
TRISCbits.TRISC2 = 0; // RC2 作為 PWM 輸出
PR2 = 124; // 設置 PWM 週期為 20ms
CCPR1L = 0; // 初始佔空比為 0
CCP1CONbits.DC1B = 0;
CCP1CONbits.CCP1M = 0b1100; // PWM 模式
T2CONbits.TMR2ON = 1; // 啟用定時器 2
}

// 設置 PWM 佔空比
void set_pwm_duty(unsigned char duty) {
unsigned short duty_cycle = duty * 4;
CCPR1L = duty_cycle >> 2;
CCP1CONbits.DC1B = duty_cycle & 0b11;
}

int main() {
// 初始化 I2C 和 PWM
init_i2c();
init_pwm();
// 初始化陀螺儀和加速度計
init_sensors();
// 主循環
while(1) {
// 讀取陀螺儀和加速度計

    read_sensors();
    // 計算姿態
    calculate_angles();
    // 控制 PWM 輸出
    set_pwm_duty(90); // 佔空比為 90
    // 延時 20ms
    __delay_ms(20);
}
return 0;
}