Skip to content
Snippets Groups Projects
Commit 2a9b2054 authored by Patrick John Grosch Obregon's avatar Patrick John Grosch Obregon
Browse files

pgrosch - Bug suma de int32 overflow en ax12- ax12_2pos modificado para que... 

pgrosch - Bug suma de int32 overflow en ax12- ax12_2pos modificado para que funcione solo con dos posiciones proyecto fede superficies flexibles - rubik plano
parent 04e21d62
No related branches found
No related tags found
No related merge requests found
Hide whitespace changes
Inline Side-by-side
servo_firmware/ax12/src/CTRL_Dynamixel.c
+ 15
− 1
View file @ 2a9b2054
......@@ -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);
......
servo_firmware/ax12_2pos/include/MEM_Dynamixel.h
+ 6
− 0
View file @ 2a9b2054
......@@ -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))
......
servo_firmware/ax12_2pos/src/CTRL_Dynamixel.c
+ 139
− 71
View file @ 2a9b2054
......@@ -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);
......
servo_firmware/ax12_2pos/src/MEM_Dynamixel.c
+ 6
− 1
View file @ 2a9b2054
......@@ -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
......
servo_firmware/ax12_2pos/src/main.c
+ 1
− 0
View file @ 2a9b2054
......@@ -86,6 +86,7 @@ int16_t main(void) {
// end of initialization
LedOFF;
//LedON;
do_control = 0;
......
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment