//Function to control DOF1
void control_dof1(unsigned int setpoint_dof1)
{
position_dof1 = ADC0Read(1);
if(setpoint_dof1 > position_dof1)
{
IOCLR0 = (1<<19);
IOSET0 = (1<<18);
//setDutyCycle(2,(setpoint_dof1 - position_dof1)*100/1024);
}
if(position_dof1 > setpoint_dof1)
{
IOCLR0 = (1<<18);
IOSET0 = (1<<19);
//setDutyCycle(2,(position_dof1 - setpoint_dof1)*100/1024);
}
setDutyCycle(2,85);
//iprintf("Position: %d Setpoint: %d\r\n",position_dof1, setpoint_dof1);
}
/**********************************************************
MAIN
**********************************************************/
int main(void)
{
static int vbat1, vbat2;
int vbat;
static int bat_cnt = 0, bat_warning = 1000;
static char bat_warning_enabled = 1;
#ifdef GPS_BEEP
static unsigned int gps_beep_cnt;
#endif
IDISABLE;
init();
LL_write_init();
beeper(OFF);
HL_Status.up_time = 0;
printf("\n\nProgramm is running ... \n");
printf("Processor Clock Frequency: %d Hz\n", processorClockFrequency());
printf("Peripheral Clock Frequency: %d Hz\n", peripheralClockFrequency());
IENABLE;
packetsTemp = packets;
LED(1, ON);
GPS_init_status = GPS_STARTUP;
// GPS_init_status = GPS_NEEDS_CONFIGURATION;
sdkInit();
while (1)
{
if (mainloop_trigger > 0)
{
if (GPS_timeout < ControllerCyclesPerSecond)
GPS_timeout++;
else if (GPS_timeout == ControllerCyclesPerSecond)
{
GPS_timeout = ControllerCyclesPerSecond + 1;
GPS_Data.status = 0;
GPS_Data.numSV = 0;
// if (GPS_init_status == GPS_STARTUP) //no GPS packet after startup
// {
GPS_init_status = GPS_NEEDS_CONFIGURATION;
// }
}
mainloop_cnt++;
if (++bat_cnt == 100)
bat_cnt = 0;
//battery monitoring
vbat1 = (vbat1 * 29 + (ADC0Read(VOLTAGE_1) * 9872 / 579)) / 30; //voltage in mV //*9872/579
HL_Status.battery_voltage_1 = vbat1;
HL_Status.battery_voltage_2 = vbat2;
vbat = vbat1;
if (vbat < BATTERY_WARNING_VOLTAGE) //decide if it's really an empty battery
{
if (bat_warning < ControllerCyclesPerSecond * 2)
bat_warning++;
else
bat_warning_enabled = 1;
}
else
{
if (bat_warning > 10)
bat_warning -= 5;
else
{
bat_warning_enabled = 0;
beeper(OFF);//IOCLR1 = (1<<17); //Beeper off
}
}
if (bat_warning_enabled)
{
if (bat_cnt > ((vbat - 9000) / BAT_DIV))
beeper(ON);//IOSET1 = (1<<17); //Beeper on
else
beeper(OFF);//IOCLR1 = (1<<17); //Beeper off
}
#ifdef GPS_BEEP
//GPS_Beep
if((GPS_Data.status&0xFF)!=3) //no lock
{
gps_beep_cnt++;
if(gps_beep_cnt>=1500) gps_beep_cnt=0;
if(gps_beep_cnt<20) beeper(ON); //IOSET1 = (1<<17); //Beeper on
else if(gps_beep_cnt==40) beeper(OFF);// IOCLR1 = (1<<17); //Beeper off
}
#endif
if (mainloop_trigger)
mainloop_trigger--;
mainloop();
}
}
return 0;
}
int main (void)
{
cpu_init();
//ADCInit( ADC_CLK );
unsigned int resultado1 = 0;
unsigned int resultado2 = 0;
unsigned int resultado3 = 0;
unsigned int resultado4 = 0;
unsigned int dif = 0;
Config_MCC_PINS();
pwm_init();
int j=0;
UART_Init(0,9600);
ADCInit( ADC_CLK );
IOSET0 = ((1<<3)|(1<<5)|(1<<10)|(1<<12)|(1<<17)|(1<<19));
//IOCLR0 = (1<<19);
while (1)
{
/*IOSET0 = (1<<3);
IOSET0 = (1<<5);
IOSET0 = (1<<10);
IOSET0 = (1<<12);
IOSET0 = (1<<17);
IOSET0 = (1<<19);
for (j = 0; j < rel; j++ ) asm volatile ("NOP");
IOCLR0 = (1<<3);
IOCLR0 = (1<<5);
IOCLR0 = (1<<10);
IOCLR0 = (1<<12);
IOCLR0 = (1<<17);
IOCLR0 = (1<<19);
for (j = 0; j < rel; j++ ) asm volatile ("NOP");*/
resultado1 = ADC0Read(1);
resultado2 = ADC0Read(2);
resultado3 = ADC0Read(3);
resultado4 = ADC0Read(4);
iprintf("ADC: %d\r\n", resultado1);
if(resultado1 > resultado4)
{
IOCLR0 = (1<<19);
IOSET0 = (1<<17);
dif = resultado1 - resultado4;
}
if(resultado1 < resultado4)
{
IOCLR0 = (1<<17);
IOSET0 = (1<<19);
dif = resultado4 - resultado1;
}
setpwm5cycle(dif);
//setpwm5cycle(resultado4); //PWM5 referente ao Fullbridge do DOF1
//setpwm2cycle(resultado4); //PWM2 referente ao Fullbridge do DOF2
//setpwm4cycle(resultado4); //PWM4 referente ao Fullbridge do DOF3
}
return(0);
}
