diff --git a/servo_firmware/ax12/Makefile b/servo_firmware/ax12/Makefile
index 2d1411a4af7a15310e7fb0c0484a1ebdae5f4bad..650c609d68773066d058c9586d110aa6fb17ed7c 100755
--- a/servo_firmware/ax12/Makefile
+++ b/servo_firmware/ax12/Makefile
@@ -2,7 +2,7 @@ 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
+SOURCES=src/main.c src/gpio.c src/dyn_slave.c src/mem.c
OBJS=$(SOURCES:.c=.o)
SRC_DIR=./src/
INCLUDE_DIR=./include/
diff --git a/servo_firmware/ax12/include/dyn_slave.h b/servo_firmware/ax12/include/dyn_slave.h
new file mode 100644
index 0000000000000000000000000000000000000000..9a8a51e3f475b8627ddbbbe29368c5176581de38
--- /dev/null
+++ b/servo_firmware/ax12/include/dyn_slave.h
@@ -0,0 +1,41 @@
+#ifndef DYN_SLAVE_H
+#define DYN_SLAVE_H
+
+// 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
+
+void dyn_slave_init(uint8_t baudrate,uint8_t id);
+void dyn_slave_set_baudrate(uint8_t baudrate);
+void dyn_slave_set_id(uint8_t id);
+uint8_t dyn_slave_is_packet_ready(void);
+uint8_t dyn_slave_check_id(void);
+uint8_t dyn_slave_check_checksum(void);
+uint8_t dyn_slave_get_instruction(void);
+uint8_t dyn_slave_get_address(void);
+uint8_t dyn_slave_get_read_length(void);
+uint8_t dyn_slave_get_write_length(void);
+uint8_t *dyn_slave_get_write_data(void);
+void dyn_slave_send_status(uint8_t error,uint8_t length, uint8_t *data);
+uint8_t dyn_slave_is_send_done(void);
+
+#endif
diff --git a/servo_firmware/ax12/include/gpio.h b/servo_firmware/ax12/include/gpio.h
new file mode 100644
index 0000000000000000000000000000000000000000..4e60eae603c23dd673e1d70121d6c7a9d02297d3
--- /dev/null
+++ b/servo_firmware/ax12/include/gpio.h
@@ -0,0 +1,8 @@
+#ifndef GPIO_H
+#define GPIO_H
+
+void gpio_init(void);
+void gpio_led_on(void);
+void gpio_led_off(void);
+
+#endif
diff --git a/servo_firmware/ax12/include/mem.h b/servo_firmware/ax12/include/mem.h
new file mode 100644
index 0000000000000000000000000000000000000000..7687157738e4671e6b5b63158a45f4d9c11a31bd
--- /dev/null
+++ b/servo_firmware/ax12/include/mem.h
@@ -0,0 +1,58 @@
+#ifndef _MEM_H
+#define _MEM_H
+
+#include <avr/io.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
+// 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
+
+void ram_init(void);
+uint8_t ram_read(uint8_t address, uint8_t length, uint8_t **data);
+uint8_t ram_write(uint8_t address, uint8_t length, uint8_t *data);
+
+#endif
diff --git a/servo_firmware/ax12/src/dyn_slave.c b/servo_firmware/ax12/src/dyn_slave.c
new file mode 100644
index 0000000000000000000000000000000000000000..d91c098dad529c4d044cd64bf020674de0c4ddbd
--- /dev/null
+++ b/servo_firmware/ax12/src/dyn_slave.c
@@ -0,0 +1,193 @@
+#include <avr/interrupt.h>
+#include <avr/io.h>
+#include "dyn_slave.h"
+#include "gpio.h"
+
+uint8_t dyn_slave_id;
+uint8_t dyn_slave_data[128];
+uint8_t dyn_slave_num_bytes;
+uint8_t dyn_slave_packet_ready;
+uint8_t dyn_slave_send_done;
+
+inline void dyn_slave_set_rx(void)
+{
+ PORTD &= 0x7F;
+ PORTD |= 0x40;
+}
+
+inline void dyn_slave_set_tx(void)
+{
+ PORTD &= 0xBF;
+ PORTD |= 0x80;
+}
+
+ISR(USART_TXC_vect)
+{
+ UDR=dyn_slave_data[dyn_slave_num_bytes];
+ if(dyn_slave_num_bytes==dyn_slave_data[3]+4)
+ {
+ dyn_slave_num_bytes=0;
+ dyn_slave_set_rx();
+ dyn_slave_send_done=1;
+ UCSRB&=~(1<<TXCIE);// disable tx interrutps
+ UCSRA|=0x80;// clear any pending interrupt
+ UCSRB|=(1<<RXCIE);// enable reception interrupts
+ }
+ else
+ dyn_slave_num_bytes++;
+}
+
+ISR(USART_RXC_vect)
+{
+ static uint8_t length;
+
+ cli();// disable any other interrupt
+ dyn_slave_data[dyn_slave_num_bytes]=UDR;
+ switch(dyn_slave_num_bytes)
+ {
+ case 0: if(dyn_slave_data[dyn_slave_num_bytes]==0xFF)
+ dyn_slave_num_bytes++;
+ break;
+ case 1: if(dyn_slave_data[dyn_slave_num_bytes]==0xFF)
+ dyn_slave_num_bytes++;
+ else
+ dyn_slave_num_bytes--;
+ break;
+ case 2: dyn_slave_num_bytes++;
+ break;
+ case 3: length=dyn_slave_data[dyn_slave_num_bytes]+3;
+ dyn_slave_num_bytes++;
+ break;
+ default: if(dyn_slave_num_bytes==length)
+ {
+ dyn_slave_num_bytes=0;
+ dyn_slave_packet_ready=1;
+ UCSRB&=~(1<<RXCIE);// disable the rx interrupt
+ }
+ else
+ dyn_slave_num_bytes++;
+ break;
+ }
+ sei();// enable all interrutps
+}
+
+void dyn_slave_init(uint8_t baudrate,uint8_t id)
+{
+ // 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
+ dyn_slave_set_rx();
+
+ // initialize the rs485 ports
+ UCSRA = (1<<U2X);// double USART transmission speed
+ dyn_slave_set_baudrate(baudrate);
+ UCSRB = (1<<RXEN)|(1<<TXEN)|(1<<RXCIE);
+ UCSRC = (1<<URSEL)|(1<<UCSZ1)|(1<< UCSZ0); // 8 bit data, no parity, 1 stop bit
+
+ dyn_slave_set_id(id);
+ // initialize private variables
+ dyn_slave_num_bytes=0;
+ dyn_slave_packet_ready=0;
+ dyn_slave_send_done=0;
+}
+
+void dyn_slave_set_baudrate(uint8_t baudrate)
+{
+ UBRRH = 0;
+ UBRRL = baudrate;
+}
+
+void dyn_slave_set_id(uint8_t id)
+{
+ dyn_slave_id=id;
+}
+
+uint8_t dyn_slave_is_packet_ready(void)
+{
+ if(dyn_slave_packet_ready==0x01)
+ {
+ dyn_slave_packet_ready=0x00;
+ return 0x01;
+ }
+ else
+ return 0x00;
+}
+
+uint8_t dyn_slave_check_id(void)
+{
+ if(dyn_slave_id==dyn_slave_data[2])
+ return 0x01;
+ else
+ return 0x00;
+}
+
+uint8_t dyn_slave_check_checksum(void)
+{
+ uint8_t i,cs=0x00;
+
+ for(i=2;i<dyn_slave_data[3]+4;i++)
+ cs+=dyn_slave_data[i];
+ if(cs==0xFF)
+ return 0x01;
+ else
+ return 0x00;
+}
+
+uint8_t dyn_slave_get_instruction(void)
+{
+ return dyn_slave_data[4];
+}
+
+uint8_t dyn_slave_get_address(void)
+{
+ return dyn_slave_data[5];
+}
+
+uint8_t dyn_slave_get_read_length(void)
+{
+ return dyn_slave_data[6];
+}
+
+uint8_t dyn_slave_get_write_length(void)
+{
+ return dyn_slave_data[3]-3;
+}
+
+uint8_t *dyn_slave_get_write_data(void)
+{
+ return &dyn_slave_data[6];
+}
+
+void dyn_slave_send_status(uint8_t error,uint8_t length, uint8_t *data)
+{
+ uint8_t i,cs=0;
+
+ dyn_slave_data[0]=0xFF;
+ dyn_slave_data[1]=0xFF;
+ dyn_slave_data[2]=dyn_slave_id;
+ cs+=dyn_slave_id;
+ dyn_slave_data[3]=length+2;
+ cs+=length;
+ dyn_slave_data[4]=error;
+ cs+=error;
+ for(i=0;i<length;i++)
+ {
+ dyn_slave_data[5+i]=data[i];
+ cs+=data[i];
+ }
+ dyn_slave_data[5+i]=~cs;
+ // set in tex mode
+ dyn_slave_set_tx();
+ // start transmission
+ UCSRA|=0x40;// clear any pending interrupt
+ UCSRB|=(1<<TXCIE);
+ dyn_slave_num_bytes=1;
+ UDR=dyn_slave_data[0];
+}
+
+uint8_t dyn_slave_is_send_done(void)
+{
+ return dyn_slave_send_done;
+}
diff --git a/servo_firmware/ax12/src/gpio.c b/servo_firmware/ax12/src/gpio.c
new file mode 100644
index 0000000000000000000000000000000000000000..f177c52ac33717ae0996d0ea59a67b3592486e83
--- /dev/null
+++ b/servo_firmware/ax12/src/gpio.c
@@ -0,0 +1,20 @@
+#include "gpio.h"
+#include <avr/io.h>
+
+#define LED_PIN PD2
+
+void gpio_init(void)
+{
+ DDRD |= (1 << LED_PIN); // Set LED as output
+ gpio_led_off();
+}
+
+void gpio_led_on(void)
+{
+ PORTD &= ~(1<<LED_PIN); // ON LED
+}
+
+void gpio_led_off(void)
+{
+ PORTD |= (1<<LED_PIN); // off LED
+}
diff --git a/servo_firmware/ax12/src/main.c b/servo_firmware/ax12/src/main.c
index c8fea0e0f7767f7a96c5337348db97705ff49857..ab3de0879e6dd041a2116e823a201cb61d4b5b17 100755
--- a/servo_firmware/ax12/src/main.c
+++ b/servo_firmware/ax12/src/main.c
@@ -4,275 +4,66 @@
// 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
- }
+#include <util/delay.h>
+#include <avr/interrupt.h>
+#include "gpio.h"
+#include "dyn_slave.h"
+#include "mem.h"
+
+void do_write(uint8_t address,uint8_t length,uint8_t *data)
+{
+ uint8_t i,num=0;
+
+ // check is EEPROM space
+ for(i=address;i<address+length;i++)
+ {
+ if(i==ID)
+ dyn_slave_set_id(data[num]);
+ num++;
+ }
}
-//**************** 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
- }
- }
+int16_t main(void)
+{
+ uint8_t *data;
+
+ gpio_init();
+ dyn_slave_init(34,1);
+ ram_init();
+ sei();
+
+ while (1)
+ {
+ if(dyn_slave_is_packet_ready())
+ {
+ if(dyn_slave_check_id())
+ {
+ if(dyn_slave_check_checksum())
+ {
+ switch(dyn_slave_get_instruction())
+ {
+ case INST_PING: dyn_slave_send_status(NO_ERROR,0x00,0x00000000);
+ break;
+ case INST_READ: if(ram_read(dyn_slave_get_address(),dyn_slave_get_read_length(),&data))
+ dyn_slave_send_status(NO_ERROR,dyn_slave_get_read_length(),data);
+ else
+ dyn_slave_send_status(INSTRUCTION_ERROR,0x00,0x00000000);
+ break;
+ case INST_WRITE: if(ram_write(dyn_slave_get_address(),dyn_slave_get_write_length(),dyn_slave_get_write_data()))
+ {
+ do_write(dyn_slave_get_address(),dyn_slave_get_write_length(),dyn_slave_get_write_data());
+ dyn_slave_send_status(NO_ERROR,0x00,0x00000000);
+ }
+ else
+ dyn_slave_send_status(INSTRUCTION_ERROR,0x00,0x00000000);
+ break;
+ default: dyn_slave_send_status(INSTRUCTION_ERROR,0x00,0x00000000);
+ break;
+ }
+ }
+ else
+ dyn_slave_send_status(CHECKSUM_ERROR,0x00,0x00000000);
+ }
+ }
+ }
}
-
diff --git a/servo_firmware/ax12/src/mem.c b/servo_firmware/ax12/src/mem.c
new file mode 100644
index 0000000000000000000000000000000000000000..59b815e926851155486800481faa83a016461f32
--- /dev/null
+++ b/servo_firmware/ax12/src/mem.c
@@ -0,0 +1,53 @@
+#include "mem.h"
+#include <avr/eeprom.h>
+
+// dynamixel RAM variables
+unsigned char ram_data[50];
+
+// Dynamixel EEPROM variables
+unsigned char EEMEM eeprom_data[19]={0x0C,0x00,0x00,0x01,0x22,0xFA,0x00,0x00,0xFF,0x03,0x00,0x50,0x3C,0xF0,0xFF,0x03,0x02,0x24,0x24};
+
+void ram_init(void)
+{
+ uint8_t i;
+
+ for(i=0;i<Alarm_Shutdown;i++)
+ ram_data[i]=eeprom_read_byte(&eeprom_data[i]);
+ for(;i<Punch_H;i++)
+ ram_data[i]=0x00;
+ ram_data[CW_Compliance_Slope]=0x20;
+ ram_data[CCW_Compliance_Slope]=0x20;
+ ram_data[Torque_Limit_L]=ram_data[Max_Torque_L];
+ ram_data[Torque_Limit_H]=ram_data[Max_Torque_H];
+ ram_data[Punch_L]=0x20;
+}
+
+uint8_t ram_read(uint8_t address, uint8_t length, uint8_t **data)
+{
+ if((address+length)>Punch_H)
+ return 0x00;
+ else
+ {
+ (*data)=&ram_data[address];
+ return 0x01;
+ }
+}
+
+uint8_t ram_write(uint8_t address, uint8_t length, uint8_t *data)
+{
+ uint8_t i,num=0;
+
+ if((address+length)>Punch_H)
+ return 0x00;
+ else
+ {
+ for(i=address;i<address+length;i++)
+ {
+ if(i<=Alarm_Shutdown)
+ eeprom_write_byte(&eeprom_data[i],data[num]);
+ ram_data[i]=data[num];
+ num++;
+ }
+ return 0x01;
+ }
+}