//DETECTOR CRUCES POR CERO
void Zero_Detect(void)
{
//Lectura de feedback
//-----------------------------------------------------------------------------------------------
CruceZero[0] = Chip_GPIO_ReadPortBit(LPC_GPIO, PORT_Z[0], PIN_Z[0]);
CruceZero[1] = Chip_GPIO_ReadPortBit(LPC_GPIO, PORT_Z[1], PIN_Z[1]);
CruceZero[2] = Chip_GPIO_ReadPortBit(LPC_GPIO, PORT_Z[2], PIN_Z[2]);
//Detecto flanco para realizar conmutación
//-----------------------------------------------------------------------------------------------
if(CruceZero0[0] != CruceZero[0]){
NextPWM();}
else{
if(CruceZero0[1] != CruceZero[1]){
NextPWM();}
else{
if(CruceZero0[2] != CruceZero[2]){
NextPWM();}
}}
}
//ARRANQUE
void Start_Up_Brushless(void)
{
long dr, dPwr;
uint32_t t = 1;
//Drive at const rate for a few cycles to make sure rotor is synched.
//-----------------------------------------------------------------------------------------------
Count = 0;
NextPWM(); //Siguiente conmutación
//while (Match_Cnt < 50000); //Delay de 1Seg para asegurar sincronizmo y arranque
while (Count < 3) //Primeras 3 conmutaciones a período inicial (lentas) por sincronizmo
{
while (Match_Cnt < StepPeriod); //Delay hasta sig conmutación
NextPWM(); //Siguiente conmutación
}
//Set variables para ecuaciones de arranque
//-----------------------------------------------------------------------------------------------
dPwr = (start.powerRange[1] - start.powerRange[0])/start.duration; //Diferencia de Duty
//r0 = 1.0 / start.periodRange[0];
dr = (start.periodRange[0] -start.periodRange[1])/start.duration;
Count = 0;
t = 0;
//Arranque del Motor (Clock:25MHz, Divisor pwm:1, Ciclos pwm:1000, -> [1 Match_Cnt = 40 MicroSeg]
//-----------------------------------------------------------------------------------------------
while (StepPeriod > start.periodRange[1])
{
while (Match_Cnt < StepPeriod);//Delay hasta la siguiente conmutación (bloqueante solo durante arranque)
NextPWM(); //Siguiente conmutación
DutyCycle = start.powerRange[0] + t * dPwr;//Incremento Duty de manera lineal desde powerRange0 a powerRange1
StepPeriod =start.periodRange[0] - t * dr; //Disminuye período entre conmutaciones de manera exponencial decreciente
t++; //desde periodRange0 hasta periodRange1
}
DutyCycle = 75; // (75/500)-> 15% Duty
//DutyCycle = 0.5 * (start.powerRange[0] + start.powerRange[1]); // Reduce Duty al final del arranque
}
//ARRANQUE
void Start_Up_Brushless(void)
{
uint32_t t = 1, dr, dPwr;
//Drive at const rate for a few cycles to make sure rotor is synched.
//-----------------------------------------------------------------------------------------------
Count = 0;
NextPWM(); //Siguiente conmutación
while (Count < 10) //Primeras 3 conmutaciones a período inicial (lentas) por sincronizmo
{
while (Match_Cnt < StepPeriod); //Delay hasta sig conmutación
NextPWM(); //Siguiente conmutación
}
//Set variables para ecuaciones de arranque
//-----------------------------------------------------------------------------------------------
dPwr = (start.powerRange[1] - start.powerRange[0])/start.duration; //Diferencia de Duty
dr = (start.periodRange[0] -start.periodRange[1])/start.duration;
t = 0;
//Arranque del Motor (Clock:25MHz, Divisor pwm:1, Ciclos pwm:1000, -> [1 Match_Cnt = 40 MicroSeg]
//-----------------------------------------------------------------------------------------------
while (StepPeriod > start.periodRange[1])
{
while (Match_Cnt < StepPeriod);//Delay hasta la siguiente conmutación (bloqueante solo durante arranque)
NextPWM(); //Siguiente conmutación
DutyCycle = start.powerRange[0] + t * dPwr;//Incremento Duty de manera lineal desde powerRange0 a powerRange1
StepPeriod =start.periodRange[0] - t * dr; //Disminuye período entre conmutaciones de manera exponencial decreciente
t++; //desde periodRange0 hasta periodRange1
}
DutyCycle = 150; // (150/1000)-> 15% Duty
Chip_PWM_SetMatch(LPC_PWM1, 5, DutyCycle);
//Chip_PWM_Reset(LPC_PWM1);
Chip_PWM_LatchEnable(LPC_PWM1, 5, PWM_OUT_ENABLED);
}
uint32_t Start_Up_Brushless(uint32_t num_motor)
{
static uint32_t t=1, dr=0, dPwr=0;
static uint32_t Suspender_Task=0;
//Drive at const rate for a few cycles to make sure rotor is synched.
//-----------------------------------------------------------------------------------------------
switch(estado_motorstartup[num_motor])
{
case 0:
NextPWM(num_motor); //Siguiente conmutación
Count[num_motor]=0; //Inicio el conteo para el arranque
estado_motorstartup[num_motor] = 1;
break;
case 1:
/* if(Count[num_motor]<10)
{
if(Match_Cnt[num_motor]>=StepPeriod[num_motor]) //Delay hasta la siguiente conmutación
NextPWM(num_motor); //Siguiente conmutación
}
else
{*/
dPwr = (start.powerRange[1] - start.powerRange[0])/start.duration; //Diferencia de Duty
dr = (start.periodRange[0] -start.periodRange[1])/start.duration;
t = 0;
estado_motorstartup[num_motor] = 2;
// }
break;
case 2:
if(StepPeriod[num_motor] > (uint32_t)start.periodRange[1])
{
if(Match_Cnt[num_motor] >= StepPeriod[num_motor])
{
NextPWM(num_motor);
DutyCycle[num_motor] = start.powerRange[0] + t * dPwr;//Incremento Duty de manera lineal desde powerRange0 a powerRange1
StepPeriod[num_motor] =start.periodRange[0] - t * dr; //Disminuye período entre conmutaciones de manera exponencial decreciente
t++; //desde periodRange0 hasta periodRange1
}
}
else
{
DutyCycle[num_motor] = 150; // (150/1000)-> 15% Duty
Chip_PWM_SetMatch(LPC_PWM1, PWM_number[num_motor], DutyCycle[num_motor]);
//Chip_PWM_Reset(LPC_PWM1);
Suspender_Task = 1;
estado_motorstartup[num_motor] = 0;
}
break;
default:
DutyCycle[num_motor] = 150; // (150/1000)-> 15% Duty
Chip_PWM_SetMatch(LPC_PWM1, PWM_number[num_motor], DutyCycle[num_motor]);
//Chip_PWM_Reset(LPC_PWM1);
Suspender_Task = 1;
estado_motorstartup[num_motor] = 0;
break;
}
/*
Count[num_motor] = 0;
NextPWM(num_motor); //Siguiente conmutación
while (Count[num_motor] < 10) //Primeras 3 conmutaciones a período inicial (lentas) por sincronismo
{
while (Match_Cnt[num_motor] < StepPeriod[num_motor]); //Delay hasta sig conmutación
NextPWM(num_motor); //Siguiente conmutación
}
//Set variables para ecuaciones de arranque
//-----------------------------------------------------------------------------------------------
dPwr = (start.powerRange[1] - start.powerRange[0])/start.duration; //Diferencia de Duty
dr = (start.periodRange[0] -start.periodRange[1])/start.duration;
t = 0;
//Arranque del Motor (Clock:25MHz, Divisor pwm:1, Ciclos pwm:1000, -> [1 Match_Cnt = 40 MicroSeg]
//-----------------------------------------------------------------------------------------------
while (StepPeriod[num_motor] > (uint32_t)start.periodRange[num_motor])
{
while (Match_Cnt[num_motor] < StepPeriod[num_motor]);//Delay hasta la siguiente conmutación (bloqueante solo durante arranque)
NextPWM(num_motor); //Siguiente conmutación
DutyCycle[num_motor] = start.powerRange[0] + t * dPwr;//Incremento Duty de manera lineal desde powerRange0 a powerRange1
StepPeriod[num_motor] =start.periodRange[0] - t * dr; //Disminuye período entre conmutaciones de manera exponencial decreciente
t++; //desde periodRange0 hasta periodRange1
}
DutyCycle[num_motor] = 150; // (150/1000)-> 15% Duty
Chip_PWM_SetMatch(LPC_PWM1, PWM_number[num_motor], DutyCycle[num_motor]);
Chip_PWM_Reset(LPC_PWM1);
Suspender_Task = 1;
*/
return Suspender_Task;
}
// MAIN PROGRAM
int main(void)
{
uint8_t Giro = 0, Giro0 = 0, CloseLoop=0;
uint16_t Inertia=0x01FF;
//Init All
//-----------------------------------------------------------------------------------------------
Stop_and_Default(); //Condiciones iniciales
InitGPIO(); //Llamo función para inicializar GPIO
InitPWM(); //Función inicialización modulo PWM
//Main Loop
//-----------------------------------------------------------------------------------------------
while (1)
{
//Lectura Pulsadores
//-------------------------------------------------------------------------------------------
if (Chip_GPIO_ReadPortBit(LPC_GPIO, PULS_PORT, PULS1)==0 && Chip_GPIO_ReadPortBit(LPC_GPIO, PULS_PORT, PULS2)==0)
{
//Detencion y valores de reinicio
Stop_and_Default(); //Detencion del motor
Giro = 0; //Flag que no siga girando
Giro0 = 0; //Flag para arranque
CloseLoop=0; //Flag para lazo cerrado
Inertia=0x01FF; //Contador para mantener velocidad hasta encontrar BEMF
AntiRebo = REBOTE_; //Restablezco anti rebote
}
if (Chip_GPIO_ReadPortBit(LPC_GPIO, PULS_PORT, PULS1) == 0 && AntiRebo == 0)
{
//Arranque Motor PWM + Period
if (Giro0 == 0) { //Primer pulso:
Start_Up_Brushless(); //Arranque del motor
Giro = 1; //Flag que continue girando
} else {
if (DutyCycle > 50)
DutyCycle = DutyCycle - 5; //Decrementar ciclo actividad
}
AntiRebo = REBOTE_; //Restablezco anti rebote
}
if (Chip_GPIO_ReadPortBit(LPC_GPIO, PULS_PORT, PULS2) == 0 && AntiRebo == 0)
{
if (DutyCycle < 450 && Giro0 == 1)
DutyCycle = DutyCycle + 5; //Incremento ciclo actividad
AntiRebo = REBOTE_; //Restablezco anti rebote
}
//-------------------------------------------------------------------------------------------
if (AntiRebo > 0)
AntiRebo--; //Antirebote a lo croto
//Test PWM
//-------------------------------------------------------------------------------------------
if(CloseLoop==0){ //Lazo abierto
if (Match_Cnt>=StepPeriod && Giro)
{
NextPWM(); //Conmutación
Giro0 = 1; //Flag para incrementar o decrementar duty
//Inertia--; //Contador decreciente para encontrar BEMF
//if(Inertia==0)
// CloseLoop=1; //Final del contador -> entro en lazo cerrado
}
}else{ //Lazo cerrado
Zero_Detect(); //Detección de cruces por cero (cincronismo)
}
//-------------------------------------------------------------------------------------------
//End Test
}
return 1;
}
int main(void)
{
uint32_t estado[4] ={0,0,0,0},suspender[4]={0,0,0,0}, StartMotores[4] = {0,0,0,0};
initHardware();
Chip_GPIO_WriteDirBit(LPC_GPIO, 2, 10, 1); //led isp
Chip_GPIO_SetPinOutHigh(LPC_GPIO, 2,10);
while(1)
{
if(available())
{
read( &data_led[0], 4 );
data=data_led[0];
data=(data<<8)|data_led[1];
data=(data<<8)|data_led[2];
data=(data<<8)|data_led[3];
}
if(data == 0xAABBCCDD)
{
Chip_GPIO_SetPinOutLow(LPC_GPIO, 2,10); //led isp
StartMotores[2] = 1;
StartMotores[3] = 1;
}
if(data == 0xEEFF0123)
{
Chip_GPIO_SetPinOutHigh(LPC_GPIO, 2,10); //led isp
estado[2] = 0;
estado[3] = 0;
Stop_and_Default(0); //Condiciones iniciales
Stop_and_Default(1); //Condiciones iniciales
Stop_and_Default(2); //Condiciones iniciales
Stop_and_Default(3); //Condiciones iniciales
}
if (StartMotores[2] && estado[2] == 0)
{
StartMotores[2] = 0;
estado[2] = 1;
msTick[2] = 0;
}
/* if (StartMotores[3] && estado[3] == 0)
{
StartMotores[3] = 0;
estado[3] = 1;
msTick[3] = 0;
}
*/
if(estado[2] == 1)
{
if(msTick[2])
{
msTick[2]=0;
suspender[2]=Start_Up_Brushless(2);
if(suspender[2])
{
suspender[2] = 0;
estado[2] = 2;
}
}
}
if(estado[2] == 2)
{
if(Conmutar[2])
{
Conmutar[2] = 0;
NextPWM(2);
}
}
/* if(estado[3] == 1)
{
if(msTick[3])
{
msTick[3]=0;
suspender[3]=Start_Up_Brushless(3);
if(suspender[3])
{
suspender[3] = 0;
estado[3] = 2;
}
}
}
if(estado[3] == 2)
{
if(Conmutar[3])
{
Conmutar[3] = 0;
NextPWM(3);
}
}*/
}
return 0;
}
