diff --git a/servo_firmware/ax12/include/mem.h b/servo_firmware/ax12/include/mem.h
index b39ab683f5939153e6e31f3cd70a7954a49a61cb..673b56ad67f11e85102ab5f483132180c4363730 100644
--- a/servo_firmware/ax12/include/mem.h
+++ b/servo_firmware/ax12/include/mem.h
@@ -50,8 +50,11 @@
#define Lock 0X2F //47 - 0x00 R/W - Locking EEPROM
#define KD_L 0X30 //48 - 0X20 R/W - Lowest byte of Punch
#define KD_H 0X31 //49 - 0X00 R/W - Highest byte of Punch
+#define DeadZone 0x32
+#define I_Clamp_L 0x33
+#define I_Clamp_H 0x34
-#define RAM_SIZE 50
+#define RAM_SIZE 53
extern unsigned char ram_data[RAM_SIZE];
diff --git a/servo_firmware/ax12/src/mem.c b/servo_firmware/ax12/src/mem.c
index bd86da21220fc0ea455eeae315c6b578e22a3763..214779b4017b92736d305af1e58ec182f05b0073 100644
--- a/servo_firmware/ax12/src/mem.c
+++ b/servo_firmware/ax12/src/mem.c
@@ -3,7 +3,7 @@
#include <avr/eeprom.h>
// dynamixel RAM variables
-unsigned char ram_data[50];
+unsigned char ram_data[RAM_SIZE];
// Dynamixel EEPROM variables
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};
@@ -14,7 +14,7 @@ void ram_init(void)
for(i=0;i<=Alarm_Shutdown;i++)
ram_data[i]=eeprom_read_byte(&eeprom_data[i]);
- for(;i<=KD_H;i++)
+ for(;i<RAM_SIZE;i++)
ram_data[i]=0x00;
ram_data[KP_L]=0x20;
ram_data[KP_H]=0x00;
@@ -24,7 +24,7 @@ void ram_init(void)
uint8_t ram_read(uint8_t address, uint8_t length, uint8_t **data)
{
- if((address+length)>KD_H)
+ if((address+length)>RAM_SIZE)
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)>KD_H)
+ if((address+length)>RAM_SIZE)
return 0x00;
else
{
diff --git a/servo_firmware/ax12/src/motor_control.c b/servo_firmware/ax12/src/motor_control.c
index 77053e3f000b7653d3597d9f723d4e4a9b086322..d2b9a00a504c47650187ff3a74b99895b8d96273 100644
--- a/servo_firmware/ax12/src/motor_control.c
+++ b/servo_firmware/ax12/src/motor_control.c
@@ -8,12 +8,13 @@
#define CW_PWM_MOTOR_PIN PB1
#define CCW_PWM_MOTOR_PIN PB2
-#define CONTROL_COUNT 10
+#define CONTROL_COUNT 7
int16_t start_position;
int16_t previous_position;
int16_t previous_seek;
int16_t seek_delta;
+int32_t seek_delta_shift;
int16_t count;
void motor_init(void)
@@ -24,13 +25,14 @@ void motor_init(void)
/* 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
+ TCCR1B|=(1 << WGM12)| (1 << CS10) ; // set Mode Fast PWM, 10bit, prescaler 1
// 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;
+ seek_delta_shift=-1;
count=0;
}
@@ -51,8 +53,9 @@ void motor_control_loop(void)
static int16_t p_component;
static int16_t d_component;
static int16_t i_component;
+ static int16_t i_clamp;
static int16_t torque_limit;
- static int16_t deadband=10;
+ static int16_t deadband;
static int32_t pwm_output;
static uint16_t p_gain;
static uint16_t i_gain;
@@ -62,6 +65,7 @@ void motor_control_loop(void)
{
// update control loop
// get all necessary variables
+ gpio_led_on();
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;
@@ -71,6 +75,8 @@ void motor_control_loop(void)
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);
+ deadband=ram_data[DeadZone];
+ i_clamp=ram_data[I_Clamp_L]+(ram_data[I_Clamp_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);
@@ -90,6 +96,7 @@ void motor_control_loop(void)
{
previous_seek=seek_position;
seek_delta=current_position;
+ seek_delta_shift=((uint32_t)current_position)*64;
start_position=current_position;
}
/* generate the intermediate target point */
@@ -97,13 +104,15 @@ void motor_control_loop(void)
{
if(start_position<seek_position)
{
- seek_delta+=seek_velocity;
+ seek_delta_shift+=seek_velocity;
+ seek_delta=seek_delta_shift/64;
if(seek_delta>=seek_position)
seek_delta=seek_position;
}
else if(start_position>seek_position)
{
- seek_delta-=seek_velocity;
+ seek_delta_shift-=seek_velocity;
+ seek_delta=seek_delta_shift/64;
if(seek_delta<=seek_position)
seek_delta=seek_position;
}
@@ -113,10 +122,10 @@ void motor_control_loop(void)
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;
+ if(i_component<-i_clamp) // Somewhat arbitrary anti integral wind-up; we're experimenting
+ i_component=-i_clamp;
+ else if(i_component>i_clamp)
+ i_component=i_clamp;
}
d_component = previous_position - current_position;
previous_position = current_position;
@@ -127,12 +136,12 @@ void motor_control_loop(void)
// 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;
+ 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;
+ pwm_output += (int32_t) d_component * (int32_t) d_gain;
}
- else
- i_component = 0;
+// else
+// i_component = 0;
pwm_output>>=8;
if(pwm_output>torque_limit)
pwm_output=torque_limit;
@@ -155,5 +164,6 @@ void motor_control_loop(void)
count=0;
else
count++;
+ gpio_led_off();
}
}