//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); }