void USART6_IRQHandler(void) {
/* Check RXNE flag value in SR register */
if(LL_USART_IsActiveFlag_RXNE(USART6) && LL_USART_IsEnabledIT_RXNE(USART6)) {
/* RXNE flag will be cleared by reading of DR register (done in call) */
/* Call function in charge of handling Character reception */
if(rx_index >= RX_BUFFER_SIZE - 1) {
printf("Error, command too Big\r\n");
rx_index = 0;
return;
}
uint8_t rx_char = LL_USART_ReceiveData8(USART6);
if(echo) { printf("%c", rx_char); }
rx_buffer[rx_index++] = rx_char;
if(rx_char == '\r' || rx_char == '\n') {
uint16_t bytes_consumed;
if(rx_index == 1) { bytes_consumed = 1; }
else {
bytes_consumed = process_serial_buffer(rx_buffer, rx_index);
}
if(bytes_consumed >= rx_index) { rx_index = 0; }
else { rx_index -= bytes_consumed; }
}
}
}
/*********************************************************************
Function: int main(void)
PreCondition: None.
Input: None.
Output: None.
Side Effects: None.
Overview: main function of the application. Peripherals are
initialized.
Note: None.
********************************************************************/
int main(void)
{
int cs;
// vars used for detection of incremental motion
unsigned short new_cmd,last_cmd, new_fb,last_fb;
setup_io(); // make all i/o pins go the right dir
STATUS_LED = 0; // led on
setup_uart(); // setup the serial interface to the PC
setup_TMR1(); // set up 1ms timer
IEC0bits.T1IE = 1; // Enable interrupts for timer 1
// needed for delays in following routines
// 1/2 seconds startup delay
timer_test = 5000;
while ( timer_test );
printf("\r\nPowerup..i/o...uart...timer...");
setup_pwm(); // start analog output
set_pwm(0.0);
printf("pwm...");
init_pid();
printf("pid...");
setup_encoder(); // 16 bit quadrature encoder module setup
printf("encoder...");
setup_capture(); // 2 pins with quadrature cmd from PC
printf("capture...");
printf("done\r\n");
// some junk for the serial channel
printf("%s%s\n\r",CPWRT,VERSION);
STATUS_LED = 1; // led off when init finished
// restore config from eeprom
// Read array named "setupEE" from DataEEPROM and place
// the result into array in RAM named, "setup"
restore_setup();
cs = calc_cksum(sizeof(pid)/sizeof(int),(int*)&pid);
if ( cs )
{
// opps, no valid setup detected
// assume we are starting from a new box
printf("No valid setup found in EEPROM\r\n");
init_pid();
}
else
{
printf("Using setup from eeprom.. ? for help\r\n");
print_tuning();
}
printf("using %fms servo loop interval\r\n",pid.ticksperservo * 0.1);
// BLOCK OF TEST ROUTINES FOR HARDWARE DEBUGGING
// test_pwm_interface(); // play with opa549 hardware
// test_pc_interface(); // echo cmded posn to serial port
// test_pid_interface(); // test pid loop operation
new_cmd = last_cmd = new_fb = last_fb = 0;
while (1)
{
if ( do_servo ) // check and see if timer 1 has asked for servo calcs to be run
{
do_servo = 0;
if (SVO_ENABLE)
{
if ( pid.enable == 0 ) // last loop, servo was off
{
set_pwm( 0.0 );
printf("servo-enabled\r\n>");
pid.enable = 1;
// make sure we dont move on enabling
cmd_posn = POSCNT; // make 16bit incr registers match
pid.command = 0L; // make 32 bit counter match
pid.feedback = 0L;
// make the 1ms loop temps match
new_cmd = last_cmd = new_fb = last_fb = 0;
pid.error_i = 0.0; // reset integrator
}
// we can time the servo cycle calcs by scoping the PID_ACTIVE pin
PID_ACTIVE = 1; // seems to take about 140us
new_cmd = cmd_posn; // grab current cmd from pc
new_fb = POSCNT; // grab current posn from encoder
pid.command += (long int)((short)(new_cmd - last_cmd));
pid.feedback += (long int)((short)(new_fb - last_fb ));
last_cmd = new_cmd;
last_fb = new_fb;
calc_pid();
// check for a drive fault ( posn error > allowed )
if (( pid.maxerror > 0.0 ) &&
( fabs(pid.error) > pid.maxerror ))
{
short temp = SVO_ENABLE;
set_pwm( 0.0 );
while (1) // trap here until svo disabled or pwr cycle
{
// reset integrator as it may have wound up
pid.error_i = 0.0;
printf("drive fault... maxerror exceeded\r\n");
STATUS_LED = 0; timer_test = 2500; while ( timer_test );
STATUS_LED = 1; timer_test = 2500; while ( timer_test );
STATUS_LED = 0; timer_test = 2500; while ( timer_test );
STATUS_LED = 1; timer_test = 2500; while ( timer_test );
if (temp != SVO_ENABLE)
break;
}
}
else
{
set_pwm(pid.output); // update motor drive
}
PID_ACTIVE = 0; // i/o pin for timing pid calcs
}
else
{
if ( pid.enable == 1 ) // last loop servo was active
{
set_pwm( 0.0 );
pid.enable = 0;
printf("servo-disabled\r\n>");
// extra delay keeps us faulted for min 1 sec to let mechanicals settle
STATUS_LED = 1; timer_test = 10000; while ( timer_test );
}
}
}
// look for serial cmds
// doing this while svo is enabled will cause bumps in servo loop
// because of serial i/o time ( unless we get smart and move svo loop
// into an isr )
if ( rxrdy )
process_serial_buffer();
if (pid.limit_state) // show we are at drive limit(error)
STATUS_LED = 0;
else
STATUS_LED = 1;
}
// to keep compiler happy....
return 0;
}
