编程6自由度机械手通常涉及以下步骤：
逆解计算
根据末端执行器的位姿和机械臂的运动学参数，反推出机械臂各个关节的角度。这通常在MATLAB中使用Robotic Toolbox进行逆解计算。
电机控制
编写程序控制电机的运动，确保机械手的各个关节能够按照预定的角度进行精确控制。这可能涉及到对电机的初始化、控制信号的生成和PWM波的输出等。
传感器集成
如果机械手配备了传感器（如位置传感器、力传感器等），需要编写相应的程序来读取传感器数据，以便实时监控机械手的状态并进行相应的调整。
人机交互
通过人机交互界面（如LCD显示器、按钮等）或者控制软件，设置机械手的坐标系、转动方向、加速度等参数，以及运动轨迹和操作模式。
调试和测试
在实际环境中测试机械手的运动，确保其按照预期进行各种动作，如抓取、移动、放置等。必要时进行调整和微调。
安全考虑
在编写程序时，需要考虑机械手的结构、重量、惯性等影响稳定性的因素，并确保机械手的动作不会对人员和设备造成危害。
```c
include
define uint unsigned int
define uchar unsigned char
sbit k1 = P3^2； // 电机复位按钮
sbit k2 = P3^3； // 电机选择按钮
sbit k3 = P3^4； // 电机正转
sbit k4 = P3^5； // 电机反转
sbit rs = P2^0；
sbit rw = P2^1；
sbit en = P2^2；
uint m, i；
void servo_init（） {
TMOD = 0x01；
TH0 = 0xB1；
TL0 = 0xE0；
ET0 = 1；
EA = 1；
TR0 = 1；
COM1 = 0x30；
C01 = 0xDC；
C01 = 0x05；
COM1 = 0x70；
C11 = 0xDC；
C11 = 0x05；
COM1 = 0xB0；
C21 = 0xDC；
C21 = 0x05；
}
void servo_on（uchar channel） {
switch （channel） {
case 1: com1 = 0x30； break；
case 2: com1 = 0x70； break；
case 3: com1 = 0xB0； break；
case 4: com1 = 0x30； break；
case 5: com1 = 0x70； break；
case 6: com1 = 0xB0； break；
}
EN = 1；
}
void servo_off（uchar channel） {
switch （channel） {
case 1: com1 = 0x30； break；
case 2: com1 = 0x70； break；
case 3: com1 = 0xB0； break；
case 4: com1 = 0x30； break；
case 5: com1 = 0x70； break；
case 6: com1 = 0xB0； break；
}
EN = 0；
}
void delay（uint n） {
while （n--） {
_nop_（）；
}
}
void main（） {
servo_init（）；
while （1） {
// 控制电机运动
servo_on（1）； // 关节1正转
delay（1000）；
servo_off（1）；
servo_on（2）； // 关节2正转
delay（1000）；
servo_off（2）；
// ... 其他关节控制
}
}
```
请注意，这只是一个非常基础的示例，实际的6自由度机械手控制系统会更加复杂