diff --git a/servo_firmware/ax12/src/CTRL_Dynamixel.c b/servo_firmware/ax12/src/CTRL_Dynamixel.c
index b7ff64dbf8ef660a0d11fb73f7cdb37427cd300b..c9114f4047a46795caa85041c9e4124ffe4d54b2 100755
--- a/servo_firmware/ax12/src/CTRL_Dynamixel.c
+++ b/servo_firmware/ax12/src/CTRL_Dynamixel.c
@@ -7,6 +7,15 @@
#define INT16_MAX 0x7fff
#define INT16_MIN (-INT16_MAX - 1)
+// transform a 64 bit to a 16 bit
+
+int16_t int64_int16(int64_t a)
+{
+ int16_t c;
+ (a>32767)?(c = (int16_t)32767) : ((a<-32768)?(c = (int16_t)-32768):(c = (int16_t) a));
+
+ return c;
+}
//#define ADD_A_B(c,a,b) ({\
int32_t x = (int32_t)a+b; \
@@ -93,6 +102,8 @@ void Control_Cycle(void)
int32_t Motor_Action_P=0;
int32_t Motor_Action_D=0;
//int32_t Motor_Action_I=0;
+ //int32_t Temp32=0;
+ int64_t Temp64=0;
// update position sensor value
Sensor_Value = Read_Sensor(CTRL_Encoder_Port);
@@ -205,7 +216,10 @@ void Control_Cycle(void)
//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);
+ // Motor_Action=ADD_A_B(Motor_Action_D,Motor_Action_P); // error ... son int32 no int16
+ Temp64=(int64_t)Motor_Action_D+Motor_Action_P;
+ Motor_Action=int64_int16(Temp64);
+
// range where small errors generate no action
Error_Margin = Read_word_Dynamixel_RAM(Error_Margin_L);
diff --git a/servo_firmware/ax12_2pos/include/MEM_Dynamixel.h b/servo_firmware/ax12_2pos/include/MEM_Dynamixel.h
index c14dc7695c8308d47111c2f32bd400732c11c3fb..f45a32eece3ee3f57aaf1cbd6c96d078e715f63c 100755
--- a/servo_firmware/ax12_2pos/include/MEM_Dynamixel.h
+++ b/servo_firmware/ax12_2pos/include/MEM_Dynamixel.h
@@ -25,6 +25,7 @@
// not define in dynamixell
#define Gate_Restore_Eeprom 0X0A // 10 0x01 R/W - Gate variable to restores eeprom factory values
//19 => 23 not used
+
#define RAM_ID_correction 24 // to keep a sequence in the parameters ID between RAM and Eemprom a correction must be done
// to read from ram_vector(parameterID - correction)
@@ -101,6 +102,11 @@
// 4 Transition_Stage = 1 in stransition gap sensor value = 1023 came into gap CCW direction
// 5 Transition_Stage = 2 in stransition gap sensor value =~ 1023 came into gap CCW direction
// 6 Transition_Stage = 3 in stransition gap sensor value = 0 came into gap CCW direction
+#define POS_A_L 0X58 //88 - 0X00 R/W - Position A low of game rubik flat
+#define POS_A_H 0X59 //89 - 0X00 R/W - Position A high of game rubik flat
+#define POS_B_L 0X5A //90 - 0X00 R/W - Position B low of game rubik flat
+#define POS_B_H 0X5B //91 - 0X00 R/W - Position B high of game rubik flat
+
#define low(x) ((uint8_t)((x) & 0xFF))
#define high(x) ((uint8_t)(((x)>>8) & 0xFF))
diff --git a/servo_firmware/ax12_2pos/src/CTRL_Dynamixel.c b/servo_firmware/ax12_2pos/src/CTRL_Dynamixel.c
index b7ff64dbf8ef660a0d11fb73f7cdb37427cd300b..95cbc7ff0269fcb6d7431cb3d28b0c997c06ddd3 100755
--- a/servo_firmware/ax12_2pos/src/CTRL_Dynamixel.c
+++ b/servo_firmware/ax12_2pos/src/CTRL_Dynamixel.c
@@ -8,6 +8,16 @@
#define INT16_MAX 0x7fff
#define INT16_MIN (-INT16_MAX - 1)
+// transform a 64 bit to a 16 bit
+int16_t int64_int16(int64_t a)
+{
+ int16_t c;
+ (a>32767)?(c = (int16_t)32767) : ((a<-32768)?(c = (int16_t)-32768):(c = (int16_t) a));
+
+ return c;
+}
+
+
//#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)); \
@@ -48,6 +58,8 @@ void Ini_Position(void)
int16_t Sensor_Value;
// update position sensor value
Sensor_Value=Read_Sensor(CTRL_Encoder_Port);
+
+
Write_word_Dynamixel_RAM(Sensor_Value_L,Sensor_Value);
Write_word_Dynamixel_RAM(Present_Position_L,Sensor_Value);
Write_word_Dynamixel_RAM(Goal_Position_L,Sensor_Value);
@@ -84,7 +96,7 @@ void Control_Cycle(void)
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_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;
@@ -93,82 +105,86 @@ void Control_Cycle(void)
int32_t Motor_Action_P=0;
int32_t Motor_Action_D=0;
//int32_t Motor_Action_I=0;
+ //int32_t Temp32=0;
+ int64_t Temp64=0;
// 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
- }
- break; //...............................................
- case 1: //-2
- if (Sensor_Value==0){ Transition_Stage_Value=2; //-1
- }else if (Sensor_Value==1023){ Transition_Stage_Value=0;} //-3
- break; //...............................................
- case 2: //-1
- if (Sensor_Value!=0){
- if (MotorDir==CTRL_Dir_CW){ Transition_Stage_Value=1; //-2
- }else { Transition_Stage_Value=3;} //0
- }
- break; //...............................................
- case 3: //0
- if (Sensor_Value==0) { Transition_Stage_Value=2; //-1
- }else if (Sensor_Value==1023) { Transition_Stage_Value=4;} //1
- break; //...............................................
- case 4: //1
- if (Sensor_Value!=1023){
- if (MotorDir==CTRL_Dir_CCW){ Transition_Stage_Value=5; //2
- }else { Transition_Stage_Value=3;} //0
- }
- break; //...............................................
- case 5: //2
- if (Sensor_Value==1023){ Transition_Stage_Value=4; //1
- }else if (Sensor_Value==0){ Transition_Stage_Value=6;} //3
- break; //...............................................
- 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
- }
- break; //...............................................
- default:
- ;
- break; //...............................................
- }
- 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);
- //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
+// // 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
+// }
+// break; //...............................................
+// case 1: //-2
+// if (Sensor_Value==0){ Transition_Stage_Value=2; //-1
+// }else if (Sensor_Value==1023){ Transition_Stage_Value=0;} //-3
+// break; //...............................................
+// case 2: //-1
+// if (Sensor_Value!=0){
+// if (MotorDir==CTRL_Dir_CW){ Transition_Stage_Value=1; //-2
+// }else { Transition_Stage_Value=3;} //0
+// }
+// break; //...............................................
+// case 3: //0
+// if (Sensor_Value==0) { Transition_Stage_Value=2; //-1
+// }else if (Sensor_Value==1023) { Transition_Stage_Value=4;} //1
+// break; //...............................................
+// case 4: //1
+// if (Sensor_Value!=1023){
+// if (MotorDir==CTRL_Dir_CCW){ Transition_Stage_Value=5; //2
+// }else { Transition_Stage_Value=3;} //0
+// }
+// break; //...............................................
+// case 5: //2
+// if (Sensor_Value==1023){ Transition_Stage_Value=4; //1
+// }else if (Sensor_Value==0){ Transition_Stage_Value=6;} //3
+// break; //...............................................
+// 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
+// }
+// break; //...............................................
+// default:
+// ;
+// break; //...............................................
+// }
+// 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);
+// //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);
+// Present_Position=ADD_A_B(Present_Position,Sensor_Value);
+
+ Present_Position=Sensor_Value;
Write_word_Dynamixel_RAM(Present_Position_L,Present_Position);
// Update Error Vector 3
- //Error_Vector_3=Read_word_Dynamixel_RAM(Error_Vector_2_L);
- //Write_word_Dynamixel_RAM(Error_Vector_3_L,Error_Vector_3);
+ Error_Vector_3=Read_word_Dynamixel_RAM(Error_Vector_2_L);
+ 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);
@@ -179,6 +195,53 @@ void Control_Cycle(void)
Write_word_Dynamixel_RAM(Error_Vector_1_L,Error_Vector_1);
// Update Error Vector 0
Goal_Position=Read_word_Dynamixel_RAM(Goal_Position_L);
+
+// // modifiy Goal position
+// if ((Sensor_Value >900)&&(Goal_Position <-1100)){
+// POSB=Read_word_Dynamixel_RAM(POS_B_L);
+// Goal_Position=POSB;
+// Write_word_Dynamixel_RAM(Goal_Position_L,Goal_Position);
+// }
+// else if((Sensor_Value <100)&&(Goal_Position >1100)){
+// POSA=Read_word_Dynamixel_RAM(POS_A_L);
+// Goal_Position=POSA;
+// Write_word_Dynamixel_RAM(Goal_Position_L,Goal_Position);
+// }
+ // is goal position out side range of sensor + direction
+ if (Goal_Position <-1100) {
+ // el error disminuye .. equivale a estar en la zona de funcionamiento del pote
+ // error negativo
+ if (Error_Vector_1 > Error_Vector_3){
+ // estaba en POSA => cambiar goal a POSB
+ if (Sensor_Value >900){
+ Goal_Position=Read_word_Dynamixel_RAM(POS_B_L);
+ Write_word_Dynamixel_RAM(Goal_Position_L,Goal_Position);
+ }
+ // estaba en POSB => cambiar goal a POSA
+ else if ((Sensor_Value < 600)&&(Sensor_Value > 300)){
+ Goal_Position=Read_word_Dynamixel_RAM(POS_A_L);
+ Write_word_Dynamixel_RAM(Goal_Position_L,Goal_Position);
+ }
+ }
+ }
+ else if(Goal_Position >1100){
+ // el error disminuye .. equivale a estar en la zona de funcionamiento del pote
+ if (Error_Vector_3 > Error_Vector_1){
+ // estaba en POSB => cambiar goal a POSA
+ if (Sensor_Value <100){
+ Goal_Position=Read_word_Dynamixel_RAM(POS_A_L);
+ Write_word_Dynamixel_RAM(Goal_Position_L,Goal_Position);
+ }
+ // estaba en POSA => cambiar goal a POSB
+ else if ((Sensor_Value < 600)&&(Sensor_Value > 300)){
+ Goal_Position=Read_word_Dynamixel_RAM(POS_B_L);
+ Write_word_Dynamixel_RAM(Goal_Position_L,Goal_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);
@@ -189,10 +252,14 @@ void Control_Cycle(void)
// CTRL_Law - algorithm http://lorien.ncl.ac.uk/ming/digicont/digimath/dpid1.htm
KP = Read_word_Dynamixel_RAM(KP_L);
//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;
+ Motor_Action_P = ((int32_t)Error_Vector_0*KP)>>7;// coste de dividir /10 es mayor que shift 4 que equivale a dividir por 16;
+
+
KD = Read_word_Dynamixel_RAM(KD_L);
//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;
+ 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);
@@ -205,7 +272,8 @@ void Control_Cycle(void)
//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);
+ Temp64=(int64_t)Motor_Action_D+Motor_Action_P;
+ Motor_Action=int64_int16(Temp64);
// range where small errors generate no action
Error_Margin = Read_word_Dynamixel_RAM(Error_Margin_L);
diff --git a/servo_firmware/ax12_2pos/src/MEM_Dynamixel.c b/servo_firmware/ax12_2pos/src/MEM_Dynamixel.c
index 239d0a5d1a54e9db1ebffca371b6eda7ab57c172..cd6ab3de199af9d56d45022efed0f618065da839 100755
--- a/servo_firmware/ax12_2pos/src/MEM_Dynamixel.c
+++ b/servo_firmware/ax12_2pos/src/MEM_Dynamixel.c
@@ -102,7 +102,12 @@ uint8_t sramVAR[70]={ // Register Id RAM - declared in dynamixel
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
0x00, // Motor_Accion_H; 0X56 //86 - 0X00 R/W - PWM on Motor HIGH
- 0x03 // Transition_Stage; 0X57 //87 - 0X00 R/W - Position inside transition stage between turns
+ 0x03, // Transition_Stage; 0X57 //87 - 0X00 R/W - Position inside transition stage between turns
+ 0xEA, // POS_A_L 0X58 //88 - 0X00 R/W - Position A of game rubik flat
+ 0x00, // POS_A_H 0X59 //89 - 0X00 R/W - Position B of game rubik flat
+ 0x1A, // POS_B_L 0X58 //88 - 0X00 R/W - Position A of game rubik flat
+ 0x03 // POS_B_H 0X59 //89 - 0X00 R/W - Position B of game rubik flat
+
};
// EEPROM READ AND WRITE
diff --git a/servo_firmware/ax12_2pos/src/main.c b/servo_firmware/ax12_2pos/src/main.c
index c8fea0e0f7767f7a96c5337348db97705ff49857..7d964325a40c35ad1c97048288fe08b1a2bae90b 100755
--- a/servo_firmware/ax12_2pos/src/main.c
+++ b/servo_firmware/ax12_2pos/src/main.c
@@ -86,6 +86,7 @@ int16_t main(void) {
// end of initialization
LedOFF;
+ //LedON;
do_control = 0;