/* This program provides a microstepping stepper motor controller down to 1/128th step achieved using PWM of the changing phase to control the current to the motor for that phase at 20KHz. The default speed of the motor is 1 rev per 240 seconds ( sidereal rate on a 360 tooth worm ) using 32 microsteps per step. The speed of the motor is specified using the timebase per rev in microsecs, the current steps/rev of the motor and the required microsteps per step. The rate limits tested are 1 rev/sec to 1 rev per 240 secs. The default encoder is specified as a 2000 pulse per rev ie 2000/240 or >8 pulses/sec The ports in use are: RA0-RA3 Stepper output phases RA4-RA5 Stepper enable bits. RB0-RB3 PWM phase mask 4 bits ( selector mask for which stepper phase is being PWM'ed ) RB4-RB5 Encoder input signals RB6-RB7 not used RC0 not used RC1 PWM output lowest bit. RC3-RC4 I2C CSL and SDA lines RC5-RC7 not used RD0-RD7 Indicator LEDS power, step, microstep RE0-RE2 Not used Hardware units in use : TMR0 - not used TMR1 - stepping timer TMR2 - encoder timer CCP1 - PWM output for phase current control USART - I2C Revisions : Date Change 03 May 2009 Added back i2c code from i2c_test.c test project to get read/write commands working. Also updated command structure. 10 Feb 2009 Changed ports to avoid PWM masking split around HW I2C port. portd now used for LEDS and portb used for stepper masking outputs. 21 Jan 2009 Removed Timebase setup to MPU code. To allow timebase to be changed on the fly for guiding speeds. 15 Nov 2008 Stepping and microstepping forward and reverse checked. 12 Nov 2008 Checked microstepping debug_b & full stepping debug_c 12 Nov 2008 Checked stepping phases, PWM mask and PWM values align To Do: Need to add in time vars to EEprom settings read/write. Need to add write settings to eeprom every time a value is changed. Note would not expect highly volatile settings to be saved e.g encoder. unless at explicit sahutdown time. Change min speed to that specified by stepperStepsPerMicrostep as submitted to timebase Add encoder delta monitoring and delta time calculation routine Notes: AIM : to keep track at sidereal rate with a 200 step stepper Reqs(1) : 1 worm rev every 4 mins hence 200 steps per 4 mins :: 240secs/200 steps :: 0.83333 steps/sec Reqs(2) : 15" per sec and at least 20 microsteps per arc sec :: 15 * 20 * 0.833R :: 250 microsteps/step Reqs(3) : 4 ms per microstep base speed :: for faster speeds reduce number of microsteps ( ratio of 250:1 possible? ) If you want to use stepper step counting as an open loop location tracker in addition to the encoder define ENABLE_STEPPER_OPEN_LOOP_TRACKING to enable the declaration of separate variables and the change in use of the encoder variables to use stepper variables. */ //#defines //#define DEBUG #ifdef ENABLESETTINGS //used to turn on or off eeprom settings stores #define EEPROM_IN_USE 0xDE #endif #define ENABLE_HW_ENCODER #define ENABLE_STEPPER #ifndef ENABLE_STEPPER #undef ENABLE_STEPPER_CTR #endif #ifndef ENABLE_HW_ENCODER #define ENABLE_STEPPER_CTR #endif #include #include "pic_microstep.h" #include #include #include #include //#include unresolved dependency on __DYNAMIC_HEAD #use standard_io(C) //pointless - specify afterwards as fast all....! #ZERO_RAM #use fast_io(ALL) //#use delay (internal=8M, restart_WDT) //use macros to allow easy changes to output ports. void write_stepper_mask( int8 data ) { int8 temp; temp = ( (data << 4 ) & 0b11100000 ) | (data & 0x01); //preserve Rc 1,2,3,4 which are used by I2C and CCP1/2 //set_tris_c ( ( TRIS_C & 0b00011110 ) ); output_bit( PIN_D6, data & 0x04 ); output_bit( PIN_D7, data & 0x08 ); output_e( (data & 0x03 ) ); output_c( temp ); //rc7,6,5,0 } void write_stepper_phase( int8 data ) { output_d( (data & 0x0f ) << 2 ) ; //display output_a( data & 0x0f ); } #define write_encoder_bits( data ) output_e( (data & 0x03 ) ) #ifdef ENABLE_HW_ENCODER #int_RB void RB_isr() { BYTE byTemp; byTemp=(input_b() & 0x30); if (OldPortB != byTemp ) { OldPortB = byTemp; write_encoder_bits( byTemp ); if (RotaryEncoder.A == OldEncoder.A) { if (RotaryEncoder.B == RotaryEncoder.A) { if( encoderCount == 0 ) encoderCount = encoderRollover -1; else --encoderCount; } else { if( encoderCount == encoderRollover -1 ) encoderCount = 0; else ++encoderCount; } } else { OldEncoder.A = RotaryEncoder.A; if (RotaryEncoder.B == RotaryEncoder.A) { if( encoderCount == encoderRollover -1 ) encoderCount = 0; else ++encoderCount; } else { if( encoderCount == 0 ) encoderCount = encoderRollover -1; else --encoderCount; } } } // Handle more PORTB stuff here, like a button, or something. output_bit( PIN_D1, !(input(PIN_D1)) ); } #endif #ifdef ENABLE_STEPPER #int_TIMER1 void TIMER1_isr() { static int32 tempphase; //Use this to keep track of step times. TMR1 counts from XXX to 65536 where it rolls over. if (t1LoopCount > 0 ) { t1LoopCount--; } else { tempphase += phaseaccum; if ( (tempphase & 0xFFFF0000 ) >0 ) { tempphase = tempphase - 0xFFFF; set_timer1( t1ResetCount -1 ); } else set_timer1( t1ResetCount ); //restart our counting t1LoopCount = t1ResetLoopCount; //Set TMR1 interrupt flag for main loop processing interruptStatus |= TMR1_TIMEDOUT; } #if defined DEBUG interruptStatus |= TMR1_TIMEDOUT; #endif } #endif #int_TIMER2 void TIMER2_isr() { /* Either output a square wave and count the times between encoder ticks and adjust stepper rate (tracking) to compensate OR 2000 ppr encoder per 240 secs gives 8 pulses per second Could also measure the time until the encoder pulse arrives. Measures jitter on shaft. */ if( t2LoopCount > 0 ) t2LoopCount--; else { set_timer2( t2ResetCount); t2Loopcount = t2ResetLoopCount; output_bit(PIN_D0, !input(PIN_D0)); #ifdef ENABLE_HW_ENCODER encoderDelta = encoderCount - encoderOld; encoderOld = encoderCount; #endif interruptStatus |= TMR2_TIMEDOUT; } } /* #int_TIMER0 void TIMER0_isr() { //empty - use for a 500ms heart beat ? 500ms at guide speed is 2000/(240*2) pulses per sec ~ 4 - not enough } */ //LED is base port number * 8 + pin number //PortA is 40, B is 48, c is 56, D is 64, E is 72 //if flashes is zero just toggles. void flashLed( int led, int16 duration, int flashes ) { int i; if ( led > 40 && led < 75 ) { output_bit( led, !input(led) ); for ( i=0; i< flashes; i++ ) { delay_ms( duration ); output_bit( led, !input(led) ); } } } void setup_stepper(STEPPER_TYPE s, int steps, int microsteps, int status ) { int pwmMask; switch ( stepperType = s ) { //{ fullStepTable1, fullStepTable2, halfStepTable, bipolarStepTable, fullstepTable}; case UNIPOLAR_A: stepTable = fullStepTable1;break; case UNIPOLAR_B: stepTable = fullStepTable2;break; case UNIPOLAR_HALF: stepTable = halfStepTable;break; case BIPOLAR_A: case BIPOLAR_B: stepTable = bipolarStepTable;break; default:stepTable = halfStepTable;break; break; } stepperStatus = status; stepperStatusTarget = status; stepperStepCount = 0; stepperStepCountCurrent = steps; stepperStepCountTarget = steps; stepperMicrostepCount = 0; #ifdef ENABLE_STEPPER_CTR stepperPositionCount = 0L; stepperPositionTarget = 0L; stepperPositionRollover = steps*microsteps; stepperPositionLast = 0L; stepperPositionDelta = 0L; #endif //Remembering we are using half steps already we divide the required number of microsteps by two //and turn it into a microstep increment count switch(microsteps) { case 2: case 4: case 8: case 16: case 32: case 64: stepperMicrostepsPerStep = microsteps/2; stepperMicrostepsPerStepTarget = stepperMicrostepsPerStep; stepperMicrostepCountIncrement = 1;//MAXMICROSTEPSPERSTEP/microsteps; stepperMicrostepCountIncrementTarget = stepperMicrostepCountIncrement; break; case 0: default: stepperMicrostepsPerStep = 0; stepperMicrostepsPerStepTarget = 0; stepperMicrostepCountIncrement = 0; stepperMicrostepCountIncrementTarget = 0; break; } //initialise outputs. //update PWM mask control - need to think about which one is changing if ( (stepperStatus & STEPPER_DIRN_MASK ) == STEPPER_DIRN_FORWARD ) { stepMask = stepTable[stepperStepCount]; pwmMask = (stepTable[stepperStepCount+1 %STEPSPERPHASE] ^ stepTable[stepperStepCount]); pwmMask &= stepTable[stepperStepCount+1 %STEPSPERPHASE]; stepMask = stepTable[stepperStepCount] | stepTable[(stepperStepCount+1)%STEPSPERPHASE]; } else { pwmMask = (stepTable[(stepperStepCount-1) %STEPSPERPHASE] ^ stepTable[stepperStepCount]); pwmMask &= stepTable[stepperStepCount]; stepMask = stepTable[stepperStepCount] | stepTable[(stepperStepCount-1)%STEPSPERPHASE]; } write_stepper_phase( stepMask ); write_stepper_mask( pwmMask ); setMicrostepPWM( pwmTable[stepperMicrostepCount] ); } void setup_pwm() { //Aim for a 20KHz frequency with a 10 bit resolution // This sets the time the pulse is high each cycle. // the high time will be: // if value is LONG INT: value*(1/clock)*t2div // if value is INT: value*4*(1/clock)*t2div // for example a value of 30 and t2div of 1 the high time is 12us // WARNING: A value too high or low will prevent the output from changing. // The cycle time will be (1/clock)*4*t2div*(period+1) setup_ccp1(CCP_PWM); //20KHz at 20MHz setup_timer_2(T2_DIV_BY_4,64,16); //PWM at zero duty to start set_pwm1_duty( 0x000); } void setup_DeltaTimer(int32 incrementTime ) { //Note timer2 is in use by PWM for square wave generation float intbase; float Tosc; float fTemp1,fTemp2; //Timer 2 setup //Aiming for 1/2 sec intervals, Tosc = (float)(64 * 4 * 4 * 16 )/CLOCK;//4 prescale, 16 postscale intbase = (float)(incrementTime/1e6)/Tosc; //scaled clocks per interval fTemp2 = modf( intbase, &fTemp1); t2LoopCount = (int16) fTemp1; t2ResetLoopCount = t2LoopCount; fTemp2 = (fTemp2 * 65536); t2ResetCount = (int16) fTemp2; Tosc= 0; } void setMicrostepPWM( int16 count ) { set_pwm1_duty( count ); } void setup_encoder( int32 rollover, int32 initial, int countsPerStep) { #ifdef ENABLE_HW_ENCODER encoderTarget = initial; encoderCount = initial; encoderRollover = rollover; encoderCountsPerStep = countsPerStep; encoderOld = encoderCount; encoderDelta = 0; #endif } void setupPrescaler( int scale ) { disable_interrupts(INT_TIMER1); if ( t1Prescaler == 0 ) { setup_TIMER_1( T1_INTERNAL | T1_DIV_BY_1 ); } else if ( t1Prescaler == 1 ) { setup_TIMER_1( T1_INTERNAL | T1_DIV_BY_2 ); } else if ( t1Prescaler == 2 ) { setup_TIMER_1( T1_INTERNAL | T1_DIV_BY_4 ); } else //if ( t1Prescaler == 4 ) { setup_TIMER_1( T1_INTERNAL | T1_DIV_BY_8 ); } enable_interrupts(INT_TIMER1); } void init_config() { int temp = 0; //Initialise hardware config set_tris_a(0x00); //All outputs port_b_pullups(TRUE); set_tris_b(0b11110000); //RB7-4 inputs set_tris_d( 0x00 );//All outputs set_tris_c( 0b00011000 ); output_d( 0x01 ); set_tris_e( 0x00 );//All outputs //SET_TRIS_C(0b00011000); //Need PWM pin (c2) set out & SCL/SDA pins (C3/4) set to input. //setup_counters(RTCC_INTERNAL,RTCC_DIV_32); //setup_i2c(); setup_pwm(); //0.1 secs or 100,000 microsecs setup_DeltaTimer( (int32) 100000 ); #ifdef DEBUG //Default values for testing t1Prescaler = 0; phaseAccum = 89; t1ResetCount = 32000; t1LoopCount = 32000; t1ResetLoopCount = 32000; #else //Set default rate as sidereal : 239.3444/(200*16) t1Prescaler = 4; phaseAccum = 89; t1ResetCount = 45241; t1LoopCount = 0; t1ResetLoopCount = 32000; #endif #ifdef ENABLE_HW_ENCODER setup_encoder( 360*4*500, 0L, 1); #endif #ifdef ENABLE_STEPPER setupPrescaler( t1Prescaler ); //Initialise components state #define DEBUG_A #ifdef DEBUG_A setup_Stepper( UNIPOLAR_A, 48, 0, STEPPER_CLOCK_TOGGLE_LOW | STEPPER_MODE_MOTION | STEPPER_DIRN_FORWARD | STEPPER_ENABLE_ON ); //Sidereal rate, includes setting up the TMR1 prescaler. //239.3444 secs per rev //timebase = 239344000; setup_timebase( t1ResetCount, t1LoopCount ); #endif #ifdef DEBUG_B setup_Stepper( UNIPOLAR_B, 48, 0, STEPPER_CLOCK_TOGGLE_LOW | STEPPER_MODE_MOTION | STEPPER_DIRN_FORWARD | STEPPER_ENABLE_ON ); //test rates setup_timebase( tiResetCount, t1LoopCount ); #endif #ifdef DEBUG_C setup_Stepper( UNIPOLAR_HALF 48, 2, STEPPER_CLOCK_TOGGLE_LOW | STEPPER_MODE_MOTION | STEPPER_DIRN_REVERSE | STEPPER_ENABLE_OFF ); setup_timebase( t1ResetCount, t1LoopCount ); t1LoopCount = 0; #endif set_timer1( t1ResetCount ); set_timer2( t2ResetCount); #endif //enable stepper #ifdef ENABLESETTINGS temp = read_eeprom( 0 ); if ( temp == EEPROM_IN_USE ) readEESettings(); else writeEESettings(); #endif //enable_interrupts(INT_EXT); //setup_timer_0( RTCC_INTERNAL | RTCC_DIV_256 );//every 1/CLOCK * (4*256*256) secs //enable_interrupts(INT_TIMER0); enable_interrupts(INT_RB); enable_interrupts(INT_TIMER1); //remove stepper for debug of cmd sequence. enable_interrupts(INT_TIMER2); //remove encoder delta timer. enable_interrupts(INT_CCP1); enable_interrupts(INT_SSP); //Finally the global enable enable_interrupts(GLOBAL); output_bit(PIN_D0,1); } void setup_timebase( int16 newResetCount, int16 newLoopCount ) { disable_interrupts(INT_TIMER1); t1ResetLoopCount = newLoopCount; t1ResetCount = newResetCount; enable_interrupts(INT_TIMER1); } void main() { int i = 0; int temp; int16 dwTemp = 0; long wTemp = 0l; int32 qwTemp = 0; init_config(); //readEESettings(); //Event based processing While ( TRUE ) { #ifdef ENABLE_STEPPER if ( interruptStatus & TMR1_TIMEDOUT ) { step_motor(); //Make any changes requested by the main loop //Do mode-based processing if ( (stepperStatus & STEPPER_MODE_MASK) == STEPPER_MODE_MOTION ) { //Enable change flagged so slow to stop or start at slowest rate temp = (stepperStatus ^ stepperStatusTarget) & STEPPER_ENABLE_MASK; if ( temp != 0 ) { //Turn on motor if( stepperStatusTarget & STEPPER_ENABLE_MASK ) { //Set or re-use phase and D/C vars. kickoff new stepping activity //Requires timebase to be already setup setup_Stepper( UNIPOLAR_A, stepperStepCountTarget, stepperMicrostepsPerStepTarget, STEPPER_CLOCK_TOGGLE_LOW | STEPPER_MODE_MOTION | STEPPER_DIRN_FORWARD | STEPPER_ENABLE_ON ); } else //slow current rate to stop. //Turn off motor { // if faster than min step rate, slow to stepperMicrostepCountIncrementTarget if( stepperMicrostepCountIncrement > stepperMicrostepCountIncrementTarget ) { stepperMicrostepCountIncrement >>= 1; } //Have slowed as far as we can so turn off else { //mix in the new dirn flag to the stepperStatus. stepperStatus &= (!STEPPER_ENABLE_MASK); stepperStatus |= stepperStatusTarget & STEPPER_ENABLE_MASK; } } } //Handle rate adjustments //Direction change flagged so slow to slowest speed to change else if ( (stepperStatus ^ stepperStatusTarget)& STEPPER_DIRN_MASK ) { if ( stepperMicrostepCount == 0 ) { if( stepperMicrostepCountIncrement > stepperMicrostepCountIncrementTarget ) { stepperMicrostepCountIncrement >>= 1; } else //Have slowed as far as we can { //mix in the new dirn flag to the stepperStatus. stepperStatus ^= STEPPER_DIRN_MASK; stepperStatus |= (stepperStatusTarget & STEPPER_DIRN_MASK); } } } else if ( stepperMicrostepCountIncrement != stepperMicrostepCountIncrementTarget ) { if ( stepperMicrostepCount == 0 ) { if( stepperMicrostepCountIncrement > stepperMicrostepCountIncrementTarget ) { //decrease stepperMicrostepCountIncrement >>= 1; } else { //increase stepperMicrostepCountIncrement <<= 1; } } } } //This mode requires normal operation to handle stepper stepping. else if ( (stepperStatus & STEPPER_MODE_MASK) == STEPPER_MODE_POSITION_CONSTANT ) { #ifdef ENABLE_HW_ENCODER qwTemp = (encoderTarget - encoderCount)/encoderCountsPerStep; #else qwTemp = (stepperPositionTarget - stepperPositionCount); #endif temp = abs(qwTemp) >> 8; if ( qwTemp == 0 ) { stepperStatus &= !STEPPER_ENABLE_MASK; } else if ( qwTemp > 0 ) { if( temp == 0 ) { stepperMicrostepCountIncrement = stepperMicrostepCountIncrementTarget; stepperMicroStepsPerStep = stepperMicrostepsPerStepTarget; stepperStatus &= !STEPPER_DIRN_MASK; stepperStatus |= (qwTemp>0)? STEPPER_DIRN_FORWARD:STEPPER_DIRN_REVERSE; stepperStatus |= STEPPER_ENABLE_MASK; } else { if( stepperMicrostepCountIncrement < stepperMicrostepCountIncrementTarget ) stepperMicrostepCountIncrement <<= temp; } } } else if ( (stepperStatus & STEPPER_MODE_MASK) == STEPPER_MODE_POSITION_RAMPED ) { } else { //unknown status - major error. reset_cpu(); } //Clear flag interruptStatus ^= TMR1_TIMEDOUT; } #endif //ENABLE_STEPPER if ( interruptStatus & TMR2_TIMEDOUT ) { //Log the error from where we expect to be and change movement rate as required. #ifdef ENABLE_HW_ENCODER if ( (encoderTarget - encoderCount) > 0 ) { //increase speed } else { //decrease speed. } #else if ( ( stepperPositionTarget - stepperPositionCount ) > 0 ) { } else { } #endif interruptStatus ^= TMR2_TIMEDOUT; } } } void step_motor() { int index; if( stepperStatus & STEPPER_ENABLE_MASK ) { //Set the next step regardless - handle longer issues later stepperMicrostepCount += stepperMicrostepCountIncrement; /* Use this example to understand phasing U up, d Down, Forward Reverse 0b00001100 starting phase 0b00001100 0b0000D1U0 in between 0b0000101D 0b00001011 half step 0b00001011 0b000010d1 in between 0b000010U1 0b00001001 full step 0b00001001 */ //Check for microstep boundary conditions //Uses stepperMicroStepsPerStep instead of MAXMICROSTEPSPERSTEP since never want to go slower than //specified microsteps //Note that even when microsteps are disabled - still get half steps. if ( stepperMicrostepCount <= 0 || stepperMicrostepCount >= stepperMicroStepsPerStep ) { //Need to change this to align with current microstep increment not full step - otherwise wait for 1 sec before response. if ( stepperMicroStepCount <= 0 || stepperMicrostepsPerStepTarget <2 ) //step aligned or no microsteps { stepperMicroStepCount = 0; stepperMicrostepCountIncrement = stepperMicrostepCountIncrementTarget; if( (stepperStatus & STEPPER_DIRN_MASK ) == STEPPER_DIRN_FORWARD ) { stepperStepCount++; stepperStepCount %= STEPSPERPHASE; #ifdef ENABLE_STEPPER_CTR stepperPositionCount++; #endif //output new step phase stepMask = stepTable[stepperStepCount]; stepMask |= stepTable[ (stepperStepCount+1)%STEPSPERPHASE]; write_stepper_phase( stepMask ); //update PWM mask control - need to think about which one is changing pwmMask = stepTable[((stepperStepCount+1) %STEPSPERPHASE)] ; pwmMask ^= stepTable[stepperStepCount]; pwmMask &= stepTable[((stepperStepCount+1) %STEPSPERPHASE)]; write_stepper_mask( pwmMask ); //update PWM value index = ABS(MAXMICROSTEPSPERSTEP/stepperMicrostepsPerStep) * stepperMicrostepCount; pwmVal = pwmTable[index]; setMicrostepPWM( pwmVal ); } else //Reverse { stepperStepCount--; stepperStepCount %= STEPSPERPHASE; #if defined ENABLE_STEPPER_CTR stepperPositionCount--; #endif //output new step phase stepMask = stepTable[stepperStepCount]; stepMask |= stepTable[(stepperStepCount-1)%STEPSPERPHASE]; write_stepper_phase( stepMask ); //update PWM mask control - need to think about which one is changing pwmMask = stepTable[((stepperStepCount-1) %STEPSPERPHASE)]; pwmMask ^= stepTable[stepperStepCount]; pwmMask &= stepTable[((stepperStepCount-1) %STEPSPERPHASE)]; write_stepper_mask( pwmMask ); //update PWM value index = ABS(MAXMICROSTEPSPERSTEP/stepperMicrostepsPerStep) * stepperMicrostepCount; pwmVal = pwmTable[index]; setMicrostepPWM( pwmVal ); } } //Half step point else // if ( stepperMicroStepCount >= MAXMICROSTEPSPERSTEP ) { stepperMicrostepCountIncrement = -1 * stepperMicrostepCountIncrement; stepperMicroStepCount = stepperMicroStepsPerStep; if( (stepperStatus & STEPPER_DIRN_MASK) == STEPPER_DIRN_FORWARD ) { //update PWM mask control - need to think about which one is changing pwmMask = (stepTable[(stepperStepCount+1)%STEPSPERPHASE] ^ stepTable[stepperStepCount]); pwmMask &= stepTable[stepperStepCount]; write_stepper_mask( pwmMask ); //update PWM value index = ABS(MAXMICROSTEPSPERSTEP/stepperMicrostepsPerStep) * stepperMicrostepCount; pwmVal = pwmTable[index]; setMicrostepPWM( pwmVal ); } else //Reverse { //update PWM mask control - need to think about which one is changing pwmMask = (stepTable[(stepperStepCount-1) %STEPSPERPHASE] ^ stepTable[stepperStepCount]); pwmMask &= stepTable[stepperStepCount]; write_stepper_mask( pwmMask ); //update PWM value index = ABS(MAXMICROSTEPSPERSTEP/stepperMicrostepsPerStep) * stepperMicrostepCount; pwmVal = pwmTable[index]; setMicrostepPWM( pwmVal ); } } } //normal microstep handling else { index = stepperMicrostepCount * ABS(MAXMICROSTEPSPERSTEP / stepperMicrostepsPerStep); pwmVal = pwmTable[index]; setMicrostepPWM( pwmVal ); } } //Step processing complete }