diff --git a/servo_firmware/ax12/Makefile b/servo_firmware/ax12/Makefile
index 650c609d68773066d058c9586d110aa6fb17ed7c..8f2792f73bca782f60187dbf36dcca4156803edf 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/gpio.c src/dyn_slave.c src/mem.c
+SOURCES=src/main.c src/gpio.c src/dyn_slave.c src/mem.c src/motor_control.c src/adc.c src/time_base.c src/sensors.c src/comm.c
OBJS=$(SOURCES:.c=.o)
SRC_DIR=./src/
INCLUDE_DIR=./include/
@@ -14,7 +14,7 @@ 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
+LDFLAGS=-mmcu=$(MMCU) -Wl,-Map=$(PROJECT).map,--section-start,.eeprom=810000 -DF_CPU=16000000UL
HEX_FLASH_FLAGS = -R .eeprom -R .fuse -R .lock -R .signature
diff --git a/servo_firmware/ax12/include/adc.h b/servo_firmware/ax12/include/adc.h
new file mode 100644
index 0000000000000000000000000000000000000000..28ca114a091233b37e01ac8d655dd972ada1fcf4
--- /dev/null
+++ b/servo_firmware/ax12/include/adc.h
@@ -0,0 +1,17 @@
+#ifndef _ADC_H
+#define _ADC_H
+
+#include <avr/interrupt.h>
+#include <avr/io.h>
+
+#define ADC_pote_Port 0 // Input port for potenciometer
+#define ADC_temp_Port 2 // Input port for temperature
+#define ADC_voltage_Port 3 // Input port for voltage
+
+void adc_init(void);
+void adc_start(uint8_t port);
+uint8_t adc_is_done(void);
+uint16_t adc_get_value(void);
+
+#endif
+
diff --git a/servo_firmware/ax12/include/comm.h b/servo_firmware/ax12/include/comm.h
new file mode 100644
index 0000000000000000000000000000000000000000..6f5080049c66948d5c98b4269d3bef081c684b76
--- /dev/null
+++ b/servo_firmware/ax12/include/comm.h
@@ -0,0 +1,12 @@
+#ifndef _COMM_H
+#define _COMM_H
+
+#include <avr/interrupt.h>
+#include <avr/io.h>
+
+void comm_init(void);
+void comm_loop(void);
+void comm_set_error(uint8_t error);
+void comm_clear_error(void);
+
+#endif
diff --git a/servo_firmware/ax12/include/dyn_slave.h b/servo_firmware/ax12/include/dyn_slave.h
index 9a8a51e3f475b8627ddbbbe29368c5176581de38..8d10a99668f2c68256b67176a953f0cad79378c6 100644
--- a/servo_firmware/ax12/include/dyn_slave.h
+++ b/servo_firmware/ax12/include/dyn_slave.h
@@ -1,6 +1,9 @@
#ifndef DYN_SLAVE_H
#define DYN_SLAVE_H
+#include <avr/interrupt.h>
+#include <avr/io.h>
+
// Instruction identifiers
#define INST_PING 0x01
#define INST_READ 0x02
diff --git a/servo_firmware/ax12/include/mem.h b/servo_firmware/ax12/include/mem.h
index 7687157738e4671e6b5b63158a45f4d9c11a31bd..aad100ccd8346653a77931825a937c3e987e44aa 100644
--- a/servo_firmware/ax12/include/mem.h
+++ b/servo_firmware/ax12/include/mem.h
@@ -26,10 +26,10 @@
// 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 KP_L 0X1A //26 - 0X01 R/W - CW Compliance margin
+#define KP_H 0X1B //27 - 0X01 R/W - CCW Compliance margin
+#define KI_L 0X1C //28 - 0X20 R/W - CW Compliance slope
+#define KI_H 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
@@ -48,8 +48,10 @@
//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
+#define KD_L 0X30 //48 - 0X20 R/W - Lowest byte of Punch
+#define KD_H 0X31 //49 - 0X00 R/W - Highest byte of Punch
+
+extern unsigned char ram_data[50];
void ram_init(void);
uint8_t ram_read(uint8_t address, uint8_t length, uint8_t **data);
diff --git a/servo_firmware/ax12/include/motor_control.h b/servo_firmware/ax12/include/motor_control.h
new file mode 100644
index 0000000000000000000000000000000000000000..1ad94d10ac220a588035cb0c6644c3d403e6db1d
--- /dev/null
+++ b/servo_firmware/ax12/include/motor_control.h
@@ -0,0 +1,11 @@
+#ifndef _MOTOR_CONTROL_H
+#define _MOTOR_CONTROL_H
+
+#include <avr/interrupt.h>
+#include <avr/io.h>
+
+void motor_init(void);
+void motor_set_pwm(uint16_t cw,uint16_t ccw);
+void motor_control_loop(void);
+
+#endif
diff --git a/servo_firmware/ax12/include/sensors.h b/servo_firmware/ax12/include/sensors.h
new file mode 100644
index 0000000000000000000000000000000000000000..193b4bc58954d4fe9550bfb1831f21d865001748
--- /dev/null
+++ b/servo_firmware/ax12/include/sensors.h
@@ -0,0 +1,7 @@
+#ifndef _SENSORS_H
+#define _SENSIRS_H
+
+void sensors_init(void);
+void sensors_loop(void);
+
+#endif
diff --git a/servo_firmware/ax12/include/time_base.h b/servo_firmware/ax12/include/time_base.h
new file mode 100644
index 0000000000000000000000000000000000000000..d44eea59e88597e2af87d816ddd6858c233233ab
--- /dev/null
+++ b/servo_firmware/ax12/include/time_base.h
@@ -0,0 +1,9 @@
+#ifndef _TIME_BASE_H
+#define _TIME_BASE_H
+
+#include <avr/io.h>
+
+void time_base_init(void);
+uint8_t time_base_period(void);
+
+#endif
diff --git a/servo_firmware/ax12/src/adc.c b/servo_firmware/ax12/src/adc.c
new file mode 100644
index 0000000000000000000000000000000000000000..d297347a6b5b24bb4b35b0f601fc4f2653e75d10
--- /dev/null
+++ b/servo_firmware/ax12/src/adc.c
@@ -0,0 +1,28 @@
+#include "adc.h"
+
+void adc_init(void)
+{
+ ADMUX|=(1 << REFS0); // use AVcc as the reference
+ ADMUX&=~(1 << ADLAR); // clear for 10 bit resolution
+ ADCSRA|=(1 << ADPS2)|(1 << ADPS1)|(1 << ADPS0)|(1 << ADEN); // 128 prescale for 8Mhz, enable ADC
+}
+
+void adc_start(uint8_t port)
+{
+ ADMUX&=0xF0;
+ ADMUX|=port;
+ ADCSRA|=(1 << ADSC); // Start the ADC conversion
+}
+
+uint8_t adc_is_done(void)
+{
+ if(ADCSRA & (1 << ADSC)) // waits for the ADC to finish
+ return 0x00;
+ else
+ return 0x01;
+}
+
+uint16_t adc_get_value(void)
+{
+ return ADCL+(ADCH << 8); // ADCH is read so ADC can be updated again
+}
diff --git a/servo_firmware/ax12/src/comm.c b/servo_firmware/ax12/src/comm.c
new file mode 100644
index 0000000000000000000000000000000000000000..bde386a84f7f6d2a4148af68f05b5c69a8b5b9c7
--- /dev/null
+++ b/servo_firmware/ax12/src/comm.c
@@ -0,0 +1,78 @@
+#include "comm.h"
+#include "dyn_slave.h"
+#include "mem.h"
+#include "gpio.h"
+
+uint8_t dyn_error;
+
+void do_write(uint8_t address,uint8_t length,uint8_t *data)
+{
+ uint8_t i,num=0;
+
+ for(i=address;i<address+length;i++)
+ {
+ if(i==ID)
+ dyn_slave_set_id(data[num]);
+ else if(i==Baud_Rate)
+ dyn_slave_set_baudrate(data[num]);
+ else if(i==LED)
+ {
+ if(data[num]==0x00) gpio_led_off();
+ else gpio_led_on();
+ }
+ num++;
+ }
+ ram_write(address,length,data);
+}
+
+void comm_init(void)
+{
+ dyn_error=NO_ERROR;
+}
+
+void comm_loop(void)
+{
+ uint8_t *data;
+
+ 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(dyn_error|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(dyn_error|NO_ERROR,dyn_slave_get_read_length(),data);
+ else
+ dyn_slave_send_status(dyn_error|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(dyn_error|NO_ERROR,0x00,0x00000000);
+ }
+ else
+ dyn_slave_send_status(dyn_error|INSTRUCTION_ERROR,0x00,0x00000000);
+ break;
+ default: dyn_slave_send_status(dyn_error|INSTRUCTION_ERROR,0x00,0x00000000);
+ break;
+ }
+ }
+ else
+ dyn_slave_send_status(dyn_error|CHECKSUM_ERROR,0x00,0x00000000);
+ }
+ }
+}
+
+void comm_set_error(uint8_t error)
+{
+ dyn_error|=error;
+}
+
+void comm_clear_error(void)
+{
+ dyn_error=NO_ERROR;
+}
diff --git a/servo_firmware/ax12/src/dyn_slave.c b/servo_firmware/ax12/src/dyn_slave.c
index d91c098dad529c4d044cd64bf020674de0c4ddbd..a43a39fdde5d2316bf8fa9a3676289ccae85a392 100644
--- a/servo_firmware/ax12/src/dyn_slave.c
+++ b/servo_firmware/ax12/src/dyn_slave.c
@@ -5,7 +5,8 @@
uint8_t dyn_slave_id;
uint8_t dyn_slave_data[128];
-uint8_t dyn_slave_num_bytes;
+uint8_t dyn_slave_num_rx_bytes;
+uint8_t dyn_slave_num_tx_bytes;
uint8_t dyn_slave_packet_ready;
uint8_t dyn_slave_send_done;
@@ -23,18 +24,17 @@ inline void dyn_slave_set_tx(void)
ISR(USART_TXC_vect)
{
- UDR=dyn_slave_data[dyn_slave_num_bytes];
- if(dyn_slave_num_bytes==dyn_slave_data[3]+4)
+ if(dyn_slave_num_tx_bytes==dyn_slave_data[3]+4)
{
- dyn_slave_num_bytes=0;
+ dyn_slave_num_rx_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++;
+ {
+ UDR=dyn_slave_data[dyn_slave_num_tx_bytes];
+ dyn_slave_num_tx_bytes++;
+ }
}
ISR(USART_RXC_vect)
@@ -42,30 +42,25 @@ 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)
+ dyn_slave_data[dyn_slave_num_rx_bytes]=UDR;
+ switch(dyn_slave_num_rx_bytes)
{
- case 0: if(dyn_slave_data[dyn_slave_num_bytes]==0xFF)
- dyn_slave_num_bytes++;
+ case 0: if(dyn_slave_data[dyn_slave_num_rx_bytes]==0xFF)
+ dyn_slave_num_rx_bytes++;
break;
- case 1: if(dyn_slave_data[dyn_slave_num_bytes]==0xFF)
- dyn_slave_num_bytes++;
+ case 1: if(dyn_slave_data[dyn_slave_num_rx_bytes]==0xFF)
+ dyn_slave_num_rx_bytes++;
else
- dyn_slave_num_bytes--;
+ dyn_slave_num_rx_bytes--;
break;
- case 2: dyn_slave_num_bytes++;
+ case 2: dyn_slave_num_rx_bytes++;
break;
- case 3: length=dyn_slave_data[dyn_slave_num_bytes]+3;
- dyn_slave_num_bytes++;
+ case 3: length=dyn_slave_data[dyn_slave_num_rx_bytes]+3;
+ dyn_slave_num_rx_bytes++;
break;
- default: if(dyn_slave_num_bytes==length)
- {
- dyn_slave_num_bytes=0;
+ default: if(dyn_slave_num_rx_bytes==length)
dyn_slave_packet_ready=1;
- UCSRB&=~(1<<RXCIE);// disable the rx interrupt
- }
- else
- dyn_slave_num_bytes++;
+ dyn_slave_num_rx_bytes++;
break;
}
sei();// enable all interrutps
@@ -83,12 +78,14 @@ void dyn_slave_init(uint8_t baudrate,uint8_t id)
// initialize the rs485 ports
UCSRA = (1<<U2X);// double USART transmission speed
dyn_slave_set_baudrate(baudrate);
- UCSRB = (1<<RXEN)|(1<<TXEN)|(1<<RXCIE);
+ UCSRA|=0x40;// clear any pending interrupt
+ UCSRB = (1<<RXEN)|(1<<TXEN)|(1<<RXCIE)|(1<<TXCIE);
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_num_tx_bytes=0;
+ dyn_slave_num_rx_bytes=0;
dyn_slave_packet_ready=0;
dyn_slave_send_done=0;
}
@@ -120,7 +117,10 @@ uint8_t dyn_slave_check_id(void)
if(dyn_slave_id==dyn_slave_data[2])
return 0x01;
else
+ {
+ dyn_slave_num_rx_bytes=0;
return 0x00;
+ }
}
uint8_t dyn_slave_check_checksum(void)
@@ -169,7 +169,7 @@ void dyn_slave_send_status(uint8_t error,uint8_t length, uint8_t *data)
dyn_slave_data[2]=dyn_slave_id;
cs+=dyn_slave_id;
dyn_slave_data[3]=length+2;
- cs+=length;
+ cs+=length+2;
dyn_slave_data[4]=error;
cs+=error;
for(i=0;i<length;i++)
@@ -181,9 +181,7 @@ void dyn_slave_send_status(uint8_t error,uint8_t length, uint8_t *data)
// set in tex mode
dyn_slave_set_tx();
// start transmission
- UCSRA|=0x40;// clear any pending interrupt
- UCSRB|=(1<<TXCIE);
- dyn_slave_num_bytes=1;
+ dyn_slave_num_tx_bytes=1;
UDR=dyn_slave_data[0];
}
diff --git a/servo_firmware/ax12/src/main.c b/servo_firmware/ax12/src/main.c
index ab3de0879e6dd041a2116e823a201cb61d4b5b17..88c25be1054a538ec14c428f025d5b5648547132 100755
--- a/servo_firmware/ax12/src/main.c
+++ b/servo_firmware/ax12/src/main.c
@@ -9,61 +9,28 @@
#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++;
- }
-}
+#include "adc.h"
+#include "motor_control.h"
+#include "time_base.h"
+#include "sensors.h"
+#include "comm.h"
int16_t main(void)
{
- uint8_t *data;
-
gpio_init();
- dyn_slave_init(34,1);
ram_init();
+ dyn_slave_init(ram_data[Baud_Rate],ram_data[ID]);
+ adc_init();
+ time_base_init();
+ sensors_init();
+ motor_init();
+ comm_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);
- }
- }
+ comm_loop();
+ sensors_loop();
+ motor_control_loop();
}
}
diff --git a/servo_firmware/ax12/src/mem.c b/servo_firmware/ax12/src/mem.c
index 59b815e926851155486800481faa83a016461f32..530c3e3af051bc126c1554ea54e55c58d782a511 100644
--- a/servo_firmware/ax12/src/mem.c
+++ b/servo_firmware/ax12/src/mem.c
@@ -1,30 +1,30 @@
#include "mem.h"
+#include "gpio.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};
+unsigned char EEMEM eeprom_data[19]={0x1C,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++)
+ for(;i<=KD_H;i++)
ram_data[i]=0x00;
- ram_data[CW_Compliance_Slope]=0x20;
- ram_data[CCW_Compliance_Slope]=0x20;
+ ram_data[KP_L]=0x20;
+ ram_data[KP_H]=0x00;
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)
+ if((address+length)>KD_H)
return 0x00;
else
{
@@ -37,7 +37,7 @@ uint8_t ram_write(uint8_t address, uint8_t length, uint8_t *data)
{
uint8_t i,num=0;
- if((address+length)>Punch_H)
+ if((address+length)>KD_H)
return 0x00;
else
{
diff --git a/servo_firmware/ax12/src/motor_control.c b/servo_firmware/ax12/src/motor_control.c
new file mode 100644
index 0000000000000000000000000000000000000000..bde876e20675bc290c147b8290705a25c68ef27f
--- /dev/null
+++ b/servo_firmware/ax12/src/motor_control.c
@@ -0,0 +1,156 @@
+#include "motor_control.h"
+#include "time_base.h"
+#include "comm.h"
+#include "mem.h"
+#include "dyn_slave.h"
+#include "gpio.h"
+
+#define CW_PWM_MOTOR_PIN PB1
+#define CCW_PWM_MOTOR_PIN PB2
+
+#define CONTROL_COUNT 10
+
+int16_t start_position;
+int16_t previous_position;
+int16_t previous_seek;
+int16_t seek_delta;
+int16_t count;
+
+void motor_init(void)
+{
+ /* configure the control signals */
+ DDRB|=(1 << CW_PWM_MOTOR_PIN)|(1 << CCW_PWM_MOTOR_PIN);
+ motor_set_pwm(0x0000,0x0000);
+
+ /* configure the PWM */
+ TCCR1A|=(1 << COM1A1)|(1 << COM1B1)|(1 << WGM11) | (1 << WGM10); // set none-inverting mode PB1 and PB2, Mode fast PWM @ 10 bits
+ TCCR1B|=(1 << WGM12)|(1 << CS11) | (1 << CS10) ; // set Mode Fast PWM, 10bit, prescaker 64
+
+ // initialize internal variables
+ previous_position=ram_data[Present_Position_L]+(ram_data[Present_Position_H]<<8);
+ start_position=previous_position;
+ previous_seek=previous_position;
+ seek_delta=-1;
+ count=0;
+}
+
+void motor_set_pwm(uint16_t cw,uint16_t ccw)
+{
+ OCR1A = ccw; //PB1 => set PWM for X% duty cycle @ 10bit
+ OCR1B = cw; //PB2 => set PWM for Y% duty cycle @ 10bit
+}
+
+void motor_control_loop(void)
+{
+ static int16_t current_position;
+ static int16_t current_velocity;
+ static int16_t seek_position;
+ static int16_t seek_velocity;
+ static int16_t minimum_position;
+ static int16_t maximum_position;
+ static int16_t p_component;
+ static int16_t d_component;
+ static int16_t i_component;
+ static int16_t torque_limit;
+ static int16_t deadband=10;
+ static int32_t pwm_output;
+ static uint16_t p_gain;
+ static uint16_t i_gain;
+ static uint16_t d_gain;
+
+ if(time_base_period())
+ {
+ // update control loop
+ // get all necessary variables
+ current_position=ram_data[Present_Position_L]+(ram_data[Present_Position_H]<<8);
+ current_velocity = current_position - previous_position;
+ ram_data[Present_Speed_L]=current_velocity&0xFF;
+ ram_data[Present_Speed_H]=((current_velocity>>8)&0xFF);
+ seek_position=ram_data[Goal_Position_L]+(ram_data[Goal_Position_H]<<8);
+ seek_velocity=ram_data[Moving_Speed_L]+(ram_data[Moving_Speed_H]<<8);
+ maximum_position=ram_data[CCW_Angle_Limit_L]+(ram_data[CCW_Angle_Limit_H]<<8);
+ minimum_position=ram_data[CW_Angle_Limit_L]+(ram_data[CW_Angle_Limit_H]<<8);
+ torque_limit=ram_data[Torque_Limit_L]+(ram_data[Torque_Limit_H]<<8);
+ p_gain=ram_data[KP_L]+(ram_data[KP_H]<<8);
+ i_gain=ram_data[KI_L]+(ram_data[KI_H]<<8);
+ d_gain=ram_data[KD_L]+(ram_data[KD_H]<<8);
+ /* saturate the target position */
+ if (seek_position < minimum_position)
+ {
+ seek_position = minimum_position;
+ comm_set_error(ANGLE_LIMIT_ERROR);
+ }
+ if (seek_position > maximum_position)
+ {
+ seek_position = maximum_position;
+ comm_set_error(ANGLE_LIMIT_ERROR);
+ }
+ /* update the new target */
+ if(previous_seek != seek_position)
+ {
+ previous_seek=seek_position;
+ seek_delta=current_position;
+ start_position=current_position;
+ }
+ /* generate the intermediate target point */
+ if(count==CONTROL_COUNT && seek_delta!=seek_position && seek_velocity>0) // Tick is our time constant
+ {
+ if(start_position<seek_position)
+ {
+ seek_delta+=seek_velocity;
+ if(seek_delta>=seek_position)
+ seek_delta=seek_position;
+ }
+ else if(start_position>seek_position)
+ {
+ seek_delta-=seek_velocity;
+ if(seek_delta<=seek_position)
+ seek_delta=seek_position;
+ }
+ }
+ /* compute the error */
+ p_component = seek_delta - current_position;
+ if(count==CONTROL_COUNT)
+ {
+ i_component += p_component;
+ if(i_component<-128) // Somewhat arbitrary anti integral wind-up; we're experimenting
+ i_component=-128;
+ else if(i_component>128)
+ i_component=128;
+ }
+ d_component = previous_position - current_position;
+ previous_position = current_position;
+ /* compute the PWM */
+ pwm_output = 0;
+ if ((p_component > deadband) || (p_component < -deadband))
+ {
+ // Apply the proportional component of the PWM output.
+ pwm_output += (int32_t) p_component * (int32_t) p_gain;
+ // Apply the integral component of the PWM output.
+// pwm_output += (int32_t) i_component * (int32_t) i_gain;
+ // Apply the derivative component of the PWM output.
+// pwm_output += (int32_t) d_component * (int32_t) d_gain;
+ }
+ else
+ i_component = 0;
+ pwm_output>>=8;
+ if(pwm_output>torque_limit)
+ pwm_output=torque_limit;
+ else if(pwm_output<-torque_limit)
+ pwm_output=-torque_limit;
+ if(pwm_output<0)
+ {
+ pwm_output=-pwm_output;
+ motor_set_pwm((uint16_t)pwm_output,0x0000);
+ }
+ else
+ motor_set_pwm(0x0000,(uint16_t)pwm_output);
+ ram_data[Present_Load_L]=pwm_output&0xFF;
+ ram_data[Present_Load_H]=((pwm_output>>8)&0xFF);
+ /* update control loop variable */
+ if(count==CONTROL_COUNT)
+ count=0;
+ else
+ count++;
+ }
+}
diff --git a/servo_firmware/ax12/src/sensors.c b/servo_firmware/ax12/src/sensors.c
new file mode 100644
index 0000000000000000000000000000000000000000..39c452ac41c0d6e5eb056e073c82f4101d7d401b
--- /dev/null
+++ b/servo_firmware/ax12/src/sensors.c
@@ -0,0 +1,78 @@
+#include "sensors.h"
+#include "adc.h"
+#include "comm.h"
+#include "mem.h"
+#include "dyn_slave.h"
+
+typedef enum {adc_get_pos=0,adc_get_temp=1,adc_get_volt=2} adc_state;
+
+uint16_t current_position;
+adc_state state;
+
+void sensors_set_pos(uint16_t pos)
+{
+ ram_data[Present_Position_L]=pos&0xFF;
+ ram_data[Present_Position_H]=(pos>>8)&0xFF;
+ current_position=pos;
+}
+
+void sensors_set_temp(uint16_t temp)
+{
+ uint8_t temperature;
+
+ temperature=(((uint32_t)temp*50)>>10);
+ ram_data[Present_Temperature]=temperature;
+ // check the limits and generate the error
+ if(temperature>ram_data[Int_Limit_Temperature])
+ comm_set_error(OVERHEATING_ERROR);
+ else
+ comm_set_error(NO_ERROR);
+}
+
+void sensors_set_voltage(uint16_t voltage)
+{
+ uint8_t volt;
+
+ volt=(((uint32_t)voltage*200)>>10);
+ ram_data[Present_Voltage]=volt;
+ // check the limits and generate the error
+ if(volt>ram_data[High_Limit_Voltage] || volt<ram_data[Low_Limit_Voltage])
+ comm_set_error(VOLTAGE_ERROR);
+ else
+ comm_set_error(NO_ERROR);
+}
+
+void sensors_init(void)
+{
+ // start the voltage conversion
+ adc_start(ADC_pote_Port);
+ while(!adc_is_done());
+ sensors_set_pos(adc_get_value());
+ adc_start(ADC_temp_Port);
+ state=adc_get_temp;
+}
+
+void sensors_loop(void)
+{
+ if(adc_is_done())// the last AD conversion has finished
+ {
+ if(state==adc_get_pos)
+ {
+ sensors_set_pos(adc_get_value());
+ adc_start(ADC_temp_Port);
+ state=adc_get_temp;
+ }
+ else if(state==adc_get_temp)
+ {
+ sensors_set_temp(adc_get_value());
+ adc_start(ADC_voltage_Port);
+ state=adc_get_volt;
+ }
+ else
+ {
+ sensors_set_voltage(adc_get_value());
+ adc_start(ADC_pote_Port);
+ state=adc_get_pos;
+ }
+ }
+}
diff --git a/servo_firmware/ax12/src/time_base.c b/servo_firmware/ax12/src/time_base.c
new file mode 100644
index 0000000000000000000000000000000000000000..f8974c5d8e1f3acdd337fd64a0f140dae45229f6
--- /dev/null
+++ b/servo_firmware/ax12/src/time_base.c
@@ -0,0 +1,24 @@
+#include "time_base.h"
+
+void time_base_init(void)
+{
+ TCCR0|=(1<<CS01)|(1<<CS00); // clk src with prescaler 64
+ TIMSK=0x00; // disable all interrupts
+ TCNT0 = 0x00;
+}
+
+uint8_t time_base_period(void)
+{
+ if(TIFR & (1<<TOV0))
+ {
+ TIFR|=(1<<TOV0);
+ return 0x01;
+ }
+ else
+ return 0x00;
+}
+
+uint8_t time_base_get_counter(void)
+{
+ return TCNT0;
+}