Minor improvements on the communication protocol implementation.

servo_firmware/ax12/src/comm.c

@@ -2,6 +2,7 @@
 #include "dyn_slave.h"
 #include "mem.h"
 #include "gpio.h"
+#include "motor_control.h"
 
 uint8_t dyn_error;
 uint8_t tmp_data[RAM_SIZE];
@@ -28,9 +29,15 @@ void do_write(uint8_t address,uint8_t length,uint8_t *data)
       if(data[num]==0x00) gpio_led_off();
       else gpio_led_on();
     }
+    else if(i==Torque_Enable)
+    {
+      if(data[num]==0x00)
+        motor_set_pwm(0x0000,0x0000);      
+    }
+    else if(i==Goal_Position_L || i==Goal_Position_H)
+      ram_data[Torque_Enable]=0x01;
     num++;
   } 
-  ram_write(address,length,data);
 }
 
 void comm_init(void)

servo_firmware/ax12/src/dyn_slave.c

@@ -2,6 +2,7 @@
 #include <avr/io.h>
 #include "dyn_slave.h"
 #include "gpio.h"
+#include "mem.h"
 
 uint8_t dyn_slave_id;
 uint8_t dyn_slave_data[128];
@@ -58,8 +59,12 @@ ISR(USART_RXC_vect)
             dyn_slave_num_rx_bytes++;
             break;
     default: if(dyn_slave_num_rx_bytes==length)
+             {
+               dyn_slave_num_rx_bytes=0;
                dyn_slave_packet_ready=1;
-             dyn_slave_num_rx_bytes++;
+             }
+             else
+               dyn_slave_num_rx_bytes++;
              break;
   }
   sei();// enable all interrutps
@@ -171,8 +176,7 @@ uint8_t dyn_slave_sync_read_id_present(void)
 {
   uint8_t i,num;
 
-  num=dyn_slave_num_rx_bytes-8;
-  dyn_slave_num_rx_bytes=0;
+  num=dyn_slave_data[3]-4;
   for(i=0;i<num;i++)
   {
     if(dyn_slave_data[7+i]==dyn_slave_id)
@@ -191,8 +195,7 @@ uint8_t dyn_slave_sync_write_id_present(uint8_t **data)
 {
   uint8_t i,num;
 
-  num=(dyn_slave_num_rx_bytes-8)/(dyn_slave_data[6]+1);
-  dyn_slave_num_rx_bytes=0;
+  num=(dyn_slave_data[3]-4)/(dyn_slave_data[6]+1);
   for(i=0;i<num;i++)
   {
     if(dyn_slave_data[7+i*(dyn_slave_data[6]+1)]==dyn_slave_id)
@@ -209,8 +212,7 @@ uint8_t dyn_slave_bulk_read_id_present(uint8_t *address,uint8_t *length)
 {
   uint8_t i,num;
 
-  num=(dyn_slave_num_rx_bytes-6)/3;
-  dyn_slave_num_rx_bytes=0;
+  num=(dyn_slave_data[3]-2)/3;
   for(i=0;i<num;i++)
   {
     if(dyn_slave_data[5+i*3]==dyn_slave_id)
@@ -231,8 +233,7 @@ uint8_t dyn_slave_bulk_write_id_present(uint8_t *address,uint8_t *length,uint8_t
 {
   uint8_t i,num;
 
-  num=dyn_slave_num_rx_bytes;
-  dyn_slave_num_rx_bytes=0;
+  num=dyn_slave_data[3]+4;
   for(i=5;i<num;)
   {
     if(dyn_slave_data[i]==dyn_slave_id)
@@ -252,25 +253,31 @@ 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+2;
-  dyn_slave_data[4]=error;
-  cs+=error;
-  for(i=0;i<length;i++)
+  if(dyn_slave_data[2]!=0xFE)
   {
-    dyn_slave_data[5+i]=data[i];
-    cs+=data[i];
+    if(dyn_slave_data[4]==INST_PING || ((dyn_slave_data[4]==INST_READ || 
+       dyn_slave_data[4]==INST_SYNC_READ || dyn_slave_data[4]==INST_BULK_READ) && 
+       ram_data[Status_Return_Level]!=0x00) || ram_data[Status_Return_Level]==0x02) 
+    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+2;
+    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
+    dyn_slave_num_tx_bytes=1;
+    UDR=dyn_slave_data[0];
   }
-  dyn_slave_data[5+i]=~cs;
-  // set in tex mode
-  dyn_slave_set_tx();
-  // start transmission
-  dyn_slave_num_tx_bytes=1;
-  UDR=dyn_slave_data[0];
 }
 
 uint8_t dyn_slave_is_send_done(void)

servo_firmware/ax12/src/mem.c

@@ -12,7 +12,7 @@ void ram_init(void)
 {
   uint8_t i;
 
-  for(i=0;i<Alarm_Shutdown;i++)
+  for(i=0;i<=Alarm_Shutdown;i++)
     ram_data[i]=eeprom_read_byte(&eeprom_data[i]);
   for(;i<=KD_H;i++)
     ram_data[i]=0x00;

servo_firmware/ax12/src/motor_control.c

@@ -138,15 +138,18 @@ void motor_control_loop(void)
       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);
+//    if(ram_data[Torque_Enable])
+//    {
+      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;