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

create copy of dir to work on single turn control

parent 6f076f60
No related branches found
No related tags found
No related merge requests found
Hide whitespace changes
Inline Side-by-side
servo_firmware/rx28/src/CTRL_sing_turn/CTRL_Dynamixel.c 0 → 100755
+ 230
− 0
View file @ 175ecb8d
#include <avr/interrupt.h>
#include <avr/io.h>
#include "MEM_Dynamixel.h"
#include "CTRL_Dynamixel.h"
#include "CFG_HW_Dynamixel.h"
#define INT16_MAX 0x7fff
#define INT16_MIN (-INT16_MAX - 1)
#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) ({\
int32_t x = (int32_t)a*b; \
(x>32767)?(c = (int16_t)32767) : ((x<-32768)?(c = (int16_t)-32768):(c = (int16_t) x)); \
})
#define num_to_FFFFbit(c,a) ({\
double x = (double)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;
// 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);
Write_word_Dynamixel_RAM(Motor_Turns_L,0);
Write_byte_Dynamixel_RAM(Torque_Enable,1);
}
int16_t Read_Sensor(uint8_t port) {
int16_t ADCval;
ADMUX = port; // use #port ADC
ADMUX |= (1 << REFS0); // use AVcc as the reference
ADMUX &= ~(1 << ADLAR); // clear for 10 bit resolution
ADCSRA |= (1 << ADPS2) | (1 << ADPS1) | (1 << ADPS0); // 128 prescale for 8Mhz
ADCSRA |= (1 << ADEN); // Enable the ADC
ADCSRA |= (1 << ADSC); // Start the ADC conversion
while (ADCSRA & (1 << ADSC)); // waits for the ADC to finish
ADCval = ADCL;
ADCval = (ADCH << 8) + ADCval; // ADCH is read so ADC can be updated again
return ADCval;
}
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 Error_Margin,KP,KD,Sensor_Resol, Max_Sensor_Range;
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;}
// 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
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);
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);
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);
// Update Error Vector 2
Error_Vector_2=Read_word_Dynamixel_RAM(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);
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);
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;
KD = Read_word_Dynamixel_RAM(KD_L);
Motor_Accion_D= ((int32_t)Error_Vector_0- Error_Vector_1*2 + Error_Vector_2 )*KD/100;
//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
//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);
//Write_word_Dynamixel_RAM(Integral_Value_L,Integral_Value);
Write_word_Dynamixel_RAM(Motor_Accion_L,Motor_Accion);
}
void HW_Security(void)
{
; // TODO revisar corriente y temperatura
}
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;}
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
}
// place PWM value on respective pin
if (PWM_Value > CTRL_ZERO) {
if (MotorDir == CTRL_Dir_CCW) {
SET_CW_PWM_MOTOR(CTRL_ZERO); //PB1 => set PWM for X% duty cycle
SET_CCW_PWM_MOTOR(PWM_Value); //PB2 => set PWM for Y% duty cycle
} else {
SET_CCW_PWM_MOTOR(CTRL_ZERO); //PB2 => set PWM for Y% duty cycle
SET_CW_PWM_MOTOR(PWM_Value); //PB1 => set PWM for X% duty cycle
}
} else {
SET_CW_PWM_MOTOR(CTRL_ZERO); //PB1 => set PWM for X% duty cycle
SET_CCW_PWM_MOTOR(CTRL_ZERO); //PB2 => set PWM for Y% duty cycle
}
}
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