diff --git a/servo_firmware/ax12_2pos/Makefile b/servo_firmware/ax12_2pos/Makefile
new file mode 100755
index 0000000000000000000000000000000000000000..2d1411a4af7a15310e7fb0c0484a1ebdae5f4bad
--- /dev/null
+++ b/servo_firmware/ax12_2pos/Makefile
@@ -0,0 +1,37 @@
+PROJECT=ax12_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 ax12
+
+clean:
+ rm $(PROJECT).*
+ rm $(OBJS)
diff --git a/servo_firmware/ax12_2pos/include/CFG_HW_Dynamixel.h b/servo_firmware/ax12_2pos/include/CFG_HW_Dynamixel.h
new file mode 100755
index 0000000000000000000000000000000000000000..eb8bb8abab688a4ee761115ce5daf0918c0fe467
--- /dev/null
+++ b/servo_firmware/ax12_2pos/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/ax12_2pos/include/CTRL_Dynamixel.h b/servo_firmware/ax12_2pos/include/CTRL_Dynamixel.h
new file mode 100755
index 0000000000000000000000000000000000000000..3d84ad06d08658626ec6b020b97add83f0839cfe
--- /dev/null
+++ b/servo_firmware/ax12_2pos/include/CTRL_Dynamixel.h
@@ -0,0 +1,21 @@
+#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);
+
+#endif
diff --git a/servo_firmware/ax12_2pos/include/MEM_Dynamixel.h b/servo_firmware/ax12_2pos/include/MEM_Dynamixel.h
new file mode 100755
index 0000000000000000000000000000000000000000..c14dc7695c8308d47111c2f32bd400732c11c3fb
--- /dev/null
+++ b/servo_firmware/ax12_2pos/include/MEM_Dynamixel.h
@@ -0,0 +1,144 @@
+#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)))
+
+#define Read_byte_Dynamixel_EEPROM(c,a) ({\
+c=eepromVAR[a]; \
+})
+
+//#define Read_word_Dynamixel_RAM(c,a) ({\
+c = TwoBytesToInt(sramVAR[a-RAM_ID_correction+1],sramVAR[a-RAM_ID_correction]); \
+})
+
+
+
+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/ax12_2pos/include/TXRX_Dynamixel.h b/servo_firmware/ax12_2pos/include/TXRX_Dynamixel.h
new file mode 100755
index 0000000000000000000000000000000000000000..383e08b62e25d4903dd18eda336bd939cff866a8
--- /dev/null
+++ b/servo_firmware/ax12_2pos/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/ax12_2pos/src/CFG_HW_Dynamixel.c b/servo_firmware/ax12_2pos/src/CFG_HW_Dynamixel.c
new file mode 100755
index 0000000000000000000000000000000000000000..cea7e34dcaa910ad380c1f9ae3eb3b0804c20d1d
--- /dev/null
+++ b/servo_firmware/ax12_2pos/src/CFG_HW_Dynamixel.c
@@ -0,0 +1,92 @@
+#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 = 0x00;
+ // 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/ax12_2pos/src/CTRL_Dynamixel.c b/servo_firmware/ax12_2pos/src/CTRL_Dynamixel.c
new file mode 100755
index 0000000000000000000000000000000000000000..b7ff64dbf8ef660a0d11fb73f7cdb37427cd300b
--- /dev/null
+++ b/servo_firmware/ax12_2pos/src/CTRL_Dynamixel.c
@@ -0,0 +1,270 @@
+#include <avr/interrupt.h>
+#include <avr/io.h>
+#include "MEM_Dynamixel.h"
+#include "CFG_HW_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)); \
+})
+
+int16_t ADD_A_B(int16_t a, int16_t b)
+{
+ int16_t c;
+
+ int32_t x = (int32_t)a+b;
+ (x>32767)?(c = (int16_t)32767) : ((x<-32768)?(c = (int16_t)-32768):(c = (int16_t) x));
+
+ return c;
+}
+
+//#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)); \
+})
+
+int16_t MULT_A_B(int16_t a, int16_t b)
+{
+ int16_t c;
+
+ int32_t x = (int32_t)a*b;
+ (x>32767)?(c = (int16_t)32767) : ((x<-32768)?(c = (int16_t)-32768):(c = (int16_t) x));
+
+ return c;
+}
+
+#define num_to_FFFFbit(c,a) ({\
+float x = (float)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_Action,MotorDir,Error_Vector_0,Error_Vector_1,Error_Vector_2;
+ //int16_t Error_Vector_3;
+ int16_t Error_Margin,KP,KD,Sensor_Resol, Max_Sensor_Range;
+ //int16_t Integral_Value,Max_Integral_Value,KI ;
+ uint16_t PWM_Value;
+ int16_t Dead_Zone,Max_PWM_Value,Temp;
+ uint8_t Transition_Stage_Value;
+ int32_t Motor_Action_P=0;
+ int32_t Motor_Action_D=0;
+ //int32_t Motor_Action_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 action
+ Motor_Action = Read_word_Dynamixel_RAM(Motor_Accion_L);
+ //Read_word_Dynamixel_RAM(Motor_Action,Motor_Accion_L);
+ if (Motor_Action<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);
+ //Read_word_Dynamixel_RAM(Motor_Turns,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);
+ //Read_word_Dynamixel_RAM(Sensor_Resol,Sensor_Resol_L);
+ Max_Sensor_Range=Read_word_Dynamixel_RAM(Max_Sensor_Range_L);
+ //Read_word_Dynamixel_RAM(Max_Sensor_Range,Max_Sensor_Range_L);
+ Present_Position = (int16_t) ((float)Motor_Turns*Sensor_Resol*360/ Max_Sensor_Range); // OJO max 26 vueltas
+ //ADD_A_B(Present_Position,Present_Position,Sensor_Value);
+ Present_Position=ADD_A_B(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);
+ //Read_word_Dynamixel_RAM(Error_Vector_2,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);
+ //Read_word_Dynamixel_RAM(Error_Vector_1,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);
+ //Read_word_Dynamixel_RAM(Goal_Position,Goal_Position_L);
+ //MULT_A_B(Present_Position,Present_Position,-1);
+ Present_Position=MULT_A_B(Present_Position,-1);
+ //ADD_A_B(Error_Vector_0,Goal_Position,Present_Position);
+ Error_Vector_0=ADD_A_B(Goal_Position,Present_Position);
+ Write_word_Dynamixel_RAM(Error_Vector_0_L,Error_Vector_0);
+
+ // CTRL_Law - algorithm http://lorien.ncl.ac.uk/ming/digicont/digimath/dpid1.htm
+ KP = Read_word_Dynamixel_RAM(KP_L);
+ //Read_word_Dynamixel_RAM(KP,KP_L);
+ Motor_Action_P = ((int32_t)Error_Vector_0*KP)>>8;// coste de dividir /10 es mayor que shift 4 que equivale a dividir por 16;
+ KD = Read_word_Dynamixel_RAM(KD_L);
+ //Read_word_Dynamixel_RAM(KD,KD_L);
+ Motor_Action_D= (((int32_t)Error_Vector_0- Error_Vector_1*2 + Error_Vector_2 )*KD)>>10;//coste de dividir /100 es mayor que shift 7 que equivale a dividir por 128;
+ //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;*(KI>>7);//coste de dividir /100 es mayor que shift 7 que equivale a dividir por 128;
+ // saturate Integral action
+ //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_Action,Motor_Action_I,Motor_Action_D);
+ //ADD_A_B(Motor_Action,Motor_Action,Motor_Action_P);
+ //ADD_A_B(Motor_Action,Motor_Action_D,Motor_Action_P);
+ Motor_Action=ADD_A_B(Motor_Action_D,Motor_Action_P);
+
+ // range where small errors generate no action
+ Error_Margin = Read_word_Dynamixel_RAM(Error_Margin_L);
+ //Read_word_Dynamixel_RAM(Error_Margin,Error_Margin_L);
+ if (Error_Vector_0 < Error_Margin) {
+ if (Error_Vector_0 > (-1 * Error_Margin)) {
+ Motor_Action=0;
+ }
+ }
+
+
+ //Write_word_Dynamixel_RAM(Integral_Value_L,Integral_Value);
+ Write_word_Dynamixel_RAM(Motor_Accion_L,Motor_Action);
+
+ //*******************************************
+ //HW_Security TODO revisar corriente y temperatura
+ // limit control action ... max value and alarms
+
+ //******************************************
+ // Control action
+ // place control action on output
+
+ // find new motor action direction
+ if (Motor_Action<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);
+ //Read_word_Dynamixel_RAM(Dead_Zone,Dead_Zone_L);
+ Max_PWM_Value= Read_word_Dynamixel_RAM(Max_PWM_Value_L);
+ //Read_word_Dynamixel_RAM(Max_PWM_Value,Max_PWM_Value_L);
+ if (Motor_Action == CTRL_ZERO) {
+ PWM_Value = CTRL_ZERO;
+ } else {
+ //MULT_A_B(Temp,Motor_Action,MotorDir);
+ Temp = MULT_A_B(Motor_Action, MotorDir);
+ //ADD_A_B(Temp,Temp,Dead_Zone);
+ Temp = ADD_A_B(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/ax12_2pos/src/MEM_Dynamixel.c b/servo_firmware/ax12_2pos/src/MEM_Dynamixel.c
new file mode 100755
index 0000000000000000000000000000000000000000..239d0a5d1a54e9db1ebffca371b6eda7ab57c172
--- /dev/null
+++ b/servo_firmware/ax12_2pos/src/MEM_Dynamixel.c
@@ -0,0 +1,177 @@
+#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
+ 0x00, //low(1024), // Sensor_Resol_L; 0X33 //51 - 0X00 R/W - 10 bit sensor resolution LOW
+ 0x04, //high(1024), // Sensor_Resol_H; 0X34 //52 - 0X00 R/W - 10 bit sensor resolution HIGH
+ 0xBC, //low(700), // Max_PWM_Value_L; 0X35 //53 - 0X00 R/W - 10 bit max PWM value output LOW
+ 0x02, //high(700), // Max_PWM_Value_H; 0X36 //54 - 0X00 R/W - 0 bit max PWM value output HIGH
+ 0x4C, //low(332), // Max_Sensor_Range_L; 0X37 //55 - 0X00 R/W - working range of encoder 300 case ax12 motor LOW
+ 0x01, //high(332), // Max_Sensor_Range_H; 0X38 //56 - 0X00 R/W - working range of encoder 300 case ax12 motor HIGH
+ 0x05, //low(5), // Error_Margin_L; 0X39 //57 - 0X00 R/W - Range of error to discard LOW
+ 0x00, //high(5), // Error_Margin_H; 0X3A //58 - 0X00 R/W - Range of error to discard HIGH
+ 0x0A, //low(10), // Dead_Zone_L; 0X3B //59 - 0X00 R/W - Min PWM to put motor in motion LOW
+ 0x00, //high(10), // Dead_Zone_H; 0X3C //60 - 0X00 R/W - Min PWM to put motor in motion HIGH
+ 0x00, //low(1280), // KP_L; 0X3D //61 - 0X00 R/W - proportional constant LOW
+ 0x05, //high(1280), // KP_H; 0X3E //62 - 0X00 R/W - proportional constant HIGH
+ 0x52, //low(1106), // KD_L; 0X3F //63 - 0X00 R/W - Derivative constant LOW
+ 0x04, //high(1106), // KD_H; 0X40 //64 - 0X00 R/W - Derivative constant HIGH
+ 0x00, //low(0), // KI_L; 0X41 //65 - 0X00 R/W - Integral constant LOW
+ 0x00, //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
+ 0xF4, //low(500), // Max_Integral_Value_L; 0X45 //69 - 0X00 R/W - Saturation Integral value LOW
+ 0x01, //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 0 time _L
+ 0xEF, // Error_Vector_0_H; 0X4A //74 - 0X00 R/W - record of error for 0 time _H
+ 0x00, // Error_Vector_1_L; 0X4B //75 - 0X00 R/W - record of error for 1 time _L
+ 0x00, // Error_Vector_1_H; 0X4C //76 - 0X00 R/W - record of error for 1 time _H
+ 0xFF, // Error_Vector_2_L; 0X4D //77 - 0X00 R/W - record of error for 2 time _L
+ 0xEF, // Error_Vector_2_H; 0X4E //78 - 0X00 R/W - record of error for 2 time _H
+ 0x00, // Error_Vector_3_L; 0X4F //79 - 0X00 R/W - record of error for 3 time _L
+ 0x00, // Error_Vector_3_H; 0X50 //80 - 0X00 R/W - record of error for 4 time _H
+ 0xE8, //low(1000), // Time_Period_L; 0X51 //81 - 0X00 R/W - delta time in between ctrl cycles LOW
+ 0x03, //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
+
+
+ // replaced by #define 20 bytes saved
+/*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;
+}
+
+ // not replaced by #define 100 bytes unsaved
+int16_t Read_word_Dynamixel_RAM(uint8_t IDregister)
+{
+ int16_t temp;
+ temp= 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/ax12_2pos/src/TXRX_Dynamixel.c b/servo_firmware/ax12_2pos/src/TXRX_Dynamixel.c
new file mode 100755
index 0000000000000000000000000000000000000000..2a9fdeb7b42e60466289c8ab564f7b4b9ab376ad
--- /dev/null
+++ b/servo_firmware/ax12_2pos/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)
+{
+ if(num_data==0)
+ return 0;
+ else
+ {
+ cli();
+ *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);
+ Read_byte_Dynamixel_EEPROM(answerA[0],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);
+ Read_byte_Dynamixel_EEPROM(answerA[0],IDregister);
+ Read_byte_Dynamixel_EEPROM(answerA[1],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/ax12_2pos/src/main.c b/servo_firmware/ax12_2pos/src/main.c
new file mode 100755
index 0000000000000000000000000000000000000000..c8fea0e0f7767f7a96c5337348db97705ff49857
--- /dev/null
+++ b/servo_firmware/ax12_2pos/src/main.c
@@ -0,0 +1,278 @@
+// BIG NOTE
+// OVER WRITE HAPPENS IF PROGRAM BIGGER THAN 6100
+// becasused of the reserved space of 2K size of Bootloader
+// be aware -- NO MESSAGGE WILL OCCUR BUT LOSS OF INFO WILL OCCOUR
+
+#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;
+
+unsigned char do_control;
+
+ISR( TIMER0_OVF_vect) {
+ int16_t TorqueEnable;
+
+ TCNT0 = 0x00;
+
+ 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) {
+ do_control = 1;
+ //cli(); // disable all interrupts just to make sure
+ // 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
+ }
+
+ // to force write to eeprom un-comment
+ //Restore_Eeprom_Factory_Values(); //once this procedure is held, no more initialization is performed
+
+ //------EEPROM initial values-------------
+ Restore_EepromVAR();
+
+ // get value of Motor ID
+ rs485_address = eepromVAR[ID];
+
+ // configure AVR chip i/o
+ Config_Hardware();
+
+ // Assign actual position to goal position and assume zero motor turns
+ Ini_Position();
+
+ // initialize the RS485 interface
+ init_RS485();
+
+ // end of initialization
+ LedOFF;
+
+ do_control = 0;
+
+ while (1) {
+
+ status = RxRS485Packet(&id, &instruction, &length, data);
+
+ if (status == CHECK_ERROR) {
+ TxRS485Packet(rs485_address, CHECKSUM_ERROR, 0, NULL);
+ } else if (status == CORRECT) {
+ if (id == rs485_address) { //1) {
+ 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;
+
+ 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;
+ //***************************************************
+ }
+ }
+ } else if (do_control) {
+ Control_Cycle();
+ do_control = 0;
+ //sei(); // enable all interrupts
+ }
+ }
+}
+
diff --git a/servo_firmware/ax12_2pos/src/sergi dynamixel.c b/servo_firmware/ax12_2pos/src/sergi dynamixel.c
new file mode 100755
index 0000000000000000000000000000000000000000..46f21a1f825d475ceb11730b22362a3c82bc2293
--- /dev/null
+++ b/servo_firmware/ax12_2pos/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;
+}