//=============================================================================
// Routine to save setup structure into eeprom
//
//=============================================================================
int save_setup( void )
{
int size = sizeof(pid);
int *sptr = (int *)&pid;
int res;
int offset = 0;
// compute correct checksum for upper part of array
// and place it in the checsum variable
pid.cksum = -calc_cksum((sizeof(pid)-sizeof(int))/sizeof(int),
(int*)&pid);
// this routine attempts to write the entire ram setup structure
// into the eeprom on board.
// write 16 words of structure at a time
while (size > 0)
{
// Erase 16 words (1 row in dsPIC30F DataEEPROM) in Data EEPROM
// from calEE structure
res = EraseEE(__builtin_tblpage(&pidEE),
__builtin_tbloffset(&pidEE)+offset, ROW);
if (res)
printf("clr of eeprom failed at %d\r\n",offset);
res = WriteEE(sptr, __builtin_tblpage(&pidEE),
__builtin_tbloffset(&pidEE)+offset, ROW);
if (res)
printf("write to eeprom failed at offset %d\r\n",offset);
offset += ROW*2; // bump offset to destination 32 bytes up
sptr += ROW; // bump source ptr up 16 words
size -= ROW*2; // 16 words or 32 bytes/write
}
return res;
}
void decode_packet() {
unsigned short cs, ccs, newbits;
if (buf[1] == 1 && buf[2] == 'r') {
// reset from board
first = true;
PORTD |= LED_ALL;
return;
}
if (buf[1] != 16) {
goto bad_packet;
}
memcpy(&cs, &buf[16], 2);
ccs = calc_cksum((unsigned short *)buf, 8);
if (cs != ccs) {
goto bad_packet;
}
memcpy(targets, &buf[2], 12);
memcpy(&newbits, &buf[14], 2);
if (newbits != iobits) {
iobits = newbits;
PORTD |= (iobits & IOBIT_MASK) | LED_RED;
PORTD &= (iobits | ~IOBIT_MASK);
}
if (first) {
first = false;
memcpy(counts, targets, 12);
}
for (unsigned char i = 0; i != 6; ++i) {
short diff = (short)(targets[i] - counts[i]);
if (diff < -10000) {
diff = 10000;
}
else if (diff > 10000) {
diff = 10000;
}
else if (diff < 0) {
diff = -diff;
}
if (diff < 2) {
diff = 2;
}
steprates[i] = 25000 / diff;
}
// no longer blue, no longer lost packet
PORTD &= ~LED_BLUE;
blue_timeout = 0;
uart_send_all(3, ack_packet);
return;
bad_packet:
// bad packets make me feel blue
PORTD |= LED_BLUE;
wdt_reset();
unsigned short max = 0;
uart_send_all(3, nak_packet);
while (!uart_available()) {
udelay(10);
if (max++ == 20) {
// no byte is there
return;
}
}
// deliberately de-sync, because I might be treating
// data as sync bytes
uart_getch();
}
/*********************************************************************
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;
}
