好久没更新技术类日志了，今天更新下，这是一个韩国robotics机器人Dynamixel数字舵机的驱动，采用C语言编写，支持Dynamixel 通信协议 ，案例是AVR单片。实际上可以采用FPGA，例如SOPC系统上，可控制小型机器人。   在这也拆解下CM-5的控制器，是一片atmega128A 哦！哈哈。这PCB做工一般，感觉韩国人是科技领域爆发户，但我们有什么资格嘲笑棒子呢，至少人家都能造CPU了，我们呢？   #include #include void InitUart0(void) { UCSR0A = 0x02; // 设置为倍速模式 UBRR0H = 0; UBRR0L = 1; UCSR0B = (1< UCSR0C = (3< DDRE &= ~_BV(PE0); // 初始化RX 端口默认方向为输入 PORTE &= ~_BV(PE0); // 初始化RX 端口默认状态为高阻 DDRE |= _BV(PE1); // 初始化TX 端口默认方向为输出 PORTE |= _BV(PE1); // 初始化TX 端口默认状态为高电平 DDRA |= _BV(PA0); // 初始化使能端口状态方向为输出 PORTA &= ~_BV(PA0); // 初始化使能端口状态为RX 状态 DDRA |= _BV(PA1); // 初始化使能端口状态方向为输出 PORTA |= _BV(PA1); // 初始化使能端口状态方为TX 状态 } void SendUart0Byte(unsigned char data) { while ( !( UCSR0A & (1< UDR0 = data;/* 将数据放入缓冲器，发送数据*/ } void SetServoLimit(unsigned char id, unsigned short int cw_limit, unsigned short int ccw_limit) { unsigned short int temp_ccw = 0; // 临时速度，用于进行方向判别 unsigned short int temp_cw = 0; unsigned char temp_ccw_h = 0; // 待发送数据h 位 unsigned char temp_ccw_l = 0; // 待发送数据l 位 unsigned char temp_cw_h = 0; unsigned char temp_cw_l = 0; unsigned char temp_sum = 0; // 校验和寄存变量 if (ccw_limit > 1023) { temp_ccw = 1023; // 限制速度值在可用范围内 } else { temp_ccw = ccw_limit; } if (cw_limit > 1023) { temp_cw = 1023; } else { temp_cw = cw_limit; } temp_ccw_h = (unsigned char)(temp_ccw >> 8); temp_ccw_l = (unsigned char)temp_ccw; // 将16bit 数据拆为2个8bit 数据 temp_cw_h = (unsigned char)(temp_cw >> 8); temp_cw_l = (unsigned char)temp_cw; // 将16bit 数据拆为2个8bit 数据 PORTA &= ~_BV(PA1); PORTA |= _BV(PA0); // 使总线处于主机发送状态 UCSR0A |= (1< SendUart0Byte(0xFF); // 发送启动符号0xFF SendUart0Byte(0xFF); // 发送启动符号0xFF SendUart0Byte(id); // 发送id SendUart0Byte(7); // 发送数据长度为参数长度+2，参数长度为3 SendUart0Byte(0x03); // 命令数据为“WRITE DATA” SendUart0Byte(0x06); // 舵机控制寄存器首地址 SendUart0Byte(temp_cw_l); // 发送顺时针位置限制低位 SendUart0Byte(temp_cw_h); // 发送顺时针位置限制高位 SendUart0Byte(temp_ccw_l); // 发送逆时针位置限制低位 SendUart0Byte(temp_ccw_h); // 发送逆时针位置限制高位 temp_sum = id + 7 + 0x03 + 0x06 + temp_cw_l + temp_cw_h + temp_ccw_l + temp_ccw_h; temp_sum = ~temp_sum; // 计算校验和 SendUart0Byte(temp_sum); // 发送校验和 while ( !( UCSR0A & (1< { // (Waiting for finishing sending) ; } PORTA |= _BV(PA1); PORTA &= ~_BV(PA0); // 使总线处于主机接收状态 _delay_ms(2); //送完成后，总线会被从机占用，反馈应答数据，所以进行延时 } void SetServoPosition(unsigned char id, unsigned short int position, unsigned short int velocity) { unsigned short int temp_velocity = 0; // 临时速度，用于进行方向判别 unsigned short int temp_position = 0; unsigned char temp_velocity_h = 0; // 待发送数据h 位 unsigned char temp_velocity_l = 0; // 待发送数据l 位 unsigned char temp_position_h = 0; unsigned char temp_position_l = 0; unsigned char temp_sum = 0; // 校验和寄存变量 if (velocity > 1023) { temp_velocity = 1023; // 限制速度值在可用范围内 } else { temp_velocity = velocity; } if (position > 1023) { temp_position = 1023; } else { temp_position = position; } temp_velocity_h = (unsigned char)(temp_velocity >> 8); // 将16bit 数据拆为2个8bit 数据 temp_velocity_l = (unsigned char)temp_velocity; temp_position_h = (unsigned char)(temp_position >> 8); // 将16bit 数据拆为2个8bit 数据 temp_position_l = (unsigned char)temp_position; PORTA &= ~_BV(PA1); PORTA |= _BV(PA0); // 使总线处于主机发送状态 UCSR0A |= (1< SendUart0Byte(0xFF); // 发送启动符号0xFF SendUart0Byte(0xFF); SendUart0Byte(id); // 发送id SendUart0Byte(7); // 发送数据长度为参数长度+2，参数长度为3 SendUart0Byte(0x03); // 命令数据为“WRITE DATA” SendUart0Byte(0x1E); // 舵机控制寄存器首地址 SendUart0Byte(temp_position_l); // 发送速度数据低位 SendUart0Byte(temp_position_h); // 发送速度数据高位 SendUart0Byte(temp_velocity_l); //发送位置低字节 SendUart0Byte(temp_velocity_h); // 发送位置高字节 temp_sum = id + 7 + 0x03 + 0x1E + temp_position_l + temp_position_h + temp_velocity_l + temp_velocity_h; temp_sum = ~temp_sum; // 计算校验和 SendUart0Byte(temp_sum); // 发送校验和 (Send the checksum) while ( !( UCSR0A & (1< { // (Waiting for finishing sending) ; } PORTA |= _BV(PA1); PORTA &= ~_BV(PA0); // 使总线处于主机接收状态 _delay_ms(2); // 发送完成后，总线会被从机占用，反馈应答数据，所以进行延时 } void SetServoVelocity(unsigned char id, signed short int velocity) { unsigned char temp_sign = 0; // 临时符号，用于进行方向判别 unsigned short int temp_velocity = 0; // 临时速度，用于进行方向判别 unsigned char temp_value_h = 0; // 待发送数据h 位 unsigned char temp_value_l = 0; // 待发送数据l 位 unsigned char temp_sum = 0; // 校验和寄存变量 if (velocity < 0) { temp_velocity = -velocity; // 如果为负数，则取绝对值 temp_sign = 1; // 设置负数符号标志 } else { temp_velocity = velocity; temp_sign = 0; // 设置正数符号标志 } if (temp_velocity > 1023) { temp_velocity = 1023; // 限制速度值在可用范围内 } temp_velocity |= (temp_sign << 10); temp_value_h = (unsigned char)(temp_velocity >> 8); // 将16bit 数据拆为2个8bit 数据 temp_value_l = (unsigned char)temp_velocity; PORTA &= ~_BV(PA1); PORTA |= _BV(PA0); // 使总线处于主机发送状态 UCSR0A |= (1< SendUart0Byte(0xFF); // 发送启动符号0xFF SendUart0Byte(0xFF); // 发送启动符号0xFF SendUart0Byte(id); // 发送id SendUart0Byte(5); // 发送数据长度为参数长度+2，参数长度为3 SendUart0Byte(0x03); // 命令数据为“WRITE DATA” SendUart0Byte(0x20); // 舵机控制寄存器首地址 SendUart0Byte(temp_value_l); // 发送速度数据低位 SendUart0Byte(temp_value_h); // 发送速度数据高位 temp_sum = id + 5 + 0x03 + 0x20 + temp_value_l + temp_value_h; temp_sum = ~temp_sum; // 计算校验和 SendUart0Byte(temp_sum); // 发送校验和 while ( !( UCSR0A & (1< { ; } PORTA |= _BV(PA1); PORTA &= ~_BV(PA0); // 使总线处于主机接收状态 _delay_ms(2); // 发送完成后，总线会被从机占用，反馈应答数据，所以进行延时 } int main(void) { InitUart0(); SetServoLimit(2,0,1023); while(1) { _delay_ms(1000); //延时1s SetServoPosition(2, 1000, 500); //控制舵机以500的速度运动到1000的位置 _delay_ms(1000); //延时1s SetServoPosition(2, 200, 100); //控制舵机以100的速度运动到200的位置 } } |
更多回帖
