Skip to content
Snippets Groups Projects
Commit ab2b18cc authored by Patrick John Grosch Obregon's avatar Patrick John Grosch Obregon
Browse files

sincronizando

parent 2f69bc77
No related branches found
No related tags found
No related merge requests found
Hide whitespace changes
Inline Side-by-side
Showing
with 1711 additions and 143 deletions
servo_firmware/mx28/Makefile
+ 1
− 0
View file @ ab2b18cc
......@@ -43,6 +43,7 @@ MAIN_OBJS = \
src/eeprom.o \
src/ram.o \
src/time.o \
src/pwm.o \
src/stm32f10x_it.o \
src/system_init.o \
src/dynamixel.o \
......
servo_firmware/mx28/src/mx28_fw.c
+ 79
− 135
View file @ ab2b18cc
/**
******************************************************************************
* @file Project/STM32F10x_StdPeriph_Template/main.c
* @author MCD Application Team
* @version V3.5.0
* @date 08-April-2011
* @brief Main program body
******************************************************************************
* @attention
*
* THE PRESENT FIRMWARE WHICH IS FOR GUIDANCE ONLY AIMS AT PROVIDING CUSTOMERS
* WITH CODING INFORMATION REGARDING THEIR PRODUCTS IN ORDER FOR THEM TO SAVE
* TIME. AS A RESULT, STMICROELECTRONICS SHALL NOT BE HELD LIABLE FOR ANY
* DIRECT, INDIRECT OR CONSEQUENTIAL DAMAGES WITH RESPECT TO ANY CLAIMS ARISING
* FROM THE CONTENT OF SUCH FIRMWARE AND/OR THE USE MADE BY CUSTOMERS OF THE
* CODING INFORMATION CONTAINED HEREIN IN CONNECTION WITH THEIR PRODUCTS.
*
* <h2><center>&copy; COPYRIGHT 2011 STMicroelectronics</center></h2>
******************************************************************************
*/
******************************************************************************
* @file Project/STM32F10x_StdPeriph_Template/main.c
* @author MCD Application Team
* @version V3.5.0
* @date 08-April-2011
* @brief Main program body
******************************************************************************
* @attention
*
* THE PRESENT FIRMWARE WHICH IS FOR GUIDANCE ONLY AIMS AT PROVIDING CUSTOMERS
* WITH CODING INFORMATION REGARDING THEIR PRODUCTS IN ORDER FOR THEM TO SAVE
* TIME. AS A RESULT, STMICROELECTRONICS SHALL NOT BE HELD LIABLE FOR ANY
* DIRECT, INDIRECT OR CONSEQUENTIAL DAMAGES WITH RESPECT TO ANY CLAIMS ARISING
* FROM THE CONTENT OF SUCH FIRMWARE AND/OR THE USE MADE BY CUSTOMERS OF THE
* CODING INFORMATION CONTAINED HEREIN IN CONNECTION WITH THEIR PRODUCTS.
*
* <h2><center>&copy; COPYRIGHT 2011 STMicroelectronics</center></h2>
******************************************************************************
*/
/* Includes ------------------------------------------------------------------*/
#include <math.h>
#include "stm32f10x.h"
#include "system_init.h"
#include "dynamixel_slave.h"
#include "time.h"
#include "eeprom.h"
#include "ram.h"
uint8_t read_operation(uint8_t address, uint8_t length, uint8_t *data)
{
if(address >= CM730_MODEL_NUMBER_L && address < EEPROM_DATA_SIZE) // EEPROM region
{
if(eeprom_read_table(address,length,data)==EEPROM_SUCCESS)
return DYN_NO_ERROR;
else
return DYN_INST_ERROR;
}
else// RAM region
{
if(ram_read_table(address,length,data)==RAM_SUCCESS)
return DYN_NO_ERROR;
else
return DYN_INST_ERROR;
}
}
uint8_t write_operation(uint8_t address, uint8_t length, uint8_t *data)
{
uint8_t error=DYN_NO_ERROR,i=0,remaining_length;
remaining_length=length;
if(address >= CM730_MODEL_NUMBER_L && address < EEPROM_DATA_SIZE) // EEPROM region
{
if(eeprom_write_table(address,length,data)==EEPROM_SUCCESS)
return DYN_NO_ERROR;
else
return DYN_INST_ERROR;
}
else// RAM region
{
while(remaining_length>0)
{
if(address==CM730_DXL_POWER)
{
i++;
address++;
remaining_length--;
}
else if(address==CM730_LED_PANNEL)
{
i++;
address++;
remaining_length--;
}
else
error=DYN_INST_ERROR;
}
return error;
}
}
int main(void) {
uint8_t inst_packet[1024];
uint32_t i, j, h;
void process_packet(uint8_t *packet)
{
uint8_t data[MAX_DATA_LENGTH];
uint8_t error,length,address;
switch(dyn_get_instruction(packet))
{
case DYN_PING: dyn_slave_send_status_packet(DYN_NO_ERROR,0,data);
break;
case DYN_READ: error=read_operation(dyn_get_read_address(packet),dyn_get_read_length(packet),data);
if(error==DYN_NO_ERROR)
dyn_slave_send_status_packet(error,dyn_get_read_length(packet),data);
else
dyn_slave_send_status_packet(DYN_NO_ERROR,0,data);
break;
case DYN_WRITE: length=dyn_get_write_data(packet,data);
address=dyn_get_write_address(packet);
error=write_operation(address,length,data);
dyn_slave_send_status_packet(error,0,data);
// update the device address
if(address<=CM730_ID && (address+length)>=CM730_ID)
dyn_slave_set_address(eeprom_read_device_id());
break;
case DYN_REG_WRITE:
break;
case DYN_ACTION:
break;
case DYN_RESET:
break;
case DYN_SYNC_WRITE:
break;
}
}
// initialize the general features of the system like:
// GPIOA Periph clock enable RCC_APB2PeriphClockCmd(RCC_APB2Periph_GPIOA, ENABLE); //el del LED
// GPIOA Periph clock enable RCC_APB2PeriphClockCmd(RCC_APB2Periph_GPIOA, ENABLE); //SHUT_DOWN DIR_POS DIR_NEG
clocks_init();
// initialize the time base module
// set to (miliseconds)
time_init();
GPIO_InitTypeDef GPIO_InitStructure;
GPIO_StructInit(&GPIO_InitStructure);
#define LED GPIO_Pin_13
#define SHUT_DOWN GPIO_Pin_12
#define DIR_POS GPIO_Pin_8
#define DIR_NEG GPIO_Pin_9
int main(void)
{
uint8_t inst_packet[1024];
int blink_count=0;
// initialize the general features of the system
clocks_init();
// initialize the internal RAM of the device
ram_init();
// initialize the time base module
time_init();
// initialize the dynamixel slave module
dyn_slave_init();
dyn_slave_set_address(eeprom_read_device_id());
GPIO_InitTypeDef GPIO_InitStructure;
GPIO_StructInit(&GPIO_InitStructure);
// configure the LED's
GPIO_InitStructure.GPIO_Pin = GPIO_Pin_13;
GPIO_InitStructure.GPIO_Speed = GPIO_Speed_50MHz;
GPIO_InitStructure.GPIO_Mode = GPIO_Mode_Out_PP;
GPIO_Init(GPIOA, &GPIO_InitStructure);
//GPIO_ResetBits(GPIOA,GPIO_Pin_13); // LED ON
GPIO_SetBits(GPIOA,GPIO_Pin_13); // LED OFF
while(1)
{
// Configure LED in output pushpull mode
GPIO_InitStructure.GPIO_Pin = LED | SHUT_DOWN | DIR_POS | DIR_NEG;
GPIO_InitStructure.GPIO_Speed = GPIO_Speed_50MHz;
GPIO_InitStructure.GPIO_Mode = GPIO_Mode_Out_PP;
GPIO_Init(GPIOA, &GPIO_InitStructure);
/*#define STATUS_LED_CM900 GPIO_Pin_2
if(dyn_slave_is_packet_ready())// check if a new instruction packet has been received
{
dyn_slave_get_inst_packet(inst_packet);// get the received packet
......@@ -183,3 +93,37 @@ int main(void)
}
}
// CONFIGURATION EXAMPLES OF DIFERENT TYPES OF IN AND OUTS
/*//Configure Leds (PC8 & PC9) mounted on STM32 Discovery board - OutPut Push Pull
GPIO_InitStructure.GPIO_Pin = GPIO_Pin_8 | GPIO_Pin_9;
GPIO_InitStructure.GPIO_Mode = GPIO_Mode_Out_PP;
GPIO_InitStructure.GPIO_Speed = GPIO_Speed_2MHz;
GPIO_Init(GPIOC, &GPIO_InitStructure);
// Configure BLUE Botton (B1 User - PA0) on STM32 Discovery board - Input Floatting
GPIO_InitStructure.GPIO_Pin = GPIO_Pin_0;
GPIO_InitStructure.GPIO_Mode = GPIO_Mode_IN_FLOATING;
GPIO_Init(GPIOA, &GPIO_InitStructure);
// COnfigure ADC on ADC1_IN10 pin PC0
GPIO_InitStructure.GPIO_Pin = GPIO_Pin_0;
GPIO_InitStructure.GPIO_Mode = GPIO_Mode_AIN;
GPIO_Init(GPIOC, &GPIO_InitStructure);
//Configure USART1 Tx (PA9) as alternate function push-pull
GPIO_InitStructure.GPIO_Pin = GPIO_Pin_9;
GPIO_InitStructure.GPIO_Speed = GPIO_Speed_2MHz;
GPIO_InitStructure.GPIO_Mode = GPIO_Mode_AF_PP;
GPIO_Init(GPIOA, &GPIO_InitStructure);
// Configure USART1 Rx (PA10) as input floating
GPIO_InitStructure.GPIO_Pin = GPIO_Pin_10;
GPIO_InitStructure.GPIO_Mode = GPIO_Mode_IN_FLOATING;
GPIO_Init(GPIOA, &GPIO_InitStructure);
// Configure SPI - CLK PA5, MISO PA6, MOSI PA7
GPIO_InitStructure.GPIO_Pin = GPIO_Pin_5 | GPIO_Pin_6 | GPIO_Pin_7;
GPIO_InitStructure.GPIO_Speed = GPIO_Speed_2MHz;
GPIO_InitStructure.GPIO_Mode = GPIO_Mode_AF_PP;
GPIO_Init(GPIOA, &GPIO_InitStructure);*/
servo_firmware/mx28/src/system_init.c
+ 14
− 4
View file @ ab2b18cc
......@@ -103,14 +103,24 @@ void clocks_init(void)
NVIC_InitStructure.NVIC_IRQChannelCmd = ENABLE;
NVIC_Init(&NVIC_InitStructure);
/* enable the peripheral clocks */
/* DMA clock enable */
/* enable GPIO's, USART1, ADC */
//***************************************************************************************************
/* se anula todo esto para ver los conflictos
// enable the peripheral clocks
// DMA clock enable
// enable GPIO's, USART1, ADC
RCC_APB2PeriphClockCmd(RCC_APB2Periph_USART1 | RCC_APB2Periph_GPIOA | RCC_APB2Periph_GPIOC | RCC_APB2Periph_GPIOB |
RCC_APB2Periph_AFIO | RCC_APB2Periph_ADC1 | RCC_APB2Periph_ADC2, ENABLE);
/* enable USART3 */
// enable USART3
RCC_APB1PeriphClockCmd(RCC_APB1Periph_SPI2 | RCC_APB1Periph_USART3 | RCC_APB1Periph_TIM2 | RCC_APB1Periph_TIM3 | RCC_APB1Periph_TIM4 ,ENABLE);
*/
// CASE ONLY LED IN PIN PA13 PA12 PA8 PA9
RCC_APB2PeriphClockCmd(RCC_APB2Periph_GPIOA , ENABLE);
// CASE ONLY LEDs IN PIN PA13 AND PB2 card CM-900
//RCC_APB2PeriphClockCmd(RCC_APB2Periph_GPIOA | RCC_APB2Periph_GPIOB , ENABLE);
//************************************************************************************************************
}
else
{ /* If HSE fails to start-up, the application will have wrong clock configuration.
......
servo_firmware/mx28/stm32_lib/include/devices/stm32f10x.h
+ 4
− 4
View file @ ab2b18cc
......@@ -15,15 +15,15 @@
* is using in the C source code, usually in main.c. This file contains:
* - Configuration section that allows to select:
* - The device used in the target application
* - To use or not the peripheral’s drivers in application code(i.e.
* code will be based on direct access to peripheral’s registers
* - To use or not the peripheral�s drivers in application code(i.e.
* code will be based on direct access to peripheral�s registers
* rather than drivers API), this option is controlled by
* "#define USE_STDPERIPH_DRIVER"
* - To change few application-specific parameters such as the HSE
* crystal frequency
* - Data structures and the address mapping for all peripherals
* - Peripheral's registers declarations and bits definition
* - Macros to access peripheral’s registers hardware
* - Macros to access peripheral�s registers hardware
*
******************************************************************************
* @attention
......@@ -52,7 +52,7 @@
#ifdef __cplusplus
extern "C" {
#endif
#endif m32f10x.h
/** @addtogroup Library_configuration_section
* @{
......
servo_firmware/rx28/.cproject 0 → 100644
+ 53
− 0
View file @ ab2b18cc
<?xml version="1.0" encoding="UTF-8" standalone="no"?>
<?fileVersion 4.0.0?>
<cproject storage_type_id="org.eclipse.cdt.core.XmlProjectDescriptionStorage">
<storageModule moduleId="org.eclipse.cdt.core.settings">
<cconfiguration id="0.1580636782">
<storageModule buildSystemId="org.eclipse.cdt.managedbuilder.core.configurationDataProvider" id="0.1580636782" moduleId="org.eclipse.cdt.core.settings" name="Default">
<externalSettings/>
<extensions>
<extension id="org.eclipse.cdt.core.VCErrorParser" point="org.eclipse.cdt.core.ErrorParser"/>
<extension id="org.eclipse.cdt.core.GmakeErrorParser" point="org.eclipse.cdt.core.ErrorParser"/>
<extension id="org.eclipse.cdt.core.CWDLocator" point="org.eclipse.cdt.core.ErrorParser"/>
<extension id="org.eclipse.cdt.core.GCCErrorParser" point="org.eclipse.cdt.core.ErrorParser"/>
<extension id="org.eclipse.cdt.core.GASErrorParser" point="org.eclipse.cdt.core.ErrorParser"/>
<extension id="org.eclipse.cdt.core.GLDErrorParser" point="org.eclipse.cdt.core.ErrorParser"/>
</extensions>
</storageModule>
<storageModule moduleId="cdtBuildSystem" version="4.0.0">
<configuration buildProperties="" description="" id="0.1580636782" name="Default" parent="org.eclipse.cdt.build.core.prefbase.cfg">
<folderInfo id="0.1580636782." name="/" resourcePath="">
<toolChain id="org.eclipse.cdt.build.core.prefbase.toolchain.751355657" name="No ToolChain" resourceTypeBasedDiscovery="false" superClass="org.eclipse.cdt.build.core.prefbase.toolchain">
<targetPlatform id="org.eclipse.cdt.build.core.prefbase.toolchain.751355657.899062278" name=""/>
<builder id="org.eclipse.cdt.build.core.settings.default.builder.833242130" keepEnvironmentInBuildfile="false" managedBuildOn="false" name="Gnu Make Builder" superClass="org.eclipse.cdt.build.core.settings.default.builder"/>
<tool id="org.eclipse.cdt.build.core.settings.holder.libs.1243286949" name="holder for library settings" superClass="org.eclipse.cdt.build.core.settings.holder.libs"/>
<tool id="org.eclipse.cdt.build.core.settings.holder.1125815023" name="Assembly" superClass="org.eclipse.cdt.build.core.settings.holder">
<inputType id="org.eclipse.cdt.build.core.settings.holder.inType.1661592086" languageId="org.eclipse.cdt.core.assembly" languageName="Assembly" sourceContentType="org.eclipse.cdt.core.asmSource" superClass="org.eclipse.cdt.build.core.settings.holder.inType"/>
</tool>
<tool id="org.eclipse.cdt.build.core.settings.holder.1459142952" name="GNU C++" superClass="org.eclipse.cdt.build.core.settings.holder">
<inputType id="org.eclipse.cdt.build.core.settings.holder.inType.482726272" languageId="org.eclipse.cdt.core.g++" languageName="GNU C++" sourceContentType="org.eclipse.cdt.core.cxxSource,org.eclipse.cdt.core.cxxHeader" superClass="org.eclipse.cdt.build.core.settings.holder.inType"/>
</tool>
<tool id="org.eclipse.cdt.build.core.settings.holder.92886424" name="GNU C" superClass="org.eclipse.cdt.build.core.settings.holder">
<inputType id="org.eclipse.cdt.build.core.settings.holder.inType.123207293" languageId="org.eclipse.cdt.core.gcc" languageName="GNU C" sourceContentType="org.eclipse.cdt.core.cSource,org.eclipse.cdt.core.cHeader" superClass="org.eclipse.cdt.build.core.settings.holder.inType"/>
</tool>
</toolChain>
</folderInfo>
</configuration>
</storageModule>
<storageModule moduleId="org.eclipse.cdt.core.externalSettings"/>
</cconfiguration>
</storageModule>
<storageModule moduleId="cdtBuildSystem" version="4.0.0">
<project id="rx28.null.146688741" name="rx28"/>
</storageModule>
<storageModule moduleId="scannerConfiguration">
<autodiscovery enabled="true" problemReportingEnabled="true" selectedProfileId=""/>
<scannerConfigBuildInfo instanceId="0.1580636782">
<autodiscovery enabled="true" problemReportingEnabled="true" selectedProfileId=""/>
</scannerConfigBuildInfo>
<scannerConfigBuildInfo instanceId="0.1321873902">
<autodiscovery enabled="true" problemReportingEnabled="true" selectedProfileId=""/>
</scannerConfigBuildInfo>
</storageModule>
</cproject>
servo_firmware/rx28/.project 0 → 100644
+ 79
− 0
View file @ ab2b18cc
<?xml version="1.0" encoding="UTF-8"?>
<projectDescription>
<name>ax12</name>
<comment></comment>
<projects>
</projects>
<buildSpec>
<buildCommand>
<name>org.eclipse.cdt.managedbuilder.core.genmakebuilder</name>
<triggers>clean,full,incremental,</triggers>
<arguments>
<dictionary>
<key>?name?</key>
<value></value>
</dictionary>
<dictionary>
<key>org.eclipse.cdt.make.core.append_environment</key>
<value>true</value>
</dictionary>
<dictionary>
<key>org.eclipse.cdt.make.core.autoBuildTarget</key>
<value>all</value>
</dictionary>
<dictionary>
<key>org.eclipse.cdt.make.core.buildArguments</key>
<value></value>
</dictionary>
<dictionary>
<key>org.eclipse.cdt.make.core.buildCommand</key>
<value>make</value>
</dictionary>
<dictionary>
<key>org.eclipse.cdt.make.core.cleanBuildTarget</key>
<value>clean</value>
</dictionary>
<dictionary>
<key>org.eclipse.cdt.make.core.contents</key>
<value>org.eclipse.cdt.make.core.activeConfigSettings</value>
</dictionary>
<dictionary>
<key>org.eclipse.cdt.make.core.enableAutoBuild</key>
<value>false</value>
</dictionary>
<dictionary>
<key>org.eclipse.cdt.make.core.enableCleanBuild</key>
<value>true</value>
</dictionary>
<dictionary>
<key>org.eclipse.cdt.make.core.enableFullBuild</key>
<value>true</value>
</dictionary>
<dictionary>
<key>org.eclipse.cdt.make.core.fullBuildTarget</key>
<value>all</value>
</dictionary>
<dictionary>
<key>org.eclipse.cdt.make.core.stopOnError</key>
<value>true</value>
</dictionary>
<dictionary>
<key>org.eclipse.cdt.make.core.useDefaultBuildCmd</key>
<value>true</value>
</dictionary>
</arguments>
</buildCommand>
<buildCommand>
<name>org.eclipse.cdt.managedbuilder.core.ScannerConfigBuilder</name>
<triggers>full,incremental,</triggers>
<arguments>
</arguments>
</buildCommand>
</buildSpec>
<natures>
<nature>org.eclipse.cdt.core.cnature</nature>
<nature>org.eclipse.cdt.core.ccnature</nature>
<nature>org.eclipse.cdt.managedbuilder.core.managedBuildNature</nature>
<nature>org.eclipse.cdt.managedbuilder.core.ScannerConfigNature</nature>
</natures>
</projectDescription>
servo_firmware/rx28/Makefile 0 → 100755
+ 37
− 0
View file @ ab2b18cc
PROJECT=rx28_fw
########################################################
# afegir tots els fitxers que s'han de compilar aquí
########################################################
SOURCES=src/main.c src/TXRX_Dynamixel.c src/CTRL_Dynamixel.c src/MEM_Dynamixel.c src/CFG_HW_Dynamixel.c
OBJS=$(SOURCES:.c=.o)
SRC_DIR=./src/
INCLUDE_DIR=./include/
BUILD_DIR=./build/
BIN_DIR=./bin/
CC=avr-gcc
OBJCOPY=avr-objcopy
MMCU=atmega8
CFLAGS=-mmcu=$(MMCU) -Wall -Os -DF_CPU=16000000UL -gdwarf-2 -std=gnu99 -funsigned-char -funsigned-bitfields -fpack-struct -fshort-enums -Wstrict-prototypes
LDFLAGS=-mmcu=$(MMCU) -Wl,-Map=$(PROJECT).map -DF_CPU=16000000UL
HEX_FLASH_FLAGS = -R .eeprom -R .fuse -R .lock -R .signature
.PHONY: all
all: $(PROJECT).hex download
$(PROJECT).hex: $(PROJECT).elf
$(OBJCOPY) -O ihex $(HEX_FLASH_FLAGS) $< $@
$(PROJECT).elf: $(OBJS)
$(CC) $(LDFLAGS) $(OBJS) $(LIB_DIRS) $(LIBS) -o $(PROJECT).elf
%.o:%.c
$(CC) -c $(CFLAGS) -I$(INCLUDE_DIR) -o $@ $<
download: $(MAIN_OUT_HEX)
../../bin/fw_downloader -d /dev/ttyUSB0 -f $(PROJECT).hex -s rx28
clean:
rm $(PROJECT).*
rm $(OBJS)
servo_firmware/rx28/include/CFG_HW_Dynamixel.h 0 → 100755
+ 27
− 0
View file @ ab2b18cc
#ifndef _CFG_HW_DYNAMIXEL_H
#define _CFG_HW_DYNAMIXEL_H
#define LED_PIN PD2
#define ENABLE_MOTOR_PIN PD7
#define CW_PWM_MOTOR_PIN PB1
#define CCW_PWM_MOTOR_PIN PB2
// this is for debuging tonggle LED i times
// void blinkLedN(uint8_t i);
#define LedOFF PORTD |= (1<<LED_PIN) // off LED
#define LedON PORTD &= ~(1<<LED_PIN) // ON LED
#define LedTOGGLE PORTD ^= _BV(LED_PIN) // Toggle LED
#define SET_CW_PWM_MOTOR(x) OCR1A = (x) //PB1 => set PWM for X% duty cycle @ 10bit
#define SET_CCW_PWM_MOTOR(x) OCR1B = (x) //PB2 => set PWM for Y% duty cycle @ 10bit
#define ENABLE_MOTOR PORTD &= ~(1<<ENABLE_MOTOR_PIN) // enable motor
#define DISABLE_MOTOR PORTD |= (1<<ENABLE_MOTOR_PIN) // disable motor
void Config_Hardware(void);
#endif
servo_firmware/rx28/include/CTRL_Dynamixel.h 0 → 100644
+ 25
− 0
View file @ ab2b18cc
#ifndef _CTRL_DYNAMIXEL_H
#define _CTRL_DYNAMIXEL_H
#define CTRL_ZERO 0x0000
#define CTRL_Dir_CCW 1
#define CTRL_Dir_CW -1
//#define CTRL_Sensor_Resol 1024 // sensor resolution
//#define CTRL_Max_Angle 300 // max angle
#define CTRL_Time_Period 0.01 // ctrl period
#define CTRL_Encoder_Port 0 // Input port potenciometer
// asign actual position to goal position and asume zero motor turns
// set motor turns = 0 ... start controller
void Ini_Position(void);
// Read ADc value in port return position
int16_t Read_Sensor(uint8_t port);
// control cycle
void Control_Cycle(void);
// limit control action ... max value and alarms
void HW_Security(void);
// place control action on output
void Write_Actuator(void);
#endif
\ No newline at end of file
servo_firmware/rx28/include/MEM_Dynamixel.h 0 → 100755
+ 133
− 0
View file @ ab2b18cc
#ifndef _MEM_DYNAMIXEL_H
#define _MEM_DYNAMIXEL_H
// Register Id EEPROM - declared in dynamixel
#define Model_Number_L 0X00 //0 - 0X0C R - Lowest byte of model number
#define Model_Number_H 0X01 //1 - 0X00 R - Highest byte of model number
#define Version_Firmware 0X02 //2 - 0X00 R - Information on the version of firmware
#define ID 0X03 //3 - 0X03 R/W - ID of Dynamixel
#define Baud_Rate 0X04 //4 - 0X01 R/W - Baud Rate of Dynamixel
#define Return_Delay_Time 0X05 //5 - 0XFA R/W - Return Delay Time
#define CW_Angle_Limit_L 0X06 //6 - 0X00 R/W - Lowest byte of clockwise Angle Limit
#define CW_Angle_Limit_H 0X07 //7 - 0X00 R/W - Highest byte of clockwise Angle Limit
#define CCW_Angle_Limit_L 0X08 //8 - 0XFF R/W - Lowest byte of counterclockwise Angle Limit
#define CCW_Angle_Limit_H 0X09 //9 - 0X03 R/W - Highest byte of counterclockwise Angle Limit
// 10 no used
#define Int_Limit_Temperature 0X0B //11 - 0X46 R/W - Internal Highest Limit Temperature
#define Low_Limit_Voltage 0X0C //12 - 0X3C R/W - the Lowest Limit Voltage
#define High_Limit_Voltage 0X0D //13 - 0XBE R/W - the Highest Limit Voltage
#define Max_Torque_L 0X0E //14 - 0XFF R/W - Lowest byte of Max. Torque
#define Max_Torque_H 0X0F //15 - 0X03 R/W - Highest byte of Max. Torque
#define Status_Return_Level 0X10 //16 - 0X02 R/W - Status Return Level
#define Alarm_LED 0X11 //17 - 0x24 R/W - LED for Alarm
#define Alarm_Shutdown 0X12 //18 - 0x24 R/W - Shutdown for Alarm
// not define in dynamixell
#define Gate_Restore_Eeprom 0X0A // 10 0x01 R/W - Gate variable to restores eeprom factory values
//19 => 23 not used
#define RAM_ID_correction 24 // to keep a sequence in the parameters ID between RAM and Eemprom a correction must be done
// to read from ram_vector(parameterID - correction)
// Register Id RAM - declared in dynamixel
#define Torque_Enable 0X18 //24 - 0x00 R/W - Torque On/Off
#define LED 0X19 //25 - 0x00 R/W - LED On/Off
#define CW_Compliance_Margin 0X1A //26 - 0X01 R/W - CW Compliance margin
#define CCW_Compliance_Margin 0X1B //27 - 0X01 R/W - CCW Compliance margin
#define CW_Compliance_Slope 0X1C //28 - 0X20 R/W - CW Compliance slope
#define CCW_Compliance_Slope 0X1D //29 - 0X20 R/W - CCW Compliance Slope
#define Goal_Position_L 0X1E //30 - 0x00 R/W - Lowest byte of Goal Position
#define Goal_Position_H 0X1F //31 - 0x00 R/W - Highest byte of Goal Position
#define Moving_Speed_L 0X20 //32 - 0xF0 R/W - Lowest byte of Moving Speed
#define Moving_Speed_H 0X21 //33 - 0x0F R/W - Highest byte of Moving Speed
#define Torque_Limit_L 0X22 //34 - 0xF0 R/W - Lowest byte of Torque Limit
#define Torque_Limit_H 0X23 //35 - 0x0F R/W - Highest byte of Torque Limit
#define Present_Position_L 0X24 //36 - 0x00 R - Lowest byte of Current Position Position_Value
#define Present_Position_H 0X25 //37 - 0x00 R - Highest byte of Current Position
#define Present_Speed_L 0X26 //38 - 0x00 R - Lowest byte of Current Speed
#define Present_Speed_H 0X27 //39 - 0x00 R - Highest byte of Current Speed
#define Present_Load_L 0X28 //40 - 0x00 R - Lowest byte of Current Load
#define Present_Load_H 0X29 //41 - 0x00 R - Highest byte of Current Load
#define Present_Voltage 0X2A //42 - 0x00 R - Current Voltage
#define Present_Temperature 0X2B //43 - 0x00 R - Current Temperature
#define Registered 0X2C //44 - 0x00 R - Means if Instruction is registered
//45 no used
#define Moving 0X2E //46 - 0X00 R - Means if there is any movement
#define Lock 0X2F //47 - 0x00 R/W - Locking EEPROM
#define Punch_L 0X30 //48 - 0X20 R/W - Lowest byte of Punch
#define Punch_H 0X31 //49 - 0X00 R/W - Highest byte of Punch
// Register Id RAM - not declared in dynamixel
#define Encoder_Port 0X32 //50 - 0X00 R/W - pin to read sensor encoder
#define Sensor_Resol_L 0X33 //51 - 0X00 R/W - 10 bit sensor resolution LOW
#define Sensor_Resol_H 0X34 //52 - 0X00 R/W - 10 bit sensor resolution HIGH
#define Max_PWM_Value_L 0X35 //53 - 0X00 R/W - 10 bit max PWM value output LOW
#define Max_PWM_Value_H 0X36 //54 - 0X00 R/W - 0 bit max PWM value output HIGH
#define Max_Sensor_Range_L 0X37 //55 - 0X00 R/W - working range of encoder 300 case ax12 motor LOW
#define Max_Sensor_Range_H 0x38 //56 - 0X00 R/W - working range of encoder 300 case ax12 motor HIGH
#define Error_Margin_L 0X39 //57 - 0X00 R/W - Range of error to discard LOW
#define Error_Margin_H 0X3A //58 - 0X00 R/W - Range of error to discard HIGH
#define Dead_Zone_L 0X3B //59 - 0X00 R/W - Min PWM to put motor in motion LOW
#define Dead_Zone_H 0X3C //60 - 0X00 R/W - Min PWM to put motor in motion HIGH
#define KP_L 0X3D //61 - 0X00 R/W - proportional constant LOW
#define KP_H 0X3E //62 - 0X00 R/W - proportional constant HIGH
#define KD_L 0X3F //63 - 0X00 R/W - Derivative constant LOW
#define KD_H 0X40 //64 - 0X00 R/W - Derivative constant HIGH
#define KI_L 0X41 //65 - 0X00 R/W - Integral constant LOW
#define KI_H 0X42 //66 - 0X00 R/W - Integral constant HIGH
#define Integral_Value_L 0X43 //67 - 0X00 R/W - integral value LOW
#define Integral_Value_H 0X44 //68 - 0X00 R/W - integral value HIGH
#define Max_Integral_Value_L 0X45 //69 - 0X00 R/W - Saturation Integral value LOW
#define Max_Integral_Value_H 0X46 //70 - 0X00 R/W - Saturation Integral value HIGH
#define Motor_Turns_L 0X47 //71 - 0X00 R/W - # of absolute turns LOW
#define Motor_Turns_H 0X48 //72 - 0X00 R/W - # of absolute turns HIGH
#define Error_Vector_0_L 0X49 //73 - 0X00 R/W - ecord of error for 4 time
#define Error_Vector_0_H 0X4A //74- 0X00 R/W - ecord of error for 4 time
#define Error_Vector_1_L 0X4B //75 - 0X00 R/W - record of error for 4 time
#define Error_Vector_1_H 0X4C //76 - 0X00 R/W - record of error for 4 time
#define Error_Vector_2_L 0X4D //77 - 0X00 R/W - record of error for 4 time
#define Error_Vector_2_H 0X4E //78 - 0X00 R/W - record of error for 4 time
#define Error_Vector_3_L 0X4F //79 - 0X00 R/W - record of error for 4 time
#define Error_Vector_3_H 0X50 //80 - 0X00 R/W - record of error for 4 time
#define Time_Period_L 0X51 //81 - 0X00 R/W - delta time in between ctrl cycles LOW
#define Time_Period_H 0X52 //82 - 0X00 R/W - delta time in between ctrl cycles HIGH
#define Sensor_Value_L 0X53 //83 - 0X00 R/W - fraction of turn value return by encoder LOW
#define Sensor_Value_H 0X54 //84 - 0X00 R/W - fraction of turn value return by encoder HIGH
#define Motor_Accion_L 0X55 //85 - 0X00 R/W - PWM on Motor LOW
#define Motor_Accion_H 0X56 //86 - 0X00 R/W - PWM on Motor HIGH
#define Transition_Stage 0X57 //87 - 0X00 R/W - Position inside transition stage between turns
// 0 Transition_Stage = -3 in stransition gap sensor value = 0 came into gap CW direction
// 1 Transition_Stage = -2 in stransition gap sensor value =~ 1023 came into gap CW direction
// 2 Transition_Stage = -1 in stransition gap sensor value = 1023 came into gap CW direction
// 3 Transition_Stage = 0 out of stransition gap
// 4 Transition_Stage = 1 in stransition gap sensor value = 1023 came into gap CCW direction
// 5 Transition_Stage = 2 in stransition gap sensor value =~ 1023 came into gap CCW direction
// 6 Transition_Stage = 3 in stransition gap sensor value = 0 came into gap CCW direction
#define low(x) ((uint8_t)((x) & 0xFF))
#define high(x) ((uint8_t)(((x)>>8) & 0xFF))
#define TwoBytesToInt(h,l) ((int16_t)(((h)<<8)|(l)))
extern uint8_t eepromVAR[];
extern uint8_t sramVAR[];
// function to read variable from emprom
uint8_t Read_byte_Dynamixel_EEPROM(uint8_t IDregister);
// function to read variable to emprom
void Write_byte_Dynamixel_EEPROM(uint8_t IDregister, uint8_t Value);
// function to read variable from RAM
uint8_t Read_byte_Dynamixel_RAM(uint8_t IDregister);
// function to write variable to RAM
void Write_byte_Dynamixel_RAM(uint8_t IDregister, uint8_t Value);
// function to read variable from emprom
int16_t Read_word_Dynamixel_EEPROM(uint8_t IDregister);
// function to read variable to emprom
void Write_word_Dynamixel_EEPROM(uint8_t IDregister, int16_t Value);
// function to read variable from RAM
int16_t Read_word_Dynamixel_RAM(uint8_t IDregister);
// function to write variable to RAM
void Write_word_Dynamixel_RAM(uint8_t IDregister, int16_t Value);
// Function restores factory values in emprom
void Restore_Eeprom_Factory_Values(void);
//--------ini vars eeprom - write eepromVAR values from EEPROM------
void Restore_EepromVAR(void);
#endif
servo_firmware/rx28/include/TXRX_Dynamixel.h 0 → 100755
+ 82
− 0
View file @ ab2b18cc
#ifndef _TXRX_DYNAMIXEL_H
#define _TXRX_DYNAMIXEL_H
// receive buffers length
#define RX_BUFFER_SIZE 256
// Instruction identifiers
#define INST_PING 0x01
#define INST_READ 0x02
#define INST_WRITE 0x03
#define INST_REG_WRITE 0x04
#define INST_ACTION 0x05
#define INST_RESET 0x06
#define INST_DIGITAL_RESET 0x07
#define INST_SYSTEM_READ 0x0C
#define INST_SYSTEM_WRITE 0x0D
#define INST_SYNC_WRITE 0x83
#define INST_SYNC_REG_WRITE 0x84
// bus errors
#define INSTRUCTION_ERROR 0x40
#define OVERLOAD_ERROR 0x20
#define CHECKSUM_ERROR 0x10
#define RANGE_ERROR 0x08
#define OVERHEATING_ERROR 0x04
#define ANGLE_LIMIT_ERROR 0x02
#define VOLTAGE_ERROR 0x01
#define NO_ERROR 0x00
// instructions to configure the data direction on the RS485 interface
#define SET_RS485_TXD PORTD &= 0xBF,PORTD |= 0x80
#define SET_RS485_RXD PORTD &= 0x7F,PORTD |= 0x40
// broadcasting address
#define BROADCASTING_ID 0xFE
// possible packet status
#define CORRECT 0
#define INCOMPLETE 1
#define CHECK_ERROR 2
#define DATA 3
#define NO_DATA 4
extern uint8_t rs485_address;
typedef enum {dyn_header1,dyn_header2,dyn_id,dyn_length,dyn_inst,dyn_params,dyn_checksum} dyn_states;
// function to initialize the RS485 interface
void init_RS485(void);
// function to assign UBRRL value from baud_rate value
void Baud_Rate_Value(void);
// function to set the rs485 device address
void get_RS485_address(unsigned char *address);
// function to send a packet through the RS485 interface
void TxRS485Packet(unsigned char id, unsigned char instruction, unsigned char param_len, unsigned char *params);
// function to receive a packet through the RS485 interface using interrupts
unsigned char RxRS485Packet(unsigned char *id, unsigned char *instruction, unsigned char *param_len, unsigned char *params);
//****************** W R I T E info send by pc ******************
// case EEMPROM and BYTE
void TxRx_Write_byte_Dynamixel_EEPROM(uint8_t IDregister, uint8_t *data, uint8_t length);
// case RAM and BYTE
void TxRx_Write_byte_Dynamixel_RAM(uint8_t IDregister, uint8_t *data, uint8_t length);
// case EEMPROM and WORD
void TxRx_Write_word_Dynamixel_EEPROM(uint8_t IDregister, uint8_t *data, uint8_t length);
// case RAM and WORD
void TxRx_Write_word_Dynamixel_RAM(uint8_t IDregister, uint8_t *data, uint8_t length);
// ************* R E A D and send info to pc ***************
// case EEMPROM and BYTE
void TxRx_Read_byte_Dynamixel_EEPROM(uint8_t IDregister, uint8_t *answer);
// case RAM and BYTE
void TxRx_Read_byte_Dynamixel_RAM(uint8_t IDregister, uint8_t *answer);
// case EEMPROM and WORD
void TxRx_Read_word_Dynamixel_EEPROM(uint8_t IDregister, uint8_t *answer);
// case RAM and WORD
void TxRx_Read_word_Dynamixel_RAM(uint8_t IDregister, uint8_t *answer);
#endif
servo_firmware/rx28/src/CFG_HW_Dynamixel.c 0 → 100755
+ 91
− 0
View file @ ab2b18cc
#include <avr/io.h>
#include <util/delay.h>
#include <avr/interrupt.h>
#include "CFG_HW_Dynamixel.h"
// LED CONTROL
/* void blinkLedN(uint8_t j) // this is for debuging tonggle i times
{
uint8_t i;
LedOFF; // led OFF
_delay_ms(500);
for (i=1; i<=(2*j); i++) {
LedTOGGLE; // tonggle led
_delay_ms(200);
}
LedOFF; // led OFF
_delay_ms(500);
}*/
void Config_Hardware(void)
{
DDRD |= (1 << LED_PIN); // Set LED as output
LedON; // ON the LED
DDRB |= (1 << CW_PWM_MOTOR_PIN); // Set CW pin as output
SET_CW_PWM_MOTOR(0x0000); //PB1 => set PWM for 0% duty cycle @ 10bit
DDRB |= (1 << CCW_PWM_MOTOR_PIN); // Set CCW pin as output
SET_CCW_PWM_MOTOR(0x0000); //PB2 => set PWM for 0% duty cycle @ 10bit
DDRD |= (1 << ENABLE_MOTOR_PIN); // Set Enable pin as output
DISABLE_MOTOR; // disable motor
cli(); // just to make sure
//*****************************************
// ini timer/counter0
/*
7 bit 6 bit 5 bit 4 bit 3 bit 2 bit 1 bit 0 bit
TCCR0 FOC0 - - - - CS02 CS01 CS00
Timer/Counter Control Register 0
CS02 CS01 CS00 DESCRIPTION
0 0 0 Timer/Counter0 Disabled
0 0 1 No Prescaling
0 1 0 Clock / 8
0 1 1 Clock / 64
1 0 0 Clock / 256
1 0 1 Clock / 1024*/
// reloj 20 mhz / 4 instruciones 5000000
// 5000000/256/prescaler
// no pre => interrupt => 0.0512 ms
// 8 => interrupt => 0.4096 ms
// 64 => interrupt => 3.2768 ms
// 256 => interrupt => 13.1072 ms
// 1024 => interrupt => 52.4288 ms
//TCCR0 |= (1<<CS00) | (1<<CS01); // clk src with prescaler 64
TCCR0 |= (1<<CS02) ; // clk src with prescaler 256
//TCCR0 |= (1<<CS00) ; // clk src with no prescaler
TIMSK |= ( 1 << TOIE0); // Enable interrupt timer 0
/* timer/counter0 limitations.. no CTC mode.
* so move starting point to where neded
* to do is add X */
// TCNT0 = 0x9C; // Timer/Counter Register 156 => 0.1 ms counter value set
//*****************************************
// ini timer/counter1
TCCR1A |= (1 << COM1A1); // set none-inverting mode PB1
TCCR1A |= (1 << COM1B1); // set none-inverting mode PB2
// set Mode Fast PWM, 10bit
TCCR1A |= (1 << WGM11) | (1 << WGM10); // set Mode Fast PWM, 10bit
TCCR1B |= (1 << WGM12) ; // set Mode Fast PWM, 10bit
/* set prescaler to and starts PWM
CS12 CS11 CS10 DESCRIPTION
0 0 0 Timer/Counter2 Disabled
0 0 1 No Prescaling
0 1 0 Clock / 8
0 1 1 Clock / 64
1 0 0 Clock / 256
1 0 1 Clock / 1024*/
TCCR1B |= (1 << CS11) | (1 << CS10);
sei(); // enable all interrupts
}
servo_firmware/rx28/src/CTRL_Dynamixel.c 0 → 100755
+ 229
− 0
View file @ ab2b18cc
#include <avr/interrupt.h>
#include <avr/io.h>
#include "MEM_Dynamixel.h"
#include "CTRL_Dynamixel.h"
#define INT16_MAX 0x7fff
#define INT16_MIN (-INT16_MAX - 1)
#define ADD_A_B(c,a,b) ({\
int32_t x = (int32_t)a+b; \
(x>32767)?(c = (int16_t)32767) : ((x<-32768)?(c = (int16_t)-32768):(c = (int16_t) x)); \
})
#define MULT_A_B(c,a,b) ({\
int32_t x = (int32_t)a*b; \
(x>32767)?(c = (int16_t)32767) : ((x<-32768)?(c = (int16_t)-32768):(c = (int16_t) x)); \
})
#define num_to_FFFFbit(c,a) ({\
double x = (double)a; \
(x>32767)?(c = (int16_t)32767) : ((x<-32768)?(c = (int16_t)-32768):(c = (int16_t) x)); \
})
void Ini_Position(void)
{
int16_t Sensor_Value;
// update position sensor value
Sensor_Value=Read_Sensor(CTRL_Encoder_Port);
Write_word_Dynamixel_RAM(Sensor_Value_L,Sensor_Value);
Write_word_Dynamixel_RAM(Present_Position_L,Sensor_Value);
Write_word_Dynamixel_RAM(Goal_Position_L,Sensor_Value);
Write_word_Dynamixel_RAM(Motor_Turns_L,0);
Write_byte_Dynamixel_RAM(Torque_Enable,1);
}
int16_t Read_Sensor(uint8_t port)
{
int16_t ADCval;
ADMUX = port; // use #port ADC
ADMUX |= (1 << REFS0); // use AVcc as the reference
ADMUX &= ~(1 << ADLAR); // clear for 10 bit resolution
ADCSRA |= (1 << ADPS2) | (1 << ADPS1) | (1 << ADPS0); // 128 prescale for 8Mhz
ADCSRA |= (1 << ADEN); // Enable the ADC
ADCSRA |= (1 << ADSC); // Start the ADC conversion
while(ADCSRA & (1 << ADSC)); // waits for the ADC to finish
ADCval = ADCL;
ADCval = (ADCH << 8) + ADCval; // ADCH is read so ADC can be updated again
return ADCval;
}
void Control_Cycle(void)
{
int16_t Motor_Turns,Sensor_Value,Present_Position,Goal_Position;
//int16_t Motor_Accion,MotorDir,Error_Vector_0,Error_Vector_1,Error_Vector_2,Error_Vector_3;
int16_t Motor_Accion,MotorDir,Error_Vector_0,Error_Vector_1,Error_Vector_2;
//int16_t Integral_Value,Max_Integral_Value,Error_Margin,KP,KD,KI,Sensor_Resol, Max_Sensor_Range;
int16_t Error_Margin,KP,KD,Sensor_Resol, Max_Sensor_Range;
uint8_t Transition_Stage_Value;
int32_t Motor_Accion_P=0;
int32_t Motor_Accion_D=0;
//int32_t Motor_Accion_I=0;
// update position sensor value
Sensor_Value = Read_Sensor(CTRL_Encoder_Port);
Write_word_Dynamixel_RAM(Sensor_Value_L,Sensor_Value);
// find direction of accion
Motor_Accion = Read_word_Dynamixel_RAM(Motor_Accion_L);
if (Motor_Accion<CTRL_ZERO){MotorDir=CTRL_Dir_CW;}
else{MotorDir=CTRL_Dir_CCW;}
// update motor turns
Transition_Stage_Value = Read_byte_Dynamixel_RAM(Transition_Stage);
Motor_Turns=Read_word_Dynamixel_RAM(Motor_Turns_L);
switch(Transition_Stage_Value) {
case 0: //-3
if (Sensor_Value!=1023){
if (MotorDir==CTRL_Dir_CW){ Transition_Stage_Value=3; //0
Motor_Turns=Motor_Turns-1;
}else { Transition_Stage_Value=1;} //-2
}
break; //...............................................
case 1: //-2
if (Sensor_Value==0){ Transition_Stage_Value=2; //-1
}else if (Sensor_Value==1023){ Transition_Stage_Value=0;} //-3
break; //...............................................
case 2: //-1
if (Sensor_Value!=0){
if (MotorDir==CTRL_Dir_CW){ Transition_Stage_Value=1; //-2
}else { Transition_Stage_Value=3;} //0
}
break; //...............................................
case 3: //0
if (Sensor_Value==0) { Transition_Stage_Value=2; //-1
}else if (Sensor_Value==1023) { Transition_Stage_Value=4;} //1
break; //...............................................
case 4: //1
if (Sensor_Value!=1023){
if (MotorDir==CTRL_Dir_CCW){ Transition_Stage_Value=5; //2
}else { Transition_Stage_Value=3;} //0
}
break; //...............................................
case 5: //2
if (Sensor_Value==1023){ Transition_Stage_Value=4; //1
}else if (Sensor_Value==0){ Transition_Stage_Value=6;} //3
break; //...............................................
case 6: //3
if (Sensor_Value!=0){
if (MotorDir==CTRL_Dir_CCW){ Transition_Stage_Value=3; //0
Motor_Turns=Motor_Turns+1;
}else { Transition_Stage_Value=5;} //2
}
break; //...............................................
default:
;
break; //...............................................
}
Write_word_Dynamixel_RAM(Motor_Turns_L,Motor_Turns);
Write_byte_Dynamixel_RAM(Transition_Stage,Transition_Stage_Value);
// calc Position including motor turns
Sensor_Resol=Read_word_Dynamixel_RAM(Sensor_Resol_L);
Max_Sensor_Range=Read_word_Dynamixel_RAM(Max_Sensor_Range_L);
Present_Position = (int16_t) ((double)Motor_Turns*Sensor_Resol*360/ Max_Sensor_Range); // OJO max 26 vueltas
ADD_A_B(Present_Position,Present_Position,Sensor_Value);
Write_word_Dynamixel_RAM(Present_Position_L,Present_Position);
// Update Error Vector 3
//Error_Vector_3=Read_word_Dynamixel_RAM(Error_Vector_2_L);
//Write_word_Dynamixel_RAM(Error_Vector_3_L,Error_Vector_3);
// Update Error Vector 2
Error_Vector_2=Read_word_Dynamixel_RAM(Error_Vector_1_L);
Write_word_Dynamixel_RAM(Error_Vector_2_L,Error_Vector_2);
// Update Error Vector 1
Error_Vector_1=Read_word_Dynamixel_RAM(Error_Vector_0_L);
Write_word_Dynamixel_RAM(Error_Vector_1_L,Error_Vector_1);
// Update Error Vector 0
Goal_Position=Read_word_Dynamixel_RAM(Goal_Position_L);
MULT_A_B(Present_Position,Present_Position,-1);
ADD_A_B(Error_Vector_0,Goal_Position,Present_Position);
Write_word_Dynamixel_RAM(Error_Vector_0_L,Error_Vector_0);
// range where small errors are discarted
Error_Margin = Read_word_Dynamixel_RAM(Error_Margin_L);
if (Error_Vector_0<Error_Margin){
if (Error_Vector_0>(-1*Error_Margin)){ Write_word_Dynamixel_RAM(Motor_Accion_L,0);
return; }
}
// CTRL_Law - algorithm http://lorien.ncl.ac.uk/ming/digicont/digimath/dpid1.htm
KP = Read_word_Dynamixel_RAM(KP_L);
Motor_Accion_P = (int32_t)Error_Vector_0*KP/10;
KD = Read_word_Dynamixel_RAM(KD_L);
Motor_Accion_D= ((int32_t)Error_Vector_0- Error_Vector_1*2 + Error_Vector_2 )*KD/100;
//KI = Read_word_Dynamixel_RAM(KI_L);
//Integral_Value = Read_word_Dynamixel_RAM(Integral_Value_L);
//ADD_A_B(Integral_Value,Integral_Value,Error_Vector_0);
//Motor_Accion_I=(int32_t)Integral_Value*KI/100;
// saturate Integral accion
//Max_Integral_Value = Read_word_Dynamixel_RAM(Max_Integral_Value_L);
//if (Motor_Accion_I>Max_Integral_Value){Motor_Accion_I=Max_Integral_Value;}
//else if (Motor_Accion_I<(-Max_Integral_Value)){Motor_Accion_I=-Max_Integral_Value;}
//ADD_A_B(Motor_Accion,Motor_Accion_I,Motor_Accion_D);
//ADD_A_B(Motor_Accion,Motor_Accion,Motor_Accion_P);
ADD_A_B(Motor_Accion,Motor_Accion_D,Motor_Accion_P);
//Write_word_Dynamixel_RAM(Integral_Value_L,Integral_Value);
Write_word_Dynamixel_RAM(Motor_Accion_L,Motor_Accion);
}
void HW_Security(void)
{
; // TODO revisar corriente y temperatura
}
void Write_Actuator(void)
{
uint16_t PWM_Value;
int16_t MotorDir,Motor_Accion,Dead_Zone,Max_PWM_Value,Temp;
Motor_Accion = Read_word_Dynamixel_RAM(Motor_Accion_L);
// find direction of accion
if (Motor_Accion<CTRL_ZERO){MotorDir=CTRL_Dir_CW;}
else{MotorDir=CTRL_Dir_CCW;}
// add motor deadzone to Motor accion if not zero
// and if needed Saturate accion to max PWM_Value
Dead_Zone=Read_word_Dynamixel_RAM(Dead_Zone_L);
Max_PWM_Value= Read_word_Dynamixel_RAM(Max_PWM_Value_L);
if (Motor_Accion==CTRL_ZERO)
{PWM_Value=CTRL_ZERO;}
else{
MULT_A_B(Temp,Motor_Accion,MotorDir);
ADD_A_B(Temp,Temp,Dead_Zone);
if ( Temp > Max_PWM_Value)
{ PWM_Value = Max_PWM_Value;} // Saturate accion to max PWM_Value
else
{ PWM_Value = (uint16_t)Temp;}// add motor deadzone to Motor accion
}
// place PWM value on respective pin
if (PWM_Value>CTRL_ZERO){
if (MotorDir==CTRL_Dir_CCW){
OCR1B = CTRL_ZERO; //PB2 => set PWM for Y% duty cycle
OCR1A = PWM_Value; //PB1 => set PWM for X% duty cycle
}else{
OCR1A = CTRL_ZERO; //PB1 => set PWM for X% duty cycle
OCR1B = PWM_Value; //PB2 => set PWM for Y% duty cycle
}
} else {
OCR1A = CTRL_ZERO; //PB1 => set PWM for X% duty cycle
OCR1B = CTRL_ZERO; //PB2 => set PWM for Y% duty cycle
}
}
servo_firmware/rx28/src/MEM_Dynamixel.c 0 → 100755
+ 176
− 0
View file @ ab2b18cc
#include <avr/interrupt.h>
#include <avr/io.h>
#include <util/delay.h>
#include "TXRX_Dynamixel.h"
#include "MEM_Dynamixel.h"
#include <avr/eeprom.h>
// Function restore factory values in eeprom
void Restore_Eeprom_Factory_Values(void) {
eeprom_write_byte((uint8_t*)Model_Number_L,0X0C); // Model_Number_L 0X00 //0 - 0X0C R - Lowest byte of model number
eeprom_write_byte((uint8_t*)Model_Number_H,0X00); // Model_Number_H 0X01 //1 - 0X00 R - Highest byte of model number
eeprom_write_byte((uint8_t*)Version_Firmware,0X00); // Version_Firmware 0X02 //2 - 0X00 R - Information on the version of firmware
eeprom_write_byte((uint8_t*)ID,0X01); // ID 0X03 //3 - 0X03 R/W - ID of Dynamixel
eeprom_write_byte((uint8_t*)Baud_Rate,0x22); // Baud_Rate 0X04 //4 - 0X01 R/W - Baud Rate of Dynamixel
eeprom_write_byte((uint8_t*)Return_Delay_Time,0XFA); // Return_Delay_Time 0X05 //5 - 0XFA R/W - Return Delay Time
eeprom_write_byte((uint8_t*)CW_Angle_Limit_L,0X00); // CW_Angle_Limit_L 0X06 //6 - 0X00 R/W - Lowest byte of clockwise Angle Limit
eeprom_write_byte((uint8_t*)CW_Angle_Limit_H,0X00); // CW_Angle_Limit_H 0X07 //7 - 0X00 R/W - Highest byte of clockwise Angle Limit
eeprom_write_byte((uint8_t*)CCW_Angle_Limit_L,0XFF); // CCW_Angle_Limit_L 0X08 //8 - 0XFF R/W - Lowest byte of counterclockwise Angle Limit
eeprom_write_byte((uint8_t*)CCW_Angle_Limit_H,0X03); // CCW_Angle_Limit_H 0X09 //9 - 0X03 R/W - Highest byte of counterclockwise Angle Limit
eeprom_write_byte((uint8_t*)Int_Limit_Temperature,0X46); // Int_Limit_Temperature 0X0B //11 - 0X46 R/W - Internal Highest Limit Temperature
eeprom_write_byte((uint8_t*)Low_Limit_Voltage,0X3C); // Low_Limit_Voltage 0X0C //12 - 0X3C R/W - the Lowest Limit Voltage
eeprom_write_byte((uint8_t*)High_Limit_Voltage,0XBE); // High_Limit_Voltage 0X0D //13 - 0XBE R/W - the Highest Limit Voltage
eeprom_write_byte((uint8_t*)Max_Torque_L,0XFF); // Max_Torque_L 0X0E //14 - 0XFF R/W - Lowest byte of Max. Torque
eeprom_write_byte((uint8_t*)Max_Torque_H,0X03); // Max_Torque_H 0X0F //15 - 0X03 R/W - Highest byte of Max. Torque
eeprom_write_byte((uint8_t*)Status_Return_Level,0X02); // Status_Return_Level 0X10 //16 - 0X02 R/W - Status Return Level
eeprom_write_byte((uint8_t*)Alarm_LED,0x24); // Alarm_LED 0X11 //17 - 0x24 R/W - LED for Alarm
eeprom_write_byte((uint8_t*)Alarm_Shutdown,0x24); // Alarm_Shutdown 0X12 //18 - 0x24 R/W - Shutdown for Alarm
eeprom_write_byte((uint8_t*)Gate_Restore_Eeprom,0xCC); // 10 0x01 R/W - Gate variable to restores eeprom factory values
}
//--------ini vars eeprom - write eepromVAR values from EEPROM------
//********** ini vars eeprom - se usa un array pero deberia ir a eeprom *****
uint8_t eepromVAR[20];
void Restore_EepromVAR(void) {
eeprom_read_block((void *)&eepromVAR,(const void*)Model_Number_L,20);
}
uint8_t sramVAR[70]={ // Register Id RAM - declared in dynamixel
0x01, // Torque_Enable 0X18 //24 - 0x00 R/W - Torque On/Off
0x00, // LED 0X19 //25 - 0x00 R/W - LED On/Off
0X01, // CW_Compliance_Margin 0X1A //26 - 0X01 R/W - CW Compliance margin
0X01, // CCW_Compliance_Margin 0X1B //27 - 0X01 R/W - CCW Compliance margin
0X20, // CW_Compliance_Slope 0X1C //28 - 0X20 R/W - CW Compliance slope
0X20, // CCW_Compliance_Slope 0X1D //29 - 0X20 R/W - CCW Compliance Slope
0x00, // Goal_Position_L 0X1E //30 - 0x00 R/W - Lowest byte of Goal Position
0x00, // Goal_Position_H 0X1F //31 - 0x00 R/W - Highest byte of Goal Position
0xF0, // Moving_Speed_L 0X20 //32 - 0xF0 R/W - Lowest byte of Moving Speed
0x0F, // Moving_Speed_H 0X21 //33 - 0x0F R/W - Highest byte of Moving Speed
0xF0, // Torque_Limit_L 0X22 //34 - 0xF0 R/W - Lowest byte of Torque Limit
0x0F, // Torque_Limit_H 0X23 //35 - 0x0F R/W - Highest byte of Torque Limit
0x00, // Present_Position_L 0X24 //36 - 0x00 R - Lowest byte of Current Position
0x00, // Present_Position_H 0X25 //37 - 0x00 R - Highest byte of Current Position
0x00, // Present_Speed_L 0X26 //38 - 0x00 R - Lowest byte of Current Speed
0x00, // Present_Speed_H 0X27 //39 - 0x00 R - Highest byte of Current Speed
0x00, // Present_Load_L 0X28 //40 - 0x00 R - Lowest byte of Current Load
0x00, // Present_Load_H 0X29 //41 - 0x00 R - Highest byte of Current Load
0x00, // Present_Voltage 0X2A //42 - 0x00 R - Current Voltage
0x00, // Present_Temperature 0X2B //43 - 0x00 R - Current Temperature
0x00, // Registered 0X2C //44 - 0x00 R - Means if Instruction is registered
0X00, // not use 0X2D //45 - 0X00 R/W -
0x00, // Moving 0X2E //46 - 0X00 R - Means if there is any movement
0x00, // Lock 0X2F //47 - 0x00 R/W - Locking EEPROM
0x20, // Punch_L 0X30 //48 - 0X20 R/W - Lowest byte of Punch
0x00, // Punch_H 0X31 //49 - 0X00 R/W - Highest byte of Punch
// Register Id RAM - not declared in dynamixel
0x00, // Encoder_Port; 0X32 //50 - 0X00 R/W - pin to read sensor
low(1024), // Sensor_Resol_L; 0X33 //51 - 0X00 R/W - 10 bit sensor resolution LOW
high(1024), // Sensor_Resol_H; 0X34 //52 - 0X00 R/W - 10 bit sensor resolution HIGH
low(700), // Max_PWM_Value_L; 0X35 //53 - 0X00 R/W - 10 bit max PWM value output LOW
high(700), // Max_PWM_Value_H; 0X36 //54 - 0X00 R/W - 0 bit max PWM value output HIGH
low(332), // Max_Sensor_Range_L; 0X37 //55 - 0X00 R/W - working range of encoder 300 case ax12 motor LOW
high(332), // Max_Sensor_Range_H; 0X38 //56 - 0X00 R/W - working range of encoder 300 case ax12 motor HIGH
low(5), // Error_Margin_L; 0X39 //57 - 0X00 R/W - Range of error to discard LOW
high(5), // Error_Margin_H; 0X3A //58 - 0X00 R/W - Range of error to discard HIGH
low(10), // Dead_Zone_L; 0X3B //59 - 0X00 R/W - Min PWM to put motor in motion LOW
high(10), // Dead_Zone_H; 0X3C //60 - 0X00 R/W - Min PWM to put motor in motion HIGH
low(50), // KP_L; 0X3D //61 - 0X00 R/W - proportional constant LOW
high(50), // KP_H; 0X3E //62 - 0X00 R/W - proportional constant HIGH
low(108), // KD_L; 0X3F //63 - 0X00 R/W - Derivative constant LOW
high(108), // KD_H; 0X40 //64 - 0X00 R/W - Derivative constant HIGH
low(0), // KI_L; 0X41 //65 - 0X00 R/W - Integral constant LOW
high(0), // KI_H; 0X42 //66 - 0X00 R/W - Integral constant HIGH
0x00, // Integral_Value_L; 0X43 //67 - 0X00 R/W - integral value LOW
0x00, // Integral_Value_H; 0X44 //68 - 0X00 R/W - integral value HIGH
low(500), // Max_Integral_Value_L; 0X45 //69 - 0X00 R/W - Saturation Integral value LOW
high(500), // Max_Integral_Value_H; 0X46 //70 - 0X00 R/W - Saturation Integral value HIGH
0x00, // Motor_Turns_L; 0X47 //71 - 0X00 R/W - # of absolute turns LOW
0x00, // Motor_Turns_H; 0X48 //72 - 0X00 R/W - # of absolute turns HIGH
0xFF, // Error_Vector_0_L; 0X49 //73 - 0X00 R/W - record of error for 4 time _L
0xEF, // Error_Vector_0_H; 0X4A //74 - 0X00 R/W - record of error for 4 time _H
0x00, // Error_Vector_1_L; 0X4B //75 - 0X00 R/W - record of error for 4 time _L
0x00, // Error_Vector_1_H; 0X4C //76 - 0X00 R/W - record of error for 4 time _H
0xFF, // Error_Vector_2_L; 0X4D //77 - 0X00 R/W - record of error for 4 time _L
0xEF, // Error_Vector_2_H; 0X4E //78 - 0X00 R/W - record of error for 4 time _H
0x00, // Error_Vector_3_L; 0X4F //79 - 0X00 R/W - record of error for 4 time _L
0x00, // Error_Vector_3_H; 0X50 //80 - 0X00 R/W - record of error for 4 time _H
low(1000), // Time_Period_L; 0X51 //81 - 0X00 R/W - delta time in between ctrl cycles LOW
high(1000), // Time_Period_H; 0X52 //82 - 0X00 R/W - delta time in between ctrl cycles HIGH
0x00, // Sensor_Value_L; 0X53 //83 - 0X00 R/W - fraction of turn value return by encoder LOW
0x00, // Sensor_Value_H; 0X54 //84 - 0X00 R/W - fraction of turn value return by encoder HIGH
0x00, // Motor_Accion_L; 0X55 //85 - 0X00 R/W - PWM on Motor LOW
0x00, // Motor_Accion_H; 0X56 //86 - 0X00 R/W - PWM on Motor HIGH
0x03 // Transition_Stage; 0X57 //87 - 0X00 R/W - Position inside transition stage between turns
};
// EEPROM READ AND WRITE
uint8_t Read_byte_Dynamixel_EEPROM(uint8_t IDregister)
{
//uint8_t temp;
//temp= memoria[IDregister];
//return temp;
return eepromVAR[IDregister];
}
void Write_byte_Dynamixel_EEPROM(uint8_t IDregister,uint8_t Value )
{
uint8_t * tempptr;
tempptr=(uint8_t *)IDregister;
eepromVAR[IDregister]=Value;
eeprom_write_byte(tempptr,Value);
}
int16_t Read_word_Dynamixel_EEPROM(uint8_t IDregister)
{
int16_t temp;
temp= (int16_t)(TwoBytesToInt(eepromVAR[IDregister+1],eepromVAR[IDregister]));
return temp;
}
void Write_word_Dynamixel_EEPROM(uint8_t IDregister,int16_t Value )
{
uint8_t temp;
uint8_t * tempptr;
tempptr=(uint8_t *)IDregister;
temp= (uint8_t)(low(Value));
eepromVAR[IDregister]=temp;
eeprom_write_byte(tempptr,temp);
tempptr=(uint8_t *)(IDregister+1);
temp= (uint8_t)(high(Value));
eepromVAR[IDregister+1]=temp;
eeprom_write_byte(tempptr,temp);
}
// RAM READ AND WRITE
uint8_t Read_byte_Dynamixel_RAM(uint8_t IDregister)
{
//uint8_t temp;
//temp= sramVAR[IDregister-RAM_ID_correction];
//return temp;
return sramVAR[IDregister-RAM_ID_correction];
}
void Write_byte_Dynamixel_RAM(uint8_t IDregister,uint8_t Value )
{
sramVAR[IDregister-RAM_ID_correction]=Value;
}
int16_t Read_word_Dynamixel_RAM(uint8_t IDregister)
{
int16_t temp;
temp= (int16_t)(TwoBytesToInt(sramVAR[IDregister-RAM_ID_correction+1],sramVAR[IDregister-RAM_ID_correction]));
return temp;
}
void Write_word_Dynamixel_RAM(uint8_t IDregister,int16_t Value )
{
sramVAR[IDregister-RAM_ID_correction]=low(Value);
sramVAR[IDregister-RAM_ID_correction+1]=high(Value);
}
servo_firmware/rx28/src/TXRX_Dynamixel.c 0 → 100755
+ 317
− 0
View file @ ab2b18cc
#include <avr/interrupt.h>
#include <avr/io.h>
#include <util/delay.h>
#include "TXRX_Dynamixel.h"
#include "MEM_Dynamixel.h"
#define NULL (void *)0x00000000
uint8_t rs485_address;
// buffer variables
volatile unsigned char buffer_data[256];
volatile unsigned char read_ptr;
volatile unsigned char write_ptr;
volatile unsigned char num_data;
unsigned char RS485_is_tx_ready(void)
{
return (UCSRA & (1<<UDRE));
}
void RS485_set_tx_done(void)
{
// USART Control and Status Register A – UCSRA
// Bit 6 – TXC: USART Transmit Complete
UCSRA|=0x40;//sbi(UCSRA,6);
}
unsigned char RS485_is_tx_done(void)
{
// USART Control and Status Register A – UCSRA
// Bit 6 – TXC: USART Transmit Complete
return bit_is_set(UCSRA,6);
}
void RS485_send(unsigned char data)
{
RS485_set_tx_done();
while(!RS485_is_tx_ready());
UDR=data;
}
unsigned char RS485_receive(unsigned char *data)
{
cli();
if(num_data==0)
{
sei();
return 0;
}
else
{
*data=buffer_data[read_ptr];
read_ptr++;
num_data--;
sei();
return 1;
}
}
void init_RS485(void)
{
// configure the IO ports
// DDRD - Port D Data Direction Register
DDRD=DDRD|0xC2;// RX_en, TX_en, TX are outputs 0xC2=11000010
DDRD=DDRD&0xFE;// RX is an input 0xFE=11111110
// configure the ports to receive data
SET_RS485_RXD;
// initialize the rs485 ports
UCSRA = 0;// Single (not double) the USART transmission speed
UBRRH = 0;
//UBRRL = 0;// BAUD = 1000000 at U2X=0, Fosc=16MHz => UBRR=0 equivale ocion 1 matlab
//UBRRL = 16;// BAUD = 57600 at U2X=0, Fosc=16MHz => UBRR=0 equivale ocion 34 matlab
Baud_Rate_Value();
//blinkLedN(5);
UCSRB = (1<<RXEN)|(1<<TXEN)|(1<<RXCIE);
UCSRC = 0x86; // 8 bit data, no parity (and URSEL must be 1)
// init buffer
read_ptr=0;
write_ptr=0;
num_data=0;
}
// function to assign UBRRL value from baud_rate value
void Baud_Rate_Value(void)
{
//char temp=34;
//UBRRL = 1;// BAUD = 1000000 at U2X=0, Fosc=16MHz => UBRR=0 equivale ocion 1 matlab
//UBRRL = 16;// BAUD = 57600 at U2X=0, Fosc=16MHz => UBRR=0 equivale ocion 34 matlab
// Speed(BPS) = 2000000/(Data+1)
// Data Matlab // Data Dynamixel // Set BPS // Target BPS // Tolerance
// 1 // 0 // 1000000.0 // 1000000.0 // 0.000 %
// 3 // 1 // 500000.0 // 500000.0 // 0.000 %
// 4 // 2 // 400000.0 // 400000.0 // 33.000 %
// 7 // 3 // 250000.0 // 250000.0 // 0.000 %
// 9 // 4 // 200000.0 // 200000.0 // 0.000 %
// 16 // 8 // 117647.1 // 115200.0 // -2.124 %
// 34 // 16 // 57142.9 // 57600.0 // 0.794 %
// 103 // 51 // 19230.8 // 19200.0 // -0.160 %
// 207 // 103 // 9615.4 // 9600.0 // -0.160 %
// blinkLedN(1);
switch(eepromVAR[Baud_Rate])
//switch(temp)
{
case 1:
UBRRL=0;
break; //..................................................
case 3:
UBRRL=1;
break; //..................................................
case 4:
UBRRL=2;
break; //..................................................
case 7:
UBRRL=3;
break; //..................................................
case 9:
UBRRL=4;
break; //..................................................
case 16:
UBRRL=8;
break; //..................................................
case 34:
UBRRL=16;
break; //..................................................
case 103:
UBRRL=51;
break; //..................................................
case 207:
UBRRL=103;
break; //..................................................
default:
UBRRL=16;
eepromVAR[Baud_Rate]=34;
break; //................................................
}
//UBRRL = 16;
//blinkLedN(10);
}
void TxRS485Packet(unsigned char id, unsigned char instruction, unsigned char param_len, unsigned char *params)
{
unsigned char checksum=0,i=0;
SET_RS485_TXD;
RS485_send(0xFF);
RS485_send(0xFF);
RS485_send(id);
checksum+=id;
RS485_send(param_len+2);
checksum+=param_len+2;
RS485_send(instruction);
checksum+=instruction;
for(i=0;i<param_len;i++)
{
RS485_send(params[i]);
checksum+=params[i];
}
RS485_send(~checksum);
while(!RS485_is_tx_done());// wait until the data is sent
SET_RS485_RXD;
}
unsigned char RxRS485Packet(unsigned char *id,unsigned char *instruction, unsigned char *param_len, unsigned char *params)
{
static dyn_states state=dyn_header1;
static unsigned char checksum=0,read_params=0,identifier=0,inst=0,length=0,data[64];
unsigned char byte,i=0;
while(RS485_receive(&byte))
{
switch(state)
{
case dyn_header1: if(byte==0xFF)
state=dyn_header2;
else
state=dyn_header1;
break;
case dyn_header2: if(byte==0xFF)
{
state=dyn_id;
checksum=0;
}
else
state=dyn_header1;
break;
case dyn_id: identifier=byte;
checksum+=byte;
state=dyn_length;
break;
case dyn_length: length=byte-2;
checksum+=byte;
state=dyn_inst;
break;
case dyn_inst: inst=byte;
checksum+=byte;
if(length>0)
{
read_params=0;
state=dyn_params;
}
else
state=dyn_checksum;
break;
case dyn_params: data[read_params]=byte;
checksum+=byte;
read_params++;
if(read_params==length)
state=dyn_checksum;
else
state=dyn_params;
break;
case dyn_checksum: checksum+=byte;
if(checksum==0xFF)
{
*id=identifier;
*instruction=inst;
*param_len=length;
for(i=0;i<length;i++)
params[i]=data[i];
state=dyn_header1;
return CORRECT;
}
else
{
state=dyn_header1;
return CHECK_ERROR;
}
break;
}
}
return NO_DATA;
}
// UART ISR
ISR(USART_RXC_vect)
{
cli();
buffer_data[write_ptr]=UDR;
write_ptr++;
num_data++;
sei();
}
//****************** W R I T E info send by pc ******************
// case EEMPROM and BYTE
void TxRx_Write_byte_Dynamixel_EEPROM(uint8_t IDregister, uint8_t *dataA, uint8_t lengthA){
if (lengthA==2){
TxRS485Packet(rs485_address,NO_ERROR,0,NULL);
Write_byte_Dynamixel_EEPROM(IDregister,dataA[1]);
}else{
TxRS485Packet(rs485_address,INSTRUCTION_ERROR,0,NULL);
}
}
// case RAM and BYTE
void TxRx_Write_byte_Dynamixel_RAM(uint8_t IDregister, uint8_t *dataA, uint8_t lengthA){
if (lengthA==2){
TxRS485Packet(rs485_address,NO_ERROR,0,NULL);
Write_byte_Dynamixel_RAM(IDregister,dataA[1]);
}else{
TxRS485Packet(rs485_address,INSTRUCTION_ERROR,0,NULL);
}
}
// case EEMPROM and WORD
void TxRx_Write_word_Dynamixel_EEPROM(uint8_t IDregister, uint8_t *dataA, uint8_t lengthA){
if (lengthA==3){
TxRS485Packet(rs485_address,NO_ERROR,0,NULL);
Write_byte_Dynamixel_EEPROM(IDregister,dataA[1]);
Write_byte_Dynamixel_EEPROM(IDregister+1,dataA[2]);
}else{
TxRS485Packet(rs485_address,INSTRUCTION_ERROR,0,NULL);
}
}
// case RAM and WORD
void TxRx_Write_word_Dynamixel_RAM(uint8_t IDregister, uint8_t *dataA, uint8_t lengthA){
if (lengthA==3){
TxRS485Packet(rs485_address,NO_ERROR,0,NULL);
Write_byte_Dynamixel_RAM(IDregister,dataA[1]);
Write_byte_Dynamixel_RAM(IDregister+1,dataA[2]);
}else{
TxRS485Packet(rs485_address,INSTRUCTION_ERROR,0,NULL);
}
}
// ************* R E A D and send info to pc ***************
// case EEMPROM and BYTE
void TxRx_Read_byte_Dynamixel_EEPROM(uint8_t IDregister, uint8_t *answerA){
answerA[0]=Read_byte_Dynamixel_EEPROM(IDregister);
//answerA[0]=IDregister;
TxRS485Packet(rs485_address,NO_ERROR,1,answerA);
}
// case RAM and BYTE
void TxRx_Read_byte_Dynamixel_RAM(uint8_t IDregister, uint8_t *answerA){
answerA[0]=Read_byte_Dynamixel_RAM(IDregister);
//answerA[0]=IDregister;
TxRS485Packet(rs485_address,NO_ERROR,1,answerA);
}
// case EEMPROM and WORD
void TxRx_Read_word_Dynamixel_EEPROM(uint8_t IDregister, uint8_t *answerA){
answerA[0]=Read_byte_Dynamixel_EEPROM(IDregister);
answerA[1]=Read_byte_Dynamixel_EEPROM(IDregister+1);
//answerA[0]=IDregister;
//answerA[1]=0;
TxRS485Packet(rs485_address,NO_ERROR,2,answerA);
}
// case RAM and WORD
void TxRx_Read_word_Dynamixel_RAM(uint8_t IDregister, uint8_t *answerA){
answerA[0]=Read_byte_Dynamixel_RAM(IDregister);
answerA[1]=Read_byte_Dynamixel_RAM(IDregister+1);
TxRS485Packet(rs485_address,NO_ERROR,2,answerA);
}
servo_firmware/rx28/src/main.c 0 → 100755
+ 257
− 0
View file @ ab2b18cc
#include <avr/io.h>
#include <util/delay.h>
#include <avr/interrupt.h>
#include "TXRX_Dynamixel.h"
#include "CTRL_Dynamixel.h"
#include "MEM_Dynamixel.h"
#include "CFG_HW_Dynamixel.h"
#include <avr/eeprom.h>
#define NULL (void *)0x00000000
#define FALSE 0
#define TRUE 1
//*************CTRL = INTERRUPT FUNCTION *********************************
uint16_t count = 0;
uint16_t count2 = 0;
/*ISR( TIMER0_OVF_vect) {
int16_t TorqueEnable;
sei();
TorqueEnable = Read_byte_Dynamixel_RAM(Torque_Enable);
if (TorqueEnable == 1) {
//if (1){
//cli(); // disable all interrupts just to make sure
//TIMSK &= ~( 1 << TOIE0); // disable timer0
count++;
if (count == 2) {
// 1.3s passed
// LedTONGGLE();
Control_Cycle();
HW_Security();
Write_Actuator();
count = 0;
}
//TIMSK |= ( 1 << TOIE0); // enable timer0
//sei(); // enable all interrupts
} else {
OCR1A = CTRL_ZERO; //PB1 => set PWM for X% duty cycle
OCR1B = CTRL_ZERO; //PB2 => set PWM for Y% duty cycle
}
}*/
//**************** M A I N *********************************
int16_t main(void) {
unsigned char data[128], id, length, instruction, answer[2], status,
en_vector, i;
// list of read only registers - to exclude from write
/*unsigned char read_only_vector[30] = { 0, 1, 2, 36, 37, 38, 39, 40, 41, 42,
43, 44, 45, 46, 67, 68, 71, 72, 73, 74, 75, 76, 77, 78, 79, 80, 83,
84, 85, 86 };*/
// ini eeprom if first time after run reflash system
/*if (eeprom_read_byte((uint8_t*) Gate_Restore_Eeprom) != 0xCC) {
Restore_Eeprom_Factory_Values(); //once this procedure is held, no more initialization is performed
//blinkLedN(3);
}*/
//------EEPROM initial values-------------
/*Restore_EepromVAR();
//blinkLedN(5);
// get value of Motor ID
rs485_address = eepromVAR[ID];*/
// configure AVR chip i/o
Config_Hardware();
/*// asign actual position to goal position and asume zero motor turns
Ini_Position();
// initialize the RS485 interface
init_RS485();*/
// end of inicialization
LedOFF;
while (1) {
//LedOFF;
/*status = RxRS485Packet(&id, &instruction, &length, data);
if (status == CHECK_ERROR) {
TxRS485Packet(rs485_address, CHECKSUM_ERROR, 0, NULL);
} else if (status == CORRECT) {
//LedON;
if (id == rs485_address) {
switch (instruction) {
//***********************************************************
// P I N G
case INST_PING:
TxRS485Packet(rs485_address, NO_ERROR, 0, NULL);
break;
//*********************************************************
// R E A D and send info to pc
case INST_READ:
if (data[0] <= 18) { // if 0 <= IDinstruction <= 18 caso eeprom
if (data[0] >= 0) {
switch (data[1]) // tamaño del paquete que pide
{
case 1: // case BYTE
TxRx_Read_byte_Dynamixel_EEPROM(data[0],
answer);
break; //................................................
case 2: // case WORD
TxRx_Read_word_Dynamixel_EEPROM(data[0],
answer);
break; //................................................
default:
TxRS485Packet(rs485_address, INSTRUCTION_ERROR,
0, NULL);
break; //................................................
}
} else {
TxRS485Packet(rs485_address, INSTRUCTION_ERROR, 0,
NULL);
} // else id error
} else if (data[0] <= 86) { // else if 24 <= ID <= 86 ram
if (data[0] >= 24) {
switch (data[1]) { // tamaño del paquete que pide
case 1: // case BYTE
TxRx_Read_byte_Dynamixel_RAM(data[0], answer);
break; //................................................
case 2: // case WORD
//LedTOGGLE;
TxRx_Read_word_Dynamixel_RAM(data[0], answer);
break; //................................................
default:
TxRS485Packet(rs485_address, INSTRUCTION_ERROR,
0, NULL);
break; //................................................
}
} else {
TxRS485Packet(rs485_address, INSTRUCTION_ERROR, 0,
NULL);
} // else id error
} else if (data[0] == 87) { // else if ID = 87 ram
switch (data[1]) { // tamaño del paquete que pide
case 1: // case BYTE
TxRx_Read_byte_Dynamixel_RAM(data[0], answer);
break; //................................................
default:
TxRS485Packet(rs485_address, INSTRUCTION_ERROR, 0,
NULL);
break; //................................................
}
} else {
TxRS485Packet(rs485_address, INSTRUCTION_ERROR, 0,
NULL);
} // else id error
break;
//**********************************************************
// W R I T E info send by pc
case INST_WRITE:
en_vector = FALSE; // list of read only registers - to exclude from write
//unsigned char read_only_vector[30]={0,1,2,36,37,38,39,40,41,42,43,44,45,46,67,68,71,72,73,74,75,76,77,78,79,80,83,84,85,86};
for (i = 0; i < 30; i++) {
if (data[0] == read_only_vector[i]) {
en_vector = TRUE;
break;
}
}
if (en_vector == FALSE) {
if (data[0] <= 18) { // if 0 <= IDinstruction <= 18 caso eeprom
if (data[0] >= 0) {
if (Read_byte_Dynamixel_RAM(Lock) == 1) { // is eeprom write lock open
switch (length) // tamaño del paquete que pide
{
case 2: // case BYTE
TxRx_Write_byte_Dynamixel_EEPROM(
data[0], data, length);
break; //................................................
case 3: // case WORD
TxRx_Write_word_Dynamixel_EEPROM(
data[0], data, length);
break; //................................................
default:
TxRS485Packet(rs485_address,
INSTRUCTION_ERROR, 0, NULL);
break; //................................................
}
switch (data[0]) // aditional instructions for particular registers in eeprom
{
case ID: // case EEMPROM and BYTE
rs485_address = eepromVAR[ID];
break; //..................................................
case Baud_Rate: // case EEMPROM and BYTE
Baud_Rate_Value();
break; //..................................................
default:
;
break; //................................................
}
Write_byte_Dynamixel_RAM(Lock, 0); // close eeprom write lock
} else {
TxRS485Packet(rs485_address,
INSTRUCTION_ERROR, 0, NULL);
} // eeprom write lock closed
} else {
TxRS485Packet(rs485_address, INSTRUCTION_ERROR,
0, NULL);
} // else id error
} else if (data[0] <= 86) { // else if 24 <= ID <= 86 ram
if (data[0] >= 24) {
switch (length) { // tamaño del paquete que pide
case 2: // case BYTE
TxRx_Write_byte_Dynamixel_RAM(data[0], data,
length);
break; //................................................
case 3: // case WORD
TxRx_Write_word_Dynamixel_RAM(data[0], data,
length);
break; //................................................
default:
TxRS485Packet(rs485_address,
INSTRUCTION_ERROR, 0, NULL);
break; //................................................
}
switch (data[0]) { // aditional instructions for particular registers in ram
case LED: // case RAM and BYTE
if (Read_byte_Dynamixel_RAM(LED) == 1) {
LedON;
} else {
LedOFF;
}
break; //................................................
default:
;
break; //................................................
}
} else {
TxRS485Packet(rs485_address, INSTRUCTION_ERROR,
0, NULL);
} // else id error
} else if (data[0] == 87) { // else if ID = 87 ram // case BYTE
TxRx_Write_byte_Dynamixel_RAM(data[0], data,
length);
} else {
TxRS485Packet(rs485_address, INSTRUCTION_ERROR, 0,
NULL);
} // else id error
} else {
TxRS485Packet(rs485_address, INSTRUCTION_ERROR, 0,
NULL);
} // else in only read register list
break;
//**********************************************************
//********* D E F A U L T *****************************
default:
TxRS485Packet(rs485_address, INSTRUCTION_ERROR, 0, NULL);
break;
//***************************************************
}
}
}*/
}
}
servo_firmware/rx28/src/sergi dynamixel.c 0 → 100755
+ 107
− 0
View file @ ab2b18cc
#include <avr/interrupt.h>
#include <avr/io.h>
#include <util/delay.h>
#include "dynamixel.h"
unsigned char RS485_is_tx_ready(void)
{
return (UCSRA & (1<<UDRE));
}
void RS485_set_tx_done(void)
{
// USART Control and Status Register A – UCSRA
// Bit 6 – TXC: USART Transmit Complete
UCSRA|=0x40;//sbi(UCSRA,6);
}
unsigned char RS485_is_tx_done(void)
{
// USART Control and Status Register A – UCSRA
// Bit 6 – TXC: USART Transmit Complete
return bit_is_set(UCSRA,6);
}
void RS485_send(unsigned char data)
{
RS485_set_tx_done();
while(!RS485_is_tx_ready());
UDR=data;
}
void RS485_receive(unsigned char *data)
{
while(!(UCSRA & (1<<RXC)));
*data=UDR;
}
void init_RS485(void)
{
// configure the IO ports
// DDRD - Port D Data Direction Register
DDRD=DDRD|0xC2;// RX_en, TX_en, TX are outputs
DDRD=DDRD&0xFE;// RX is an input - LED OFF
// configure the ports to receive data
SET_RS485_RXD;
// initialize the rs485 ports
UCSRA = 0;// Single (not double) the USART transmission speed
UBRRH = 0;
UBRRL = 0;// BAUD = 1000000 at U2X=0, Fosc=16MHz => UBRR=0
UCSRB = (1<<RXEN)|(1<<TXEN);
UCSRC = 0x86; // 8 bit data, no parity (and URSEL must be 1)
}
void TxRS485Packet(unsigned char id, unsigned char instruction, unsigned char param_len, unsigned char *params)
{
unsigned char checksum=0,i=0;
SET_RS485_TXD;
RS485_send(0xFF);
RS485_send(0xFF);
RS485_send(id);
checksum+=id;
RS485_send(param_len+2);
checksum+=param_len+2;
RS485_send(instruction);
checksum+=instruction;
for(i=0;i<param_len;i++)
{
RS485_send(params[i]);
checksum+=params[i];
}
RS485_send(~checksum);
while(!RS485_is_tx_done());// wait until the data is sent
SET_RS485_RXD;
}
unsigned char RxRS485Packet(unsigned char *id,unsigned char *instruction, unsigned char *param_len, unsigned char *params)
{
unsigned char checksum=0,read_params=0;
unsigned char byte,i=0;
RS485_receive(&byte);
if(byte!=0xFF) return INCOMPLETE;
RS485_receive(&byte);
if(byte!=0xFF) return INCOMPLETE;
RS485_receive(id);
checksum+=(*id);
RS485_receive(param_len);
checksum+=(*param_len);
(*param_len)=(*param_len)-2;
RS485_receive(instruction);
checksum+=(*instruction);
for(i=0;i<(*param_len);i++)
{
RS485_receive(&params[read_params]);
checksum+=params[read_params];
read_params++;
}
RS485_receive(&byte);
checksum+=byte;
if(checksum==0xFF)
return CORRECT;
else
return CHECK_ERROR;
}
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment