Increased the PWM frequency.

Solved a problem in the PID controller.
Allowed minimum speeds of <1º/s.

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];
 

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
   {

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();
   }
 }