Пример #1
0
int shutdown_stepper(void)
{
    FILE *rtf;
    char *home;
    char str[32];
    
    sprintf(str, "/B 7%c", CR);                     /* deselect previous motor */
    write(motorPort, str, strlen(str)); delay(10);
    sprintf(str, "B 1%c", CR);                      /* set B 1 high */
    write(motorPort, str, strlen(str));     delay(10);
    
    /* clear all ports */
    sprintf(str, "/B 2%c", CR);
    write(motorPort, str, strlen(str));     delay(10);
    sprintf(str, "/B 3%c", CR);
    write(motorPort, str, strlen(str));     delay(10);
    sprintf(str, "/B 4%c", CR);
    write(motorPort, str, strlen(str));     delay(10);
    
    sprintf(str, "B 2%c", CR);
    write(motorPort, str, strlen(str));     delay(10);
    sprintf(str, "/B 2%c", CR);
    write(motorPort, str, strlen(str));     delay(10);
    sprintf(str, "B 3%c", CR);
    write(motorPort, str, strlen(str));     delay(10);
    
    sprintf(str, "B 2%c", CR);
    write(motorPort, str, strlen(str));     delay(10);
    sprintf(str, "/B 2%c", CR);
    write(motorPort, str, strlen(str));     delay(10);
    sprintf(str, "/B 3%c", CR);
    write(motorPort, str, strlen(str));     delay(10);
    
    sprintf(str, "B 4%c", CR);
    write(motorPort, str, strlen(str));     delay(10);
    sprintf(str, "B 2%c", CR);
    write(motorPort, str, strlen(str));     delay(10);
    sprintf(str, "/B 2%c", CR);
    write(motorPort, str, strlen(str));     delay(10);
    
    sprintf(str, "B 3%c", CR);
    write(motorPort, str, strlen(str));     delay(10);
    sprintf(str, "B 2%c", CR);
    write(motorPort, str, strlen(str));     delay(10);
    
    /* all ports cleared */
    sprintf(str, "B 7%c", CR);                      /* close latch */
    write(motorPort, str, strlen(str)); delay(10);
    
    sprintf(str, "B 0%c", CR);                      /* free all motors */
    write(motorPort, str, strlen(str)); delay(10);
    
	restore_setup(motorPort);
	close(motorPort);
    return(0);
}
Пример #2
0
/*********************************************************************
  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;
}