diff --git a/servo_firmware/ax12/include/CFG_HW_Dynamixel.h b/servo_firmware/ax12/include/CFG_HW_Dynamixel.h
deleted file mode 100755
index eb8bb8abab688a4ee761115ce5daf0918c0fe467..0000000000000000000000000000000000000000
--- a/servo_firmware/ax12/include/CFG_HW_Dynamixel.h
+++ /dev/null
@@ -1,27 +0,0 @@
-#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/include/CTRL_Dynamixel.h b/servo_firmware/ax12/include/CTRL_Dynamixel.h
deleted file mode 100755
index 3d84ad06d08658626ec6b020b97add83f0839cfe..0000000000000000000000000000000000000000
--- a/servo_firmware/ax12/include/CTRL_Dynamixel.h
+++ /dev/null
@@ -1,21 +0,0 @@
-#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/include/MEM_Dynamixel.h b/servo_firmware/ax12/include/MEM_Dynamixel.h
deleted file mode 100755
index c14dc7695c8308d47111c2f32bd400732c11c3fb..0000000000000000000000000000000000000000
--- a/servo_firmware/ax12/include/MEM_Dynamixel.h
+++ /dev/null
@@ -1,144 +0,0 @@
-#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/include/TXRX_Dynamixel.h b/servo_firmware/ax12/include/TXRX_Dynamixel.h
deleted file mode 100755
index 383e08b62e25d4903dd18eda336bd939cff866a8..0000000000000000000000000000000000000000
--- a/servo_firmware/ax12/include/TXRX_Dynamixel.h
+++ /dev/null
@@ -1,82 +0,0 @@
-#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/src/CFG_HW_Dynamixel.c b/servo_firmware/ax12/src/CFG_HW_Dynamixel.c
deleted file mode 100755
index cea7e34dcaa910ad380c1f9ae3eb3b0804c20d1d..0000000000000000000000000000000000000000
--- a/servo_firmware/ax12/src/CFG_HW_Dynamixel.c
+++ /dev/null
@@ -1,92 +0,0 @@
-#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/src/CTRL_Dynamixel.c b/servo_firmware/ax12/src/CTRL_Dynamixel.c
deleted file mode 100755
index c9114f4047a46795caa85041c9e4124ffe4d54b2..0000000000000000000000000000000000000000
--- a/servo_firmware/ax12/src/CTRL_Dynamixel.c
+++ /dev/null
@@ -1,284 +0,0 @@
-#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)
-// transform a 64 bit to a 16 bit
-
-int16_t int64_int16(int64_t a)
-{
- int16_t c;
- (a>32767)?(c = (int16_t)32767) : ((a<-32768)?(c = (int16_t)-32768):(c = (int16_t) a));
-
- return c;
-}
-
-//#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;
- //int32_t Temp32=0;
- int64_t Temp64=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); // error ... son int32 no int16
- Temp64=(int64_t)Motor_Action_D+Motor_Action_P;
- Motor_Action=int64_int16(Temp64);
-
-
- // 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/src/MEM_Dynamixel.c b/servo_firmware/ax12/src/MEM_Dynamixel.c
deleted file mode 100755
index 239d0a5d1a54e9db1ebffca371b6eda7ab57c172..0000000000000000000000000000000000000000
--- a/servo_firmware/ax12/src/MEM_Dynamixel.c
+++ /dev/null
@@ -1,177 +0,0 @@
-#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/src/TXRX_Dynamixel.c b/servo_firmware/ax12/src/TXRX_Dynamixel.c
deleted file mode 100755
index 2a9fdeb7b42e60466289c8ab564f7b4b9ab376ad..0000000000000000000000000000000000000000
--- a/servo_firmware/ax12/src/TXRX_Dynamixel.c
+++ /dev/null
@@ -1,317 +0,0 @@
-#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/src/sergi dynamixel.c b/servo_firmware/ax12/src/sergi dynamixel.c
deleted file mode 100755
index 46f21a1f825d475ceb11730b22362a3c82bc2293..0000000000000000000000000000000000000000
--- a/servo_firmware/ax12/src/sergi dynamixel.c
+++ /dev/null
@@ -1,107 +0,0 @@
-#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;
-}