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