diff --git a/servo_firmware/ax12/Makefile b/servo_firmware/ax12/Makefile
index 8bdf19a7477384333d9c2fd04ea10ab21736832d..2d1411a4af7a15310e7fb0c0484a1ebdae5f4bad 100755
--- a/servo_firmware/ax12/Makefile
+++ b/servo_firmware/ax12/Makefile
@@ -1,4 +1,4 @@
-PROJECT=rx28_fw
+PROJECT=ax12_fw
########################################################
# afegir tots els fitxers que s'han de compilar aquÃ
########################################################
@@ -30,7 +30,7 @@ $(PROJECT).elf: $(OBJS)
$(CC) -c $(CFLAGS) -I$(INCLUDE_DIR) -o $@ $<
download: $(MAIN_OUT_HEX)
- ../../bin/fw_downloader -d /dev/ttyUSB0 -f $(PROJECT).hex -s rx28
+ ../../bin/fw_downloader -d /dev/ttyUSB0 -f $(PROJECT).hex -s ax12
clean:
rm $(PROJECT).*
diff --git a/servo_firmware/ax12/include/CFG_HW_Dynamixel.h b/servo_firmware/ax12/include/CFG_HW_Dynamixel.h
index cee708ddc68e992f8e02c5deb912d4140ef682f5..eb8bb8abab688a4ee761115ce5daf0918c0fe467 100755
--- a/servo_firmware/ax12/include/CFG_HW_Dynamixel.h
+++ b/servo_firmware/ax12/include/CFG_HW_Dynamixel.h
@@ -13,7 +13,7 @@
#define LedOFF PORTD |= (1<<LED_PIN) // off LED
#define LedON PORTD &= ~(1<<LED_PIN) // ON LED
-#define LedTOGGLE PORTD ^= _BV(LED_PIN) // Toggle LED
+//#define LedTOGGLE PORTD ^= _BV(LED_PIN) // Toggle LED
#define SET_CW_PWM_MOTOR(x) OCR1A = (x) //PB1 => set PWM for X% duty cycle @ 10bit
#define SET_CCW_PWM_MOTOR(x) OCR1B = (x) //PB2 => set PWM for Y% duty cycle @ 10bit
diff --git a/servo_firmware/ax12/include/CTRL_Dynamixel.h b/servo_firmware/ax12/include/CTRL_Dynamixel.h
index c619e44895058f1485727061f2a348ba04f76d9f..3d84ad06d08658626ec6b020b97add83f0839cfe 100755
--- a/servo_firmware/ax12/include/CTRL_Dynamixel.h
+++ b/servo_firmware/ax12/include/CTRL_Dynamixel.h
@@ -17,9 +17,5 @@ void Ini_Position(void);
int16_t Read_Sensor(uint8_t port);
// control cycle
void Control_Cycle(void);
- // limit control action ... max value and alarms
-void HW_Security(void);
- // place control action on output
-void Write_Actuator(void);
-#endif
\ No newline at end of file
+#endif
diff --git a/servo_firmware/ax12/include/MEM_Dynamixel.h b/servo_firmware/ax12/include/MEM_Dynamixel.h
index 0c91efed71de92831a18bb4b212f5e820434014a..c14dc7695c8308d47111c2f32bd400732c11c3fb 100755
--- a/servo_firmware/ax12/include/MEM_Dynamixel.h
+++ b/servo_firmware/ax12/include/MEM_Dynamixel.h
@@ -1,6 +1,7 @@
#ifndef _MEM_DYNAMIXEL_H
#define _MEM_DYNAMIXEL_H
+
// Register Id EEPROM - declared in dynamixel
#define Model_Number_L 0X00 //0 - 0X0C R - Lowest byte of model number
#define Model_Number_H 0X01 //1 - 0X00 R - Highest byte of model number
@@ -105,12 +106,22 @@
#define high(x) ((uint8_t)(((x)>>8) & 0xFF))
#define TwoBytesToInt(h,l) ((int16_t)(((h)<<8)|(l)))
+#define Read_byte_Dynamixel_EEPROM(c,a) ({\
+c=eepromVAR[a]; \
+})
+
+//#define Read_word_Dynamixel_RAM(c,a) ({\
+c = TwoBytesToInt(sramVAR[a-RAM_ID_correction+1],sramVAR[a-RAM_ID_correction]); \
+})
+
+
+
extern uint8_t eepromVAR[];
extern uint8_t sramVAR[];
// function to read variable from emprom
-uint8_t Read_byte_Dynamixel_EEPROM(uint8_t IDregister);
+//uint8_t Read_byte_Dynamixel_EEPROM(uint8_t IDregister);
// function to read variable to emprom
void Write_byte_Dynamixel_EEPROM(uint8_t IDregister, uint8_t Value);
// function to read variable from RAM
diff --git a/servo_firmware/ax12/src/CFG_HW_Dynamixel.c b/servo_firmware/ax12/src/CFG_HW_Dynamixel.c
index 10ae74a9610e43b0fa94bfab6f57d12be9fdde84..cea7e34dcaa910ad380c1f9ae3eb3b0804c20d1d 100755
--- a/servo_firmware/ax12/src/CFG_HW_Dynamixel.c
+++ b/servo_firmware/ax12/src/CFG_HW_Dynamixel.c
@@ -1,5 +1,5 @@
#include <avr/io.h>
-#include <util/delay.h>
+//#include <util/delay.h>
#include <avr/interrupt.h>
#include "CFG_HW_Dynamixel.h"
diff --git a/servo_firmware/ax12/src/CTRL_Dynamixel.c b/servo_firmware/ax12/src/CTRL_Dynamixel.c
index 0aa66ad5ee51dbad0c4f1a2de37da45a494f8f33..b7ff64dbf8ef660a0d11fb73f7cdb37427cd300b 100755
--- a/servo_firmware/ax12/src/CTRL_Dynamixel.c
+++ b/servo_firmware/ax12/src/CTRL_Dynamixel.c
@@ -1,28 +1,48 @@
#include <avr/interrupt.h>
#include <avr/io.h>
#include "MEM_Dynamixel.h"
+#include "CFG_HW_Dynamixel.h"
#include "CTRL_Dynamixel.h"
#define INT16_MAX 0x7fff
#define INT16_MIN (-INT16_MAX - 1)
-#define ADD_A_B(c,a,b) ({\
+//#define ADD_A_B(c,a,b) ({\
int32_t x = (int32_t)a+b; \
(x>32767)?(c = (int16_t)32767) : ((x<-32768)?(c = (int16_t)-32768):(c = (int16_t) x)); \
})
-#define MULT_A_B(c,a,b) ({\
+int16_t ADD_A_B(int16_t a, int16_t b)
+{
+ int16_t c;
+
+ int32_t x = (int32_t)a+b;
+ (x>32767)?(c = (int16_t)32767) : ((x<-32768)?(c = (int16_t)-32768):(c = (int16_t) x));
+
+ return c;
+}
+
+//#define MULT_A_B(c,a,b) ({\
int32_t x = (int32_t)a*b; \
(x>32767)?(c = (int16_t)32767) : ((x<-32768)?(c = (int16_t)-32768):(c = (int16_t) x)); \
})
+
+int16_t MULT_A_B(int16_t a, int16_t b)
+{
+ int16_t c;
+
+ int32_t x = (int32_t)a*b;
+ (x>32767)?(c = (int16_t)32767) : ((x<-32768)?(c = (int16_t)-32768):(c = (int16_t) x));
+
+ return c;
+}
#define num_to_FFFFbit(c,a) ({\
-double x = (double)a; \
+float x = (float)a; \
(x>32767)?(c = (int16_t)32767) : ((x<-32768)?(c = (int16_t)-32768):(c = (int16_t) x)); \
})
-
void Ini_Position(void)
{
int16_t Sensor_Value;
@@ -58,85 +78,92 @@ int16_t Read_Sensor(uint8_t port)
}
+
void Control_Cycle(void)
{
-
- int16_t Motor_Turns,Sensor_Value,Present_Position,Goal_Position;
- //int16_t Motor_Accion,MotorDir,Error_Vector_0,Error_Vector_1,Error_Vector_2,Error_Vector_3;
- int16_t Motor_Accion,MotorDir,Error_Vector_0,Error_Vector_1,Error_Vector_2;
- //int16_t Integral_Value,Max_Integral_Value,Error_Margin,KP,KD,KI,Sensor_Resol, Max_Sensor_Range;
+
+ int16_t Motor_Turns,Sensor_Value,Present_Position,Goal_Position;
+ int16_t Motor_Action,MotorDir,Error_Vector_0,Error_Vector_1,Error_Vector_2;
+ //int16_t Error_Vector_3;
int16_t Error_Margin,KP,KD,Sensor_Resol, Max_Sensor_Range;
+ //int16_t Integral_Value,Max_Integral_Value,KI ;
+ uint16_t PWM_Value;
+ int16_t Dead_Zone,Max_PWM_Value,Temp;
uint8_t Transition_Stage_Value;
- int32_t Motor_Accion_P=0;
- int32_t Motor_Accion_D=0;
- //int32_t Motor_Accion_I=0;
-
- // update position sensor value
- Sensor_Value = Read_Sensor(CTRL_Encoder_Port);
- Write_word_Dynamixel_RAM(Sensor_Value_L,Sensor_Value);
-
- // find direction of accion
- Motor_Accion = Read_word_Dynamixel_RAM(Motor_Accion_L);
- if (Motor_Accion<CTRL_ZERO){MotorDir=CTRL_Dir_CW;}
- else{MotorDir=CTRL_Dir_CCW;}
+ int32_t Motor_Action_P=0;
+ int32_t Motor_Action_D=0;
+ //int32_t Motor_Action_I=0;
- // update motor turns
- Transition_Stage_Value = Read_byte_Dynamixel_RAM(Transition_Stage);
- Motor_Turns=Read_word_Dynamixel_RAM(Motor_Turns_L);
-
- switch(Transition_Stage_Value) {
- case 0: //-3
+ // update position sensor value
+ Sensor_Value = Read_Sensor(CTRL_Encoder_Port);
+ Write_word_Dynamixel_RAM(Sensor_Value_L,Sensor_Value);
+
+ // find direction of action
+ Motor_Action = Read_word_Dynamixel_RAM(Motor_Accion_L);
+ //Read_word_Dynamixel_RAM(Motor_Action,Motor_Accion_L);
+ if (Motor_Action<CTRL_ZERO){MotorDir=CTRL_Dir_CW;}
+ else{MotorDir=CTRL_Dir_CCW;}
+
+ // update motor turns
+ Transition_Stage_Value = Read_byte_Dynamixel_RAM(Transition_Stage);
+ Motor_Turns=Read_word_Dynamixel_RAM(Motor_Turns_L);
+ //Read_word_Dynamixel_RAM(Motor_Turns,Motor_Turns_L);
+ //
+ switch(Transition_Stage_Value) {
+ case 0: //-3
if (Sensor_Value!=1023){
if (MotorDir==CTRL_Dir_CW){ Transition_Stage_Value=3; //0
Motor_Turns=Motor_Turns-1;
- }else { Transition_Stage_Value=1;} //-2
- }
+ }else { Transition_Stage_Value=1;} //-2
+ }
break; //...............................................
- case 1: //-2
+ case 1: //-2
if (Sensor_Value==0){ Transition_Stage_Value=2; //-1
- }else if (Sensor_Value==1023){ Transition_Stage_Value=0;} //-3
+ }else if (Sensor_Value==1023){ Transition_Stage_Value=0;} //-3
break; //...............................................
- case 2: //-1
+ case 2: //-1
if (Sensor_Value!=0){
if (MotorDir==CTRL_Dir_CW){ Transition_Stage_Value=1; //-2
- }else { Transition_Stage_Value=3;} //0
+ }else { Transition_Stage_Value=3;} //0
}
break; //...............................................
- case 3: //0
+ case 3: //0
if (Sensor_Value==0) { Transition_Stage_Value=2; //-1
- }else if (Sensor_Value==1023) { Transition_Stage_Value=4;} //1
+ }else if (Sensor_Value==1023) { Transition_Stage_Value=4;} //1
break; //...............................................
- case 4: //1
+ case 4: //1
if (Sensor_Value!=1023){
if (MotorDir==CTRL_Dir_CCW){ Transition_Stage_Value=5; //2
- }else { Transition_Stage_Value=3;} //0
+ }else { Transition_Stage_Value=3;} //0
}
break; //...............................................
- case 5: //2
+ case 5: //2
if (Sensor_Value==1023){ Transition_Stage_Value=4; //1
- }else if (Sensor_Value==0){ Transition_Stage_Value=6;} //3
+ }else if (Sensor_Value==0){ Transition_Stage_Value=6;} //3
break; //...............................................
- case 6: //3
+ case 6: //3
if (Sensor_Value!=0){
if (MotorDir==CTRL_Dir_CCW){ Transition_Stage_Value=3; //0
Motor_Turns=Motor_Turns+1;
- }else { Transition_Stage_Value=5;} //2
+ }else { Transition_Stage_Value=5;} //2
}
break; //...............................................
- default:
+ default:
;
break; //...............................................
- }
- Write_word_Dynamixel_RAM(Motor_Turns_L,Motor_Turns);
- Write_byte_Dynamixel_RAM(Transition_Stage,Transition_Stage_Value);
-
-
+ }
+ Write_word_Dynamixel_RAM(Motor_Turns_L,Motor_Turns);
+ Write_byte_Dynamixel_RAM(Transition_Stage,Transition_Stage_Value);
+
// calc Position including motor turns
- Sensor_Resol=Read_word_Dynamixel_RAM(Sensor_Resol_L);
- Max_Sensor_Range=Read_word_Dynamixel_RAM(Max_Sensor_Range_L);
- Present_Position = (int16_t) ((double)Motor_Turns*Sensor_Resol*360/ Max_Sensor_Range); // OJO max 26 vueltas
- ADD_A_B(Present_Position,Present_Position,Sensor_Value);
+ Sensor_Resol=Read_word_Dynamixel_RAM(Sensor_Resol_L);
+ //Read_word_Dynamixel_RAM(Sensor_Resol,Sensor_Resol_L);
+ Max_Sensor_Range=Read_word_Dynamixel_RAM(Max_Sensor_Range_L);
+ //Read_word_Dynamixel_RAM(Max_Sensor_Range,Max_Sensor_Range_L);
+ Present_Position = (int16_t) ((float)Motor_Turns*Sensor_Resol*360/ Max_Sensor_Range); // OJO max 26 vueltas
+ //ADD_A_B(Present_Position,Present_Position,Sensor_Value);
+ Present_Position=ADD_A_B(Present_Position,Sensor_Value);
Write_word_Dynamixel_RAM(Present_Position_L,Present_Position);
// Update Error Vector 3
@@ -144,73 +171,87 @@ void Control_Cycle(void)
//Write_word_Dynamixel_RAM(Error_Vector_3_L,Error_Vector_3);
// Update Error Vector 2
Error_Vector_2=Read_word_Dynamixel_RAM(Error_Vector_1_L);
+ //Read_word_Dynamixel_RAM(Error_Vector_2,Error_Vector_1_L);
Write_word_Dynamixel_RAM(Error_Vector_2_L,Error_Vector_2);
// Update Error Vector 1
Error_Vector_1=Read_word_Dynamixel_RAM(Error_Vector_0_L);
+ //Read_word_Dynamixel_RAM(Error_Vector_1,Error_Vector_0_L);
Write_word_Dynamixel_RAM(Error_Vector_1_L,Error_Vector_1);
// Update Error Vector 0
Goal_Position=Read_word_Dynamixel_RAM(Goal_Position_L);
- MULT_A_B(Present_Position,Present_Position,-1);
- ADD_A_B(Error_Vector_0,Goal_Position,Present_Position);
+ //Read_word_Dynamixel_RAM(Goal_Position,Goal_Position_L);
+ //MULT_A_B(Present_Position,Present_Position,-1);
+ Present_Position=MULT_A_B(Present_Position,-1);
+ //ADD_A_B(Error_Vector_0,Goal_Position,Present_Position);
+ Error_Vector_0=ADD_A_B(Goal_Position,Present_Position);
Write_word_Dynamixel_RAM(Error_Vector_0_L,Error_Vector_0);
- // range where small errors are discarted
- Error_Margin = Read_word_Dynamixel_RAM(Error_Margin_L);
- if (Error_Vector_0<Error_Margin){
- if (Error_Vector_0>(-1*Error_Margin)){ Write_word_Dynamixel_RAM(Motor_Accion_L,0);
- return; }
- }
// CTRL_Law - algorithm http://lorien.ncl.ac.uk/ming/digicont/digimath/dpid1.htm
KP = Read_word_Dynamixel_RAM(KP_L);
- Motor_Accion_P = (int32_t)Error_Vector_0*KP/10;
+ //Read_word_Dynamixel_RAM(KP,KP_L);
+ Motor_Action_P = ((int32_t)Error_Vector_0*KP)>>8;// coste de dividir /10 es mayor que shift 4 que equivale a dividir por 16;
KD = Read_word_Dynamixel_RAM(KD_L);
- Motor_Accion_D= ((int32_t)Error_Vector_0- Error_Vector_1*2 + Error_Vector_2 )*KD/100;
+ //Read_word_Dynamixel_RAM(KD,KD_L);
+ Motor_Action_D= (((int32_t)Error_Vector_0- Error_Vector_1*2 + Error_Vector_2 )*KD)>>10;//coste de dividir /100 es mayor que shift 7 que equivale a dividir por 128;
//KI = Read_word_Dynamixel_RAM(KI_L);
//Integral_Value = Read_word_Dynamixel_RAM(Integral_Value_L);
//ADD_A_B(Integral_Value,Integral_Value,Error_Vector_0);
- //Motor_Accion_I=(int32_t)Integral_Value*KI/100;
- // saturate Integral accion
+ //Motor_Accion_I=(int32_t)Integral_Value*KI/100;*(KI>>7);//coste de dividir /100 es mayor que shift 7 que equivale a dividir por 128;
+ // saturate Integral action
//Max_Integral_Value = Read_word_Dynamixel_RAM(Max_Integral_Value_L);
//if (Motor_Accion_I>Max_Integral_Value){Motor_Accion_I=Max_Integral_Value;}
//else if (Motor_Accion_I<(-Max_Integral_Value)){Motor_Accion_I=-Max_Integral_Value;}
- //ADD_A_B(Motor_Accion,Motor_Accion_I,Motor_Accion_D);
- //ADD_A_B(Motor_Accion,Motor_Accion,Motor_Accion_P);
- ADD_A_B(Motor_Accion,Motor_Accion_D,Motor_Accion_P);
+ //ADD_A_B(Motor_Action,Motor_Action_I,Motor_Action_D);
+ //ADD_A_B(Motor_Action,Motor_Action,Motor_Action_P);
+ //ADD_A_B(Motor_Action,Motor_Action_D,Motor_Action_P);
+ Motor_Action=ADD_A_B(Motor_Action_D,Motor_Action_P);
+
+ // range where small errors generate no action
+ Error_Margin = Read_word_Dynamixel_RAM(Error_Margin_L);
+ //Read_word_Dynamixel_RAM(Error_Margin,Error_Margin_L);
+ if (Error_Vector_0 < Error_Margin) {
+ if (Error_Vector_0 > (-1 * Error_Margin)) {
+ Motor_Action=0;
+ }
+ }
+
//Write_word_Dynamixel_RAM(Integral_Value_L,Integral_Value);
- Write_word_Dynamixel_RAM(Motor_Accion_L,Motor_Accion);
-}
+ Write_word_Dynamixel_RAM(Motor_Accion_L,Motor_Action);
-void HW_Security(void)
-{
- ; // TODO revisar corriente y temperatura
-}
+ //*******************************************
+ //HW_Security TODO revisar corriente y temperatura
+ // limit control action ... max value and alarms
-void Write_Actuator(void)
-{
- uint16_t PWM_Value;
- int16_t MotorDir,Motor_Accion,Dead_Zone,Max_PWM_Value,Temp;
-
- Motor_Accion = Read_word_Dynamixel_RAM(Motor_Accion_L);
- // find direction of accion
- if (Motor_Accion<CTRL_ZERO){MotorDir=CTRL_Dir_CW;}
+ //******************************************
+ // Control action
+ // place control action on output
+
+ // find new motor action direction
+ if (Motor_Action<CTRL_ZERO){MotorDir=CTRL_Dir_CW;}
else{MotorDir=CTRL_Dir_CCW;}
-
+
// add motor deadzone to Motor accion if not zero
// and if needed Saturate accion to max PWM_Value
- Dead_Zone=Read_word_Dynamixel_RAM(Dead_Zone_L);
- Max_PWM_Value= Read_word_Dynamixel_RAM(Max_PWM_Value_L);
- if (Motor_Accion==CTRL_ZERO)
- {PWM_Value=CTRL_ZERO;}
- else{
- MULT_A_B(Temp,Motor_Accion,MotorDir);
- ADD_A_B(Temp,Temp,Dead_Zone);
- if ( Temp > Max_PWM_Value)
- { PWM_Value = Max_PWM_Value;} // Saturate accion to max PWM_Value
- else
- { PWM_Value = (uint16_t)Temp;}// add motor deadzone to Motor accion
- }
+ Dead_Zone=Read_word_Dynamixel_RAM(Dead_Zone_L);
+ //Read_word_Dynamixel_RAM(Dead_Zone,Dead_Zone_L);
+ Max_PWM_Value= Read_word_Dynamixel_RAM(Max_PWM_Value_L);
+ //Read_word_Dynamixel_RAM(Max_PWM_Value,Max_PWM_Value_L);
+ if (Motor_Action == CTRL_ZERO) {
+ PWM_Value = CTRL_ZERO;
+ } else {
+ //MULT_A_B(Temp,Motor_Action,MotorDir);
+ Temp = MULT_A_B(Motor_Action, MotorDir);
+ //ADD_A_B(Temp,Temp,Dead_Zone);
+ Temp = ADD_A_B(Temp, Dead_Zone);
+ if (Temp > Max_PWM_Value) {
+ PWM_Value = Max_PWM_Value;
+ } // Saturate accion to max PWM_Value
+ else {
+ PWM_Value = (uint16_t) Temp;
+ } // add motor deadzone to Motor accion
+ }
// place PWM value on respective pin
@@ -226,4 +267,4 @@ void Write_Actuator(void)
OCR1A = CTRL_ZERO; //PB1 => set PWM for X% duty cycle
OCR1B = CTRL_ZERO; //PB2 => set PWM for Y% duty cycle
}
-}
+}
diff --git a/servo_firmware/ax12/src/MEM_Dynamixel.c b/servo_firmware/ax12/src/MEM_Dynamixel.c
index 4a2bf2fee291e38fa265f55cbbb4e926f72c6ea0..239d0a5d1a54e9db1ebffca371b6eda7ab57c172 100755
--- a/servo_firmware/ax12/src/MEM_Dynamixel.c
+++ b/servo_firmware/ax12/src/MEM_Dynamixel.c
@@ -1,6 +1,6 @@
#include <avr/interrupt.h>
#include <avr/io.h>
-#include <util/delay.h>
+//#include <util/delay.h>
#include "TXRX_Dynamixel.h"
#include "MEM_Dynamixel.h"
#include <avr/eeprom.h>
@@ -66,38 +66,38 @@ uint8_t sramVAR[70]={ // Register Id RAM - declared in dynamixel
0x00, // Punch_H 0X31 //49 - 0X00 R/W - Highest byte of Punch
// Register Id RAM - not declared in dynamixel
0x00, // Encoder_Port; 0X32 //50 - 0X00 R/W - pin to read sensor
- low(1024), // Sensor_Resol_L; 0X33 //51 - 0X00 R/W - 10 bit sensor resolution LOW
- high(1024), // Sensor_Resol_H; 0X34 //52 - 0X00 R/W - 10 bit sensor resolution HIGH
- low(700), // Max_PWM_Value_L; 0X35 //53 - 0X00 R/W - 10 bit max PWM value output LOW
- high(700), // Max_PWM_Value_H; 0X36 //54 - 0X00 R/W - 0 bit max PWM value output HIGH
- low(332), // Max_Sensor_Range_L; 0X37 //55 - 0X00 R/W - working range of encoder 300 case ax12 motor LOW
- high(332), // Max_Sensor_Range_H; 0X38 //56 - 0X00 R/W - working range of encoder 300 case ax12 motor HIGH
- low(5), // Error_Margin_L; 0X39 //57 - 0X00 R/W - Range of error to discard LOW
- high(5), // Error_Margin_H; 0X3A //58 - 0X00 R/W - Range of error to discard HIGH
- low(10), // Dead_Zone_L; 0X3B //59 - 0X00 R/W - Min PWM to put motor in motion LOW
- high(10), // Dead_Zone_H; 0X3C //60 - 0X00 R/W - Min PWM to put motor in motion HIGH
- low(50), // KP_L; 0X3D //61 - 0X00 R/W - proportional constant LOW
- high(50), // KP_H; 0X3E //62 - 0X00 R/W - proportional constant HIGH
- low(108), // KD_L; 0X3F //63 - 0X00 R/W - Derivative constant LOW
- high(108), // KD_H; 0X40 //64 - 0X00 R/W - Derivative constant HIGH
- low(0), // KI_L; 0X41 //65 - 0X00 R/W - Integral constant LOW
- high(0), // KI_H; 0X42 //66 - 0X00 R/W - Integral constant HIGH
+ 0x00, //low(1024), // Sensor_Resol_L; 0X33 //51 - 0X00 R/W - 10 bit sensor resolution LOW
+ 0x04, //high(1024), // Sensor_Resol_H; 0X34 //52 - 0X00 R/W - 10 bit sensor resolution HIGH
+ 0xBC, //low(700), // Max_PWM_Value_L; 0X35 //53 - 0X00 R/W - 10 bit max PWM value output LOW
+ 0x02, //high(700), // Max_PWM_Value_H; 0X36 //54 - 0X00 R/W - 0 bit max PWM value output HIGH
+ 0x4C, //low(332), // Max_Sensor_Range_L; 0X37 //55 - 0X00 R/W - working range of encoder 300 case ax12 motor LOW
+ 0x01, //high(332), // Max_Sensor_Range_H; 0X38 //56 - 0X00 R/W - working range of encoder 300 case ax12 motor HIGH
+ 0x05, //low(5), // Error_Margin_L; 0X39 //57 - 0X00 R/W - Range of error to discard LOW
+ 0x00, //high(5), // Error_Margin_H; 0X3A //58 - 0X00 R/W - Range of error to discard HIGH
+ 0x0A, //low(10), // Dead_Zone_L; 0X3B //59 - 0X00 R/W - Min PWM to put motor in motion LOW
+ 0x00, //high(10), // Dead_Zone_H; 0X3C //60 - 0X00 R/W - Min PWM to put motor in motion HIGH
+ 0x00, //low(1280), // KP_L; 0X3D //61 - 0X00 R/W - proportional constant LOW
+ 0x05, //high(1280), // KP_H; 0X3E //62 - 0X00 R/W - proportional constant HIGH
+ 0x52, //low(1106), // KD_L; 0X3F //63 - 0X00 R/W - Derivative constant LOW
+ 0x04, //high(1106), // KD_H; 0X40 //64 - 0X00 R/W - Derivative constant HIGH
+ 0x00, //low(0), // KI_L; 0X41 //65 - 0X00 R/W - Integral constant LOW
+ 0x00, //high(0), // KI_H; 0X42 //66 - 0X00 R/W - Integral constant HIGH
0x00, // Integral_Value_L; 0X43 //67 - 0X00 R/W - integral value LOW
0x00, // Integral_Value_H; 0X44 //68 - 0X00 R/W - integral value HIGH
- low(500), // Max_Integral_Value_L; 0X45 //69 - 0X00 R/W - Saturation Integral value LOW
- high(500), // Max_Integral_Value_H; 0X46 //70 - 0X00 R/W - Saturation Integral value HIGH
+ 0xF4, //low(500), // Max_Integral_Value_L; 0X45 //69 - 0X00 R/W - Saturation Integral value LOW
+ 0x01, //high(500), // Max_Integral_Value_H; 0X46 //70 - 0X00 R/W - Saturation Integral value HIGH
0x00, // Motor_Turns_L; 0X47 //71 - 0X00 R/W - # of absolute turns LOW
0x00, // Motor_Turns_H; 0X48 //72 - 0X00 R/W - # of absolute turns HIGH
- 0xFF, // Error_Vector_0_L; 0X49 //73 - 0X00 R/W - record of error for 4 time _L
- 0xEF, // Error_Vector_0_H; 0X4A //74 - 0X00 R/W - record of error for 4 time _H
- 0x00, // Error_Vector_1_L; 0X4B //75 - 0X00 R/W - record of error for 4 time _L
- 0x00, // Error_Vector_1_H; 0X4C //76 - 0X00 R/W - record of error for 4 time _H
- 0xFF, // Error_Vector_2_L; 0X4D //77 - 0X00 R/W - record of error for 4 time _L
- 0xEF, // Error_Vector_2_H; 0X4E //78 - 0X00 R/W - record of error for 4 time _H
- 0x00, // Error_Vector_3_L; 0X4F //79 - 0X00 R/W - record of error for 4 time _L
+ 0xFF, // Error_Vector_0_L; 0X49 //73 - 0X00 R/W - record of error for 0 time _L
+ 0xEF, // Error_Vector_0_H; 0X4A //74 - 0X00 R/W - record of error for 0 time _H
+ 0x00, // Error_Vector_1_L; 0X4B //75 - 0X00 R/W - record of error for 1 time _L
+ 0x00, // Error_Vector_1_H; 0X4C //76 - 0X00 R/W - record of error for 1 time _H
+ 0xFF, // Error_Vector_2_L; 0X4D //77 - 0X00 R/W - record of error for 2 time _L
+ 0xEF, // Error_Vector_2_H; 0X4E //78 - 0X00 R/W - record of error for 2 time _H
+ 0x00, // Error_Vector_3_L; 0X4F //79 - 0X00 R/W - record of error for 3 time _L
0x00, // Error_Vector_3_H; 0X50 //80 - 0X00 R/W - record of error for 4 time _H
- low(1000), // Time_Period_L; 0X51 //81 - 0X00 R/W - delta time in between ctrl cycles LOW
- high(1000), // Time_Period_H; 0X52 //82 - 0X00 R/W - delta time in between ctrl cycles HIGH
+ 0xE8, //low(1000), // Time_Period_L; 0X51 //81 - 0X00 R/W - delta time in between ctrl cycles LOW
+ 0x03, //high(1000), // Time_Period_H; 0X52 //82 - 0X00 R/W - delta time in between ctrl cycles HIGH
0x00, // Sensor_Value_L; 0X53 //83 - 0X00 R/W - fraction of turn value return by encoder LOW
0x00, // Sensor_Value_H; 0X54 //84 - 0X00 R/W - fraction of turn value return by encoder HIGH
0x00, // Motor_Accion_L; 0X55 //85 - 0X00 R/W - PWM on Motor LOW
@@ -105,17 +105,17 @@ uint8_t sramVAR[70]={ // Register Id RAM - declared in dynamixel
0x03 // Transition_Stage; 0X57 //87 - 0X00 R/W - Position inside transition stage between turns
};
+// EEPROM READ AND WRITE
-
-// EEPROM READ AND WRITE
-uint8_t Read_byte_Dynamixel_EEPROM(uint8_t IDregister)
+ // replaced by #define 20 bytes saved
+/*uint8_t Read_byte_Dynamixel_EEPROM(uint8_t IDregister)
{
//uint8_t temp;
//temp= memoria[IDregister];
//return temp;
return eepromVAR[IDregister];
-}
+}*/
void Write_byte_Dynamixel_EEPROM(uint8_t IDregister,uint8_t Value )
{
@@ -162,10 +162,11 @@ void Write_byte_Dynamixel_RAM(uint8_t IDregister,uint8_t Value )
sramVAR[IDregister-RAM_ID_correction]=Value;
}
+ // not replaced by #define 100 bytes unsaved
int16_t Read_word_Dynamixel_RAM(uint8_t IDregister)
{
int16_t temp;
- temp= (int16_t)(TwoBytesToInt(sramVAR[IDregister-RAM_ID_correction+1],sramVAR[IDregister-RAM_ID_correction]));
+ temp= TwoBytesToInt(sramVAR[IDregister-RAM_ID_correction+1],sramVAR[IDregister-RAM_ID_correction]);
return temp;
}
diff --git a/servo_firmware/ax12/src/TXRX_Dynamixel.c b/servo_firmware/ax12/src/TXRX_Dynamixel.c
index a43eeef8da1fa4f5c1185d33cd2abdc0c25a276b..2a9fdeb7b42e60466289c8ab564f7b4b9ab376ad 100755
--- a/servo_firmware/ax12/src/TXRX_Dynamixel.c
+++ b/servo_firmware/ax12/src/TXRX_Dynamixel.c
@@ -1,6 +1,6 @@
#include <avr/interrupt.h>
#include <avr/io.h>
-#include <util/delay.h>
+//#include <util/delay.h>
#include "TXRX_Dynamixel.h"
#include "MEM_Dynamixel.h"
@@ -288,9 +288,10 @@ void TxRx_Write_word_Dynamixel_RAM(uint8_t IDregister, uint8_t *dataA, uint8_t l
// ************* R E A D and send info to pc ***************
// case EEMPROM and BYTE
void TxRx_Read_byte_Dynamixel_EEPROM(uint8_t IDregister, uint8_t *answerA){
- answerA[0]=Read_byte_Dynamixel_EEPROM(IDregister);
- //answerA[0]=IDregister;
- TxRS485Packet(rs485_address,NO_ERROR,1,answerA);
+ //answerA[0]=Read_byte_Dynamixel_EEPROM(IDregister);
+ Read_byte_Dynamixel_EEPROM(answerA[0],IDregister);
+ //answerA[0]=IDregister;
+ TxRS485Packet(rs485_address,NO_ERROR,1,answerA);
}
// case RAM and BYTE
void TxRx_Read_byte_Dynamixel_RAM(uint8_t IDregister, uint8_t *answerA){
@@ -300,11 +301,13 @@ void TxRx_Read_byte_Dynamixel_RAM(uint8_t IDregister, uint8_t *answerA){
}
// case EEMPROM and WORD
void TxRx_Read_word_Dynamixel_EEPROM(uint8_t IDregister, uint8_t *answerA){
- answerA[0]=Read_byte_Dynamixel_EEPROM(IDregister);
- answerA[1]=Read_byte_Dynamixel_EEPROM(IDregister+1);
- //answerA[0]=IDregister;
- //answerA[1]=0;
- TxRS485Packet(rs485_address,NO_ERROR,2,answerA);
+ //answerA[0]=Read_byte_Dynamixel_EEPROM(IDregister);
+ //answerA[1]=Read_byte_Dynamixel_EEPROM(IDregister+1);
+ Read_byte_Dynamixel_EEPROM(answerA[0],IDregister);
+ Read_byte_Dynamixel_EEPROM(answerA[1],IDregister+1);
+ //answerA[0]=IDregister;
+ //answerA[1]=0;
+ TxRS485Packet(rs485_address,NO_ERROR,2,answerA);
}
// case RAM and WORD
void TxRx_Read_word_Dynamixel_RAM(uint8_t IDregister, uint8_t *answerA){
diff --git a/servo_firmware/ax12/src/main.c b/servo_firmware/ax12/src/main.c
index ac25c6a344692fda48e18d548a852f5538e39bdf..c8fea0e0f7767f7a96c5337348db97705ff49857 100755
--- a/servo_firmware/ax12/src/main.c
+++ b/servo_firmware/ax12/src/main.c
@@ -1,5 +1,10 @@
+// BIG NOTE
+// OVER WRITE HAPPENS IF PROGRAM BIGGER THAN 6100
+// becasused of the reserved space of 2K size of Bootloader
+// be aware -- NO MESSAGGE WILL OCCUR BUT LOSS OF INFO WILL OCCOUR
+
#include <avr/io.h>
-#include <util/delay.h>
+//#include <util/delay.h>
#include <avr/interrupt.h>
#include "TXRX_Dynamixel.h"
#include "CTRL_Dynamixel.h"
@@ -20,7 +25,7 @@ unsigned char do_control;
ISR( TIMER0_OVF_vect) {
int16_t TorqueEnable;
- TCNT0 = 0x00;
+ TCNT0 = 0x00;
sei();
TorqueEnable = Read_byte_Dynamixel_RAM(Torque_Enable);
@@ -30,7 +35,8 @@ ISR( TIMER0_OVF_vect) {
//TIMSK &= ~( 1 << TOIE0); // disable timer0
count++;
if (count == 2) {
- do_control=1;
+ do_control = 1;
+ //cli(); // disable all interrupts just to make sure
// 1.3s passed
// LedTONGGLE();
//Control_Cycle();
@@ -58,44 +64,47 @@ int16_t main(void) {
// ini eeprom if first time after run reflash system
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);
}
+ // to force write to eeprom un-comment
+ //Restore_Eeprom_Factory_Values(); //once this procedure is held, no more initialization is performed
+
//------EEPROM initial values-------------
Restore_EepromVAR();
- //blinkLedN(5);
+
// get value of Motor ID
rs485_address = eepromVAR[ID];
// configure AVR chip i/o
Config_Hardware();
- // asign actual position to goal position and asume zero motor turns
+
+ // Assign actual position to goal position and assume zero motor turns
Ini_Position();
// initialize the RS485 interface
init_RS485();
- // end of inicialization
+ // end of initialization
LedOFF;
- do_control=0;
+ do_control = 0;
while (1) {
- //LedOFF;
+
status = RxRS485Packet(&id, &instruction, &length, data);
+
if (status == CHECK_ERROR) {
TxRS485Packet(rs485_address, CHECKSUM_ERROR, 0, NULL);
} else if (status == CORRECT) {
- //LedON;
- if (id == rs485_address) {
+ if (id == rs485_address) { //1) {
switch (instruction) {
- //***********************************************************
- // P I N G
+//***********************************************************
+// P I N G
case INST_PING:
TxRS485Packet(rs485_address, NO_ERROR, 0, NULL);
break;
- //*********************************************************
- // R E A D and send info to pc
+//*********************************************************
+// R E A D and send info to pc
case INST_READ:
if (data[0] <= 18) { // if 0 <= IDinstruction <= 18 caso eeprom
if (data[0] >= 0) {
@@ -154,11 +163,11 @@ int16_t main(void) {
} // else id error
break;
- //**********************************************************
- // W R I T E info send by pc
+//**********************************************************
+// W R I T E info send by pc
case INST_WRITE:
- en_vector = FALSE; // 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,43,44,45,46,67,68,71,72,73,74,75,76,77,78,79,80,83,84,85,86};
+ en_vector = FALSE;
+
for (i = 0; i < 30; i++) {
if (data[0] == read_only_vector[i]) {
en_vector = TRUE;
@@ -259,14 +268,11 @@ int16_t main(void) {
//***************************************************
}
}
+ } else if (do_control) {
+ Control_Cycle();
+ do_control = 0;
+ //sei(); // enable all interrupts
}
- else if(do_control)
- {
- Control_Cycle();
- HW_Security();
- Write_Actuator();
- do_control=0;
- }
}
}