diff --git a/servo_firmware/rx28/src/CTRL_sing_turn/CTRL_Dynamixel.c b/servo_firmware/rx28/src/CTRL_sing_turn/CTRL_Dynamixel.c
new file mode 100755
index 0000000000000000000000000000000000000000..17499e7775e3a25fb4f99d768b4cf4760c5430e9
--- /dev/null
+++ b/servo_firmware/rx28/src/CTRL_sing_turn/CTRL_Dynamixel.c
@@ -0,0 +1,230 @@
+#include <avr/interrupt.h>
+#include <avr/io.h>
+#include "MEM_Dynamixel.h"
+#include "CTRL_Dynamixel.h"
+#include "CFG_HW_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) {
+ SET_CW_PWM_MOTOR(CTRL_ZERO); //PB1 => set PWM for X% duty cycle
+ SET_CCW_PWM_MOTOR(PWM_Value); //PB2 => set PWM for Y% duty cycle
+ } else {
+ SET_CCW_PWM_MOTOR(CTRL_ZERO); //PB2 => set PWM for Y% duty cycle
+ SET_CW_PWM_MOTOR(PWM_Value); //PB1 => set PWM for X% duty cycle
+ }
+ } else {
+ SET_CW_PWM_MOTOR(CTRL_ZERO); //PB1 => set PWM for X% duty cycle
+ SET_CCW_PWM_MOTOR(CTRL_ZERO); //PB2 => set PWM for Y% duty cycle
+ }
+}
+