diff --git a/servo_firmware/mx28/Makefile b/servo_firmware/mx28/Makefile
index 027265d40e07e85980082634a6d22e3b90a4e762..d07bca3c57b71b3171149f323952acb1e22ceeb9 100755
--- a/servo_firmware/mx28/Makefile
+++ b/servo_firmware/mx28/Makefile
@@ -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 \
diff --git a/servo_firmware/mx28/src/mx28_fw.c b/servo_firmware/mx28/src/mx28_fw.c
index 7e2db16b42de6d685d95fc8fc666e907b6e08e44..6ced4727ddfc339122e9f4e0b3accedc02af9c97 100755
--- a/servo_firmware/mx28/src/mx28_fw.c
+++ b/servo_firmware/mx28/src/mx28_fw.c
@@ -1,150 +1,60 @@
/**
- ******************************************************************************
- * @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>© 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>© 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);*/
+
diff --git a/servo_firmware/mx28/src/system_init.c b/servo_firmware/mx28/src/system_init.c
index 0d0da3e95769359695da1d9d5266c704d2a80e75..ebb746b74a8823f41a5bb260324fb2e82aafd6f2 100755
--- a/servo_firmware/mx28/src/system_init.c
+++ b/servo_firmware/mx28/src/system_init.c
@@ -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.
diff --git a/servo_firmware/mx28/stm32_lib/include/devices/stm32f10x.h b/servo_firmware/mx28/stm32_lib/include/devices/stm32f10x.h
index bf97a2ce30657d4d479776e761fdcf62d5cdcb5f..efd9c286e96039bc46ef764adb1c4dac8618ab5f 100755
--- a/servo_firmware/mx28/stm32_lib/include/devices/stm32f10x.h
+++ b/servo_firmware/mx28/stm32_lib/include/devices/stm32f10x.h
@@ -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
* @{
diff --git a/servo_firmware/rx28/.cproject b/servo_firmware/rx28/.cproject
new file mode 100644
index 0000000000000000000000000000000000000000..513a7f12d424b67f5b4cf32b935f53f0d4b86835
--- /dev/null
+++ b/servo_firmware/rx28/.cproject
@@ -0,0 +1,53 @@
+<?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>
diff --git a/servo_firmware/rx28/.project b/servo_firmware/rx28/.project
new file mode 100644
index 0000000000000000000000000000000000000000..3e61a8e254f4a1b2b227da72ffd74d0a4fdb53e8
--- /dev/null
+++ b/servo_firmware/rx28/.project
@@ -0,0 +1,79 @@
+<?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>
diff --git a/servo_firmware/rx28/Makefile b/servo_firmware/rx28/Makefile
new file mode 100755
index 0000000000000000000000000000000000000000..8bdf19a7477384333d9c2fd04ea10ab21736832d
--- /dev/null
+++ b/servo_firmware/rx28/Makefile
@@ -0,0 +1,37 @@
+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)
diff --git a/servo_firmware/rx28/include/CFG_HW_Dynamixel.h b/servo_firmware/rx28/include/CFG_HW_Dynamixel.h
new file mode 100755
index 0000000000000000000000000000000000000000..cee708ddc68e992f8e02c5deb912d4140ef682f5
--- /dev/null
+++ b/servo_firmware/rx28/include/CFG_HW_Dynamixel.h
@@ -0,0 +1,27 @@
+#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
diff --git a/servo_firmware/rx28/include/CTRL_Dynamixel.h b/servo_firmware/rx28/include/CTRL_Dynamixel.h
new file mode 100644
index 0000000000000000000000000000000000000000..c619e44895058f1485727061f2a348ba04f76d9f
--- /dev/null
+++ b/servo_firmware/rx28/include/CTRL_Dynamixel.h
@@ -0,0 +1,25 @@
+#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
diff --git a/servo_firmware/rx28/include/MEM_Dynamixel.h b/servo_firmware/rx28/include/MEM_Dynamixel.h
new file mode 100755
index 0000000000000000000000000000000000000000..0c91efed71de92831a18bb4b212f5e820434014a
--- /dev/null
+++ b/servo_firmware/rx28/include/MEM_Dynamixel.h
@@ -0,0 +1,133 @@
+#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
diff --git a/servo_firmware/rx28/include/TXRX_Dynamixel.h b/servo_firmware/rx28/include/TXRX_Dynamixel.h
new file mode 100755
index 0000000000000000000000000000000000000000..383e08b62e25d4903dd18eda336bd939cff866a8
--- /dev/null
+++ b/servo_firmware/rx28/include/TXRX_Dynamixel.h
@@ -0,0 +1,82 @@
+#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
diff --git a/servo_firmware/rx28/src/CFG_HW_Dynamixel.c b/servo_firmware/rx28/src/CFG_HW_Dynamixel.c
new file mode 100755
index 0000000000000000000000000000000000000000..5eda0e922896e78cf5e8314c9f90ed036a45466b
--- /dev/null
+++ b/servo_firmware/rx28/src/CFG_HW_Dynamixel.c
@@ -0,0 +1,91 @@
+#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
+}
diff --git a/servo_firmware/rx28/src/CTRL_Dynamixel.c b/servo_firmware/rx28/src/CTRL_Dynamixel.c
new file mode 100755
index 0000000000000000000000000000000000000000..0aa66ad5ee51dbad0c4f1a2de37da45a494f8f33
--- /dev/null
+++ b/servo_firmware/rx28/src/CTRL_Dynamixel.c
@@ -0,0 +1,229 @@
+#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
+ }
+}
diff --git a/servo_firmware/rx28/src/MEM_Dynamixel.c b/servo_firmware/rx28/src/MEM_Dynamixel.c
new file mode 100755
index 0000000000000000000000000000000000000000..4a2bf2fee291e38fa265f55cbbb4e926f72c6ea0
--- /dev/null
+++ b/servo_firmware/rx28/src/MEM_Dynamixel.c
@@ -0,0 +1,176 @@
+#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);
+}
diff --git a/servo_firmware/rx28/src/TXRX_Dynamixel.c b/servo_firmware/rx28/src/TXRX_Dynamixel.c
new file mode 100755
index 0000000000000000000000000000000000000000..56a5a58591633cedf1b6df51d79a614dcbd98158
--- /dev/null
+++ b/servo_firmware/rx28/src/TXRX_Dynamixel.c
@@ -0,0 +1,317 @@
+#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);
+}
diff --git a/servo_firmware/rx28/src/main.c b/servo_firmware/rx28/src/main.c
new file mode 100755
index 0000000000000000000000000000000000000000..51fc747666db09c662bf93ada42fff2a4b75c0e1
--- /dev/null
+++ b/servo_firmware/rx28/src/main.c
@@ -0,0 +1,257 @@
+#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;
+ //***************************************************
+ }
+ }
+ }*/
+ }
+}
+
diff --git a/servo_firmware/rx28/src/sergi dynamixel.c b/servo_firmware/rx28/src/sergi dynamixel.c
new file mode 100755
index 0000000000000000000000000000000000000000..46f21a1f825d475ceb11730b22362a3c82bc2293
--- /dev/null
+++ b/servo_firmware/rx28/src/sergi dynamixel.c
@@ -0,0 +1,107 @@
+#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(¶ms[read_params]);
+ checksum+=params[read_params];
+ read_params++;
+ }
+ RS485_receive(&byte);
+ checksum+=byte;
+ if(checksum==0xFF)
+ return CORRECT;
+ else
+ return CHECK_ERROR;
+}