From 9510906c5e492900a76080e732fadf4474aa5ac0 Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Sergi=20Hern=C3=A1ndez?= <shernand@iri.upc.edu>
Date: Mon, 25 Nov 2013 12:01:23 +0000
Subject: [PATCH] Solved a problem in the communications. The higher priority
of the timer 0 was the cause of the loss of some data bytes, causing
intermitent communication errors.
---
servo_firmware/ax12/src/CFG_HW_Dynamixel.c | 1 +
servo_firmware/ax12/src/TXRX_Dynamixel.c | 5 +--
servo_firmware/ax12/src/main.c | 45 ++++++++++++++--------
3 files changed, 32 insertions(+), 19 deletions(-)
diff --git a/servo_firmware/ax12/src/CFG_HW_Dynamixel.c b/servo_firmware/ax12/src/CFG_HW_Dynamixel.c
index 5eda0e9..10ae74a 100755
--- a/servo_firmware/ax12/src/CFG_HW_Dynamixel.c
+++ b/servo_firmware/ax12/src/CFG_HW_Dynamixel.c
@@ -66,6 +66,7 @@ void Config_Hardware(void)
/* timer/counter0 limitations.. no CTC mode.
* so move starting point to where neded
* to do is add X */
+ TCNT0 = 0x00;
// TCNT0 = 0x9C; // Timer/Counter Register 156 => 0.1 ms counter value set
//*****************************************
diff --git a/servo_firmware/ax12/src/TXRX_Dynamixel.c b/servo_firmware/ax12/src/TXRX_Dynamixel.c
index 56a5a58..a43eeef 100755
--- a/servo_firmware/ax12/src/TXRX_Dynamixel.c
+++ b/servo_firmware/ax12/src/TXRX_Dynamixel.c
@@ -43,14 +43,11 @@ void RS485_send(unsigned char data)
unsigned char RS485_receive(unsigned char *data)
{
- cli();
if(num_data==0)
- {
- sei();
return 0;
- }
else
{
+ cli();
*data=buffer_data[read_ptr];
read_ptr++;
num_data--;
diff --git a/servo_firmware/ax12/src/main.c b/servo_firmware/ax12/src/main.c
index 51fc747..ac25c6a 100755
--- a/servo_firmware/ax12/src/main.c
+++ b/servo_firmware/ax12/src/main.c
@@ -14,9 +14,14 @@
//*************CTRL = INTERRUPT FUNCTION *********************************
uint16_t count = 0;
uint16_t count2 = 0;
-/*ISR( TIMER0_OVF_vect) {
+
+unsigned char do_control;
+
+ISR( TIMER0_OVF_vect) {
int16_t TorqueEnable;
+ TCNT0 = 0x00;
+
sei();
TorqueEnable = Read_byte_Dynamixel_RAM(Torque_Enable);
if (TorqueEnable == 1) {
@@ -25,11 +30,12 @@ uint16_t count2 = 0;
//TIMSK &= ~( 1 << TOIE0); // disable timer0
count++;
if (count == 2) {
+ do_control=1;
// 1.3s passed
// LedTONGGLE();
- Control_Cycle();
- HW_Security();
- Write_Actuator();
+ //Control_Cycle();
+ //HW_Security();
+ //Write_Actuator();
count = 0;
}
//TIMSK |= ( 1 << TOIE0); // enable timer0
@@ -38,43 +44,45 @@ uint16_t count2 = 0;
OCR1A = CTRL_ZERO; //PB1 => set PWM for X% duty cycle
OCR1B = CTRL_ZERO; //PB2 => set PWM for Y% duty cycle
}
-}*/
+}
//**************** 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,
+ 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 };*/
+ 84, 85, 86 };
// ini eeprom if first time after run reflash system
- /*if (eeprom_read_byte((uint8_t*) Gate_Restore_Eeprom) != 0xCC) {
+ if (eeprom_read_byte((uint8_t*) Gate_Restore_Eeprom) != 0xCC) {
Restore_Eeprom_Factory_Values(); //once this procedure is held, no more initialization is performed
//blinkLedN(3);
- }*/
+ }
//------EEPROM initial values-------------
- /*Restore_EepromVAR();
+ Restore_EepromVAR();
//blinkLedN(5);
// get value of Motor ID
- rs485_address = eepromVAR[ID];*/
+ rs485_address = eepromVAR[ID];
// configure AVR chip i/o
Config_Hardware();
- /*// asign actual position to goal position and asume zero motor turns
+ // asign actual position to goal position and asume zero motor turns
Ini_Position();
// initialize the RS485 interface
- init_RS485();*/
+ init_RS485();
// end of inicialization
LedOFF;
+ do_control=0;
+
while (1) {
//LedOFF;
- /*status = RxRS485Packet(&id, &instruction, &length, data);
+ status = RxRS485Packet(&id, &instruction, &length, data);
if (status == CHECK_ERROR) {
TxRS485Packet(rs485_address, CHECKSUM_ERROR, 0, NULL);
} else if (status == CORRECT) {
@@ -251,7 +259,14 @@ int16_t main(void) {
//***************************************************
}
}
- }*/
+ }
+ else if(do_control)
+ {
+ Control_Cycle();
+ HW_Security();
+ Write_Actuator();
+ do_control=0;
+ }
}
}
--
GitLab