CCS PCM C Compiler, Version 4.084, 39291 19-Jul-09 17:28 Filename: pic_microstep.lst ROM used: 4010 words (49%) Largest free fragment is 2048 RAM used: 123 (34%) at main() level 204 (57%) worst case Stack: 4 worst case (3 in main + 1 for interrupts) * 0000: NOP 0001: MOVLW 08 0002: MOVWF 0A 0003: GOTO 5ED 0004: MOVWF 7F 0005: SWAPF 03,W 0006: CLRF 03 0007: MOVWF 21 0008: MOVF 0A,W 0009: MOVWF 20 000A: CLRF 0A 000B: MOVF 04,W 000C: MOVWF 22 000D: MOVF 77,W 000E: MOVWF 23 000F: MOVF 78,W 0010: MOVWF 24 0011: MOVF 79,W 0012: MOVWF 25 0013: MOVF 7A,W 0014: MOVWF 26 0015: MOVF 7B,W 0016: MOVWF 27 0017: BCF 03.7 0018: BCF 03.5 0019: BTFSS 0B.3 001A: GOTO 01D 001B: BTFSC 0B.0 001C: GOTO 03C 001D: MOVLW 8C 001E: MOVWF 04 001F: BTFSS 00.0 0020: GOTO 023 0021: BTFSC 0C.0 0022: GOTO 03F 0023: MOVLW 8C 0024: MOVWF 04 0025: BTFSS 00.1 0026: GOTO 029 0027: BTFSC 0C.1 0028: GOTO 042 0029: MOVF 22,W 002A: MOVWF 04 002B: MOVF 23,W 002C: MOVWF 77 002D: MOVF 24,W 002E: MOVWF 78 002F: MOVF 25,W 0030: MOVWF 79 0031: MOVF 26,W 0032: MOVWF 7A 0033: MOVF 27,W 0034: MOVWF 7B 0035: MOVF 20,W 0036: MOVWF 0A 0037: SWAPF 21,W 0038: MOVWF 03 0039: SWAPF 7F,F 003A: SWAPF 7F,W 003B: RETFIE 003C: BCF 0A.3 003D: BCF 0A.4 003E: GOTO 0CB 003F: BCF 0A.3 0040: BCF 0A.4 0041: GOTO 1BA 0042: BCF 0A.3 0043: BCF 0A.4 0044: GOTO 222 .................... /* .................... 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 .................... /////////////////////////////////////////////////////////////////////////// .................... //// (C) Copyright 1996,2003 Custom Computer Services //// .................... //// This source code may only be used by licensed users of the CCS C //// .................... //// compiler. This source code may only be distributed to other //// .................... //// licensed users of the CCS C compiler. No other use, reproduction //// .................... //// or distribution is permitted without written permission. //// .................... //// Derivative programs created using this software in object code //// .................... //// form are not restricted in any way. //// .................... /////////////////////////////////////////////////////////////////////////// .................... .................... #ifndef _STDDEF .................... .................... #define _STDDEF .................... .................... #if sizeof(unsigned int8 *)==1 .................... #define ptrdiff_t unsigned int8 .................... #else .................... #define ptrdiff_t unsigned int16 .................... #endif .................... .................... #define size_t unsigned int8 .................... #define wchar_t char .................... #define NULL 0 .................... .................... #define offsetof(s,f) (offsetofbit(s,f)/8) .................... .................... #endif .................... .................... #include "pic_microstep.h" .................... //#include <16F887.h> .................... #include <16F877a.h> .................... //////// Standard Header file for the PIC16F877A device //////////////// .................... #device PIC16F877A .................... #list .................... .................... //#include <16C62a.h> .................... #device ICD=TRUE .................... #device *=16 .................... //#use delay(clock=8000000) .................... #fuses NOWDT,HS, NOPUT, NOPROTECT, BROWNOUT .................... //#define CLOCK ((int32)19660800) .................... #use delay(clock=20M, oscillator ) .................... #define CLOCK ((int32)20000000) .................... #define STEP_A PIN_A0 .................... #define STEP_B PIN_A1 .................... #define STEP_C PIN_A2 .................... #define STEP_D PIN_A3 .................... #define ENC_I PIN_B6 .................... #define ENC_B PIN_B4 .................... #define ENC_A PIN_B5 .................... #define STEP_LED PIN_D0 .................... #define ENC_LED PIN_D1 .................... #define TRIS_C 0x87 .................... .................... #byte SSPBUF = 0x13 .................... #use i2c(Slave,slow,sda=PIN_C4,scl=PIN_C3,force_hw,address=0xd8) .................... //Setup TRIS in init sequence. .................... #use fast_io(ALL) .................... //#use fast_io(B) .................... //#use fast_io(C) .................... .................... //Global variables .................... //Interrupt States .................... byte interruptStatus; .................... #define TMR1_TIMEDOUT 0x01 .................... #define TMR2_TIMEDOUT 0x02 .................... #define I2C_READ_PENDING 0x04 .................... #define I2C_READ_READY 0x08 .................... #define I2C_CMD_READY 0x10 .................... #define TMR1_TIMEDOUT_MASK 0x01 .................... #define TMR2_TIMEDOUT_MASK 0x02 .................... #define I2C_READ_PENDING_MASK 0x04 .................... #define I2C_READ_READY_MASK 0x08 .................... #define I2C_CMD_READY_MASK 0x10 .................... .................... //copy from telcontrol.h .................... //enum deviceType { NULL, RTCC, MOTOR, MOTOR_WITH_ENCODER, ENCODER, FOCUSER, FOCUSER_WITH_ENCODER, SHUTTER, CAMERA, DC_SERVO }; .................... #ifdef ENABLE_HW_ENCODER .................... #define DEVICE_NAME 0x0003 //From device type enum in telcontroller.h .................... #else .................... #define DEVICE_NAME 0x0002 //From device type enum in telcontroller.h .................... #endif .................... #define DEVICE_VERSION 0x0100 //software Version number, High byte is major, low byte is minor. .................... .................... #byte SSPBUF = 0x13 .................... #define SSPSTATMASK 0x3F .................... struct SSPSTATStruct .................... { .................... //Buffer Full : (receive) 1 = receive complete SSPBUFF full .................... //Buffer Full : (transmit) 1 = transmit in progress, SSPBUFF full .................... int BF:1; .................... //Update address , 1 indicates that user needs to update the address in the 10 bit SSPADD field .................... int UA:1; .................... //Write Low, holds R/WL bit of last address match. 1=read .................... int RWL:1; .................... //Start bit, 1 = start detected last. .................... int S:1; .................... //Stop bit, 1=stop detected last .................... int P:1; .................... //D/AL 1 - last byte received was data .................... int DAL:1; .................... //SMBus inputs enable .................... int CKE:1; .................... //Slew rate control bit - 1 disables for 100KHz and 1MHz .................... int SMP:1; .................... }; .................... struct SSPSTATStruct SSPSTAT; .................... #pragma BYTE SSPSTAT = 0x94 // Place structure right over SSPSTAT at location 0x94 .................... .................... #define SSPCONMASK 0xF0 .................... struct SSPCONTROLStruct .................... { .................... //SSP select .................... int SSPSEL:4; .................... //SSP Clock polarity .................... int CKP:1; .................... //Sync Serial port Enable - in I2C mode enables SSP as I2C using SDA and SCL .................... int SSPEN:1; .................... //SSP overflow, 1 when I2C byte received while SSPBUFF full, don't care in transmit. .................... //Must be cleared by software in either case .................... int SSPOV:1; .................... //Write collision, SSPBUF written while still transmitting previous byte, 1= collision .................... int WCOL:1; .................... }; .................... struct SSPCONTROLStruct SSPCON; .................... #pragma BYTE SSPCON = 0x14 .................... #define CMD 0 .................... #define DATA0 1 .................... #define DATA1 2 .................... #define DATA2 3 .................... #define DATA3 4 .................... #define SERIAL_BUFF_SIZE 10 .................... byte serialCmd; //Used to identify the I2C command .................... byte serialInCount = 0; //How many bytes have been processed inwards by the ISR 0E47: CLRF 2B .................... byte serialOutcount = 0; //How many bytes have been processed outwards by the ISR 0E48: CLRF 2C .................... byte serialBuffIn[SERIAL_BUFF_SIZE]; .................... byte serialBuffOut[SERIAL_BUFF_SIZE]; .................... byte serialBuffOutLength = 0; 0E49: CLRF 41 .................... .................... /* .................... These defines are the command IDS of the I2C data commands. .................... They should attempt to lign up so that the Commadn ID is also the register address for the data. .................... Hence Device name is always at register 00 length 2 .................... Hence device type is always at register 02 length 2 .................... The register concept does not map onto the file register they are stroed at but there is no reason why not.... .................... */ .................... //Device Commands .................... #define SERIAL_CMD_DEVICE_NAME_GET 0x00 .................... #define SERIAL_CMD_DEVICE_VERSION_GET 0x01 .................... .................... //Stepper CMDs .................... #define SERIAL_CMD_STEPPER_STEPS_SET 0x10 .................... #define SERIAL_CMD_STEPPER_STEPS_GET 0x11 .................... #define SERIAL_CMD_STEPPER_DIRECTION_SET 0x12 .................... #define SERIAL_CMD_STEPPER_DIRECTION_GET 0x13 .................... #define SERIAL_CMD_STEPPER_MICROSTEPS_SET 0x14 //Use this to accelerate by factor two's .................... #define SERIAL_CMD_STEPPER_MICROSTEPS_GET 0x15 .................... #define SERIAL_CMD_STEPPER_MODE_SET 0x16 .................... #define SERIAL_CMD_STEPPER_MODE_GET 0x17 .................... #define SERIAL_CMD_STEPPER_ACCEL_SET 0x18 .................... #define SERIAL_CMD_STEPPER_ACCEL_GET 0x19 .................... #define SERIAL_CMD_STEPPER_ENABLE_SET 0x1A .................... #define SERIAL_CMD_STEPPER_ENABLE_GET 0x1B .................... #define SERIAL_CMD_STEPPER_CTR_SET 0x1C .................... #define SERIAL_CMD_STEPPER_CTR_GET 0x1D .................... #define SERIAL_CMD_STEPPER_CTR_RESET 0x1E .................... #define SERIAL_CMD_STEPPER_CTR_ROLLOVER_SET 0x1F .................... #define SERIAL_CMD_STEPPER_CTR_ROLLOVER_GET 0x20 .................... #define SERIAL_CMD_STEPPER_CTR_TARGET_SET 0x21 .................... #define SERIAL_CMD_STEPPER_CTR_TARGET_GET 0x22 .................... #define SERIAL_CMD_STEPPER_STOP_ENABLE_SET 0x23 .................... #define SERIAL_CMD_STEPPER_STOP_ENABLE_GET 0x24 .................... #define SERIAL_CMD_STEPPER_STEPSPERREV_SET 0x25 .................... #define SERIAL_CMD_STEPPER_STEPSPERREV_GET 0x26 .................... .................... //encoder CMDs .................... #define SERIAL_CMD_ENCODER_SET 0x30 .................... #define SERIAL_CMD_ENCODER_GET 0x31 .................... #define SERIAL_CMD_ENCODER_ROLLOVER_SET 0x32 .................... #define SERIAL_CMD_ENCODER_ROLLOVER_GET 0x33 .................... #define SERIAL_CMD_ENCODER_TARGET_SET 0x34 .................... #define SERIAL_CMD_ENCODER_TARGET_GET 0x35 .................... #define SERIAL_CMD_ENCODER_COUNTS_PER_STEP_SET 0x36 .................... #define SERIAL_CMD_ENCODER_COUNTS_PER_STEP_GET 0x37 .................... #define SERIAL_CMD_ENCODER_RESET 0x38 .................... //TIMEBASE CMDS .................... #define SERIAL_CMD_TIMEBASE_SET 0x40 .................... #define SERIAL_CMD_TIMEBASE_GET 0x41 .................... #define SERIAL_CMD_TIME_ACCUMULATOR_SET 0x42 .................... #define SERIAL_CMD_TIME_ACCUMULATOR_GET 0x43 .................... #define SERIAL_CMD_TIME_PRESCALER_SET 0x44 .................... #define SERIAL_CMD_TIME_PRESCALER_GET 0x45 .................... .................... //Application CMDs .................... #define SERIAL_CMD_APP_RESET 0xDB .................... #define SERIAL_CMD_APP_INIT 0xDE .................... #define SERIAL_CMD_APP_SHUT 0xDF .................... .................... #ifdef ENABLE_HW_ENCODER .................... //Encoder status in & out .................... struct RotaryEncoderStruct .................... { .................... int unused:4; // RB[0:3] .................... int A:1; // RB4 .................... int B:1; // RB5 .................... int Index:1; //RB6 .................... int unused2:1; // RB7 .................... }; .................... struct RotaryEncoderStruct RotaryEncoder; .................... #pragma BYTE RotaryEncoder = 0x06 // Place structure right over PORTB at location 0x06 .................... static BYTE OldPortB; 0E4A: CLRF 42 .................... static struct RotaryEncoderStruct OldEncoder; 0E4B: CLRF 43 .................... int32 encoderCount; .................... int32 encoderTarget; .................... int32 encoderRollover; .................... int32 encoderCountsPerStep; .................... int32 encoderOld; .................... int32 encoderDelta; .................... #endif .................... .................... //Stepper status variables and masks .................... enum STEPPER_TYPE { UNIPOLAR_A,//4-phase 5,6 or 8 wires, full step .................... UNIPOLAR_B,//alternate form to A .................... UNIPOLAR_HALF,//half step .................... BIPOLAR_A, //serial bipolar 8 wires .................... BIPOLAR_B, //parallel bipolar 8 wires .................... BIPOLAR_C}; .................... int const STEPSPERPHASE = 8; .................... #define MAXMICROSTEPINCREMENT 64 .................... #define MAXMICROSTEPSPERSTEP 64 .................... //Table of unipolar step phases, bipolar parallel and bipolar serial are the same.( read 1=+, 0=- ) .................... //table of ptrs to phase tables for the respective motor types. .................... //int16* const stepPtrTable[6] = { stepTable, stepTable, stepTable, stepTable, stepTable, stepTable }; .................... .................... byte const fullstepTable1[STEPSPERPHASE]={ .................... 0b00000011, .................... 0b00000110, .................... 0b00001100, .................... 0b00001001, .................... 0b00000011, .................... 0b00000110, .................... 0b00001100, .................... 0b00001001 }; .................... /*Jones Full step table*/ .................... byte const fullstepTable2[STEPSPERPHASE]={ .................... 0b00000001, .................... 0b00000100, .................... 0b00000010, .................... 0b00001000, .................... 0b00000001, .................... 0b00000100, .................... 0b00000010, .................... 0b00001000 }; .................... /*Jones Half stepping table */ .................... byte const halfstepTable[STEPSPERPHASE]={ .................... 0b00000001, .................... 0b00000101, .................... 0b00000100, .................... 0b00000110, .................... 0b00000010, .................... 0b00001010, .................... 0b00001000, .................... 0b00001001 }; .................... /*Original*/ .................... byte const bipolarstepTable[STEPSPERPHASE]={ .................... 0b00001010, .................... 0b00000110, .................... 0b00000101, .................... 0b00001001, .................... 0b00001010, .................... 0b00000110, .................... 0b00000101, .................... 0b00001001 }; .................... byte *stepTable;// = { fullStepTable1, fullStepTable2, halfStepTable, bipolarStepTable, fullstepTable}; .................... .................... int16 const pwmTable[MAXMICROSTEPSPERSTEP+1] = {0,1,2,6,10,15,22,30,39,49,60,73,86,101,116,133,150,168,187,207,228,249,271,293,316,340,363,388,412, .................... 437,462,487,512,537,562,587,612,636,661,684,708,731,753,775,796,817,837,856,874,891,908,923,938,951,964,975,985,994,1002,1009, .................... 1014,1018,1022,1023,1024 }; .................... enum STEPPER_TYPE stepperType; .................... int stepperStatus; //Current status .................... int stepperStatusTarget; //Desired status .................... int stepperStepCount; //Current steps per phase .................... int stepperStepCountCurrent; //Current step count per phase .................... int stepperStepCountTarget; //Target step count per phase .................... int stepperMicrostepCount; //Current microstep count .................... signed int stepperMicrostepCountIncrement; //Current microstep increment size .................... int stepperMicrostepCountIncrementTarget; //Desired increment for microstepping - sets speed along with timebase. .................... int stepperMicrostepsPerStep; //Stepper number of microsteps per step - used to generate microstep increment and timebase .................... int stepperMicrostepsPerStepTarget; //Desired number of microsteps per step .................... //For debugging WATCH purposes .................... int pwmMask; .................... int16 pwmVal; .................... int stepMask; .................... #if defined ENABLE_STEPPER_CTR .................... //If an encoder is not fitted, these variable can be usaed for 'open-loop' tracking .................... int32 stepperPositionCount; .................... int32 stepperPositionTarget; .................... int32 stepperPositionRollover; .................... int32 stepperPositionLast; .................... int32 stepperPositionDelta; .................... #endif .................... .................... //CMD Arguments ( status bits 0 ) for stepper clock direction .................... #define STEPPER_CLOCK_TOGGLE_MASK 0x01 .................... #define STEPPER_CLOCK_TOGGLE_LOW 0x00 .................... #define STEPPER_CLOCK_TOGGLE_HIGH 0x00 .................... .................... //CMD Arguments ( status bits 1 ) for stepper direction .................... #define STEPPER_DIRN_MASK 0x02 .................... #define STEPPER_DIRN_FORWARD 0x02 .................... #define STEPPER_DIRN_REVERSE 0x00 .................... .................... //CMD Arguments ( status bits 2 )for open-loop position counting .................... #define STEPPER_COUNT_MASK 0x04 .................... #define STEPPER_COUNT_ENABLE 0x04 .................... #define STEPPER_COUNT_DISABLE 0x04 .................... .................... //CMD Arguments ( status bits 3 )for open-loop position counting .................... .................... //CMD Arguments (status bits 5-6 ) for stepper mode .................... #define STEPPER_MODE_MASK 0x60 .................... //Move uniformly at rate specified by timebase and microstep increment, continuously. .................... #define STEPPER_MODE_MOTION 0x00 .................... //Seek to encoder target value . .................... #define STEPPER_MODE_POSITION_CONSTANT 0x40 .................... //Seek to encoder target value using ramping profile .................... #define STEPPER_MODE_POSITION_RAMPED 0x60 .................... .................... //CMD Arguments (status bits 7 ) for stepper enable .................... #define STEPPER_ENABLE_MASK 0x80 .................... #define STEPPER_ENABLE_ON 0x80 .................... #define STEPPER_ENABLE_OFF 0x00 .................... .................... //PWM status variables and settings .................... //int16 dutyCycle; // up to 10 bits of resolution of duty cycle. .................... //int incrementSign; //Add or delete the increment. .................... .................... //TMR1 timing variables. .................... int32 phaseAccum; //the decimal remainder added until it is significant and then subtracted from the reset value on use. .................... int t1Prescaler = 0; 0E4C: CLRF 6D .................... int16 t1ResetCount; //the value to reset the timer to. .................... int16 t1ResetLoopCount; //the value to reset the timer timeout loop counter to .................... int16 t1LoopCount; //How many times we have looped around the TMR1 16 bit timeout. .................... int16 newResetCount; //Holders for new values .................... int16 newResetLoopCount;//Holders for new values. .................... .................... //TMR2 Timing variables .................... //TMR2 is owned by PWM prescaler, however can count loops. Use time increment to measure encoder deltas .................... int16 t2LoopCount; .................... int16 t2ResetLoopCount; .................... int16 t2ResetCount; .................... .................... /* Timing considerations for star-tracking .................... Sidereal day is 86400-3m56 secs per day = 86164 secs; .................... Clock frequency is 19660800 Hz, each clock is 50.999 ns .................... Hence Tosc frequency is 4915200 Hz .................... Need to do 1 rev of the telescope in 86164 secs per day, or 1 worm rev in 86164/360 secs = 239.3444R secs .................... .................... With a 200 step stepper need to do 200 steps in 239.3444R secs, each with up to 32 microsteps .................... Thus each microstep is ( at sidereal stepping rate ) 239.3444R/( 200*32)=0.0373975694444375 secs long .................... Which is 183323.216128 Tosc clocks .................... OR 44.7564456 (Tosc 255 clock timeouts and x16 postscaler timeouts). .................... OR 716.10631301 256 count timeouts. .................... OR 2.7972902851 which is 2 full 16bit timeouts and 52251.2161 counts .................... OR 1 full 16 bit timeout with div 2 prescaler and 26125.608064 counts .................... .................... Table : steppers and microstep time .................... 100 200 400 .................... time(s) 0.037397569 .................... TMR1 1 & 26125.608064 .................... The PWM period = [(PR2) + 1] • 4 • TOSC • (TMR2 prescale value) .................... Hence the PWM period is 256 * 4*50.999*1 = 53476327.424 or 19.148 KHz .................... */ .................... .................... /* Function prototypes */ .................... void setup_timebase( int16 newResetCount, int16 newLoopCount ); .................... void setup_DeltaTimer(int32 incrementTime ); .................... void setup_stepper(STEPPER_TYPE s, int steps, int microsteps, int status ); .................... void setup_pwm(); .................... void setMicrostepPWM( int16 count ); .................... void setup_encoder( int32 rollover, int32 current ); .................... void setup_encoder( int32 rollover, int32 initial, int countsPerStep); .................... void step_motor(); .................... //Indicators .................... void flashLed( int, int16, int ); .................... .................... .................... .................... .................... #include .................... /////////////////////////////////////////////////////////////////////////// .................... //// (C) Copyright 1996,2003 Custom Computer Services //// .................... //// This source code may only be used by licensed users of the CCS C //// .................... //// compiler. This source code may only be distributed to other //// .................... //// licensed users of the CCS C compiler. No other use, reproduction //// .................... //// or distribution is permitted without written permission. //// .................... //// Derivative programs created using this software in object code //// .................... //// form are not restricted in any way. //// .................... /////////////////////////////////////////////////////////////////////////// .................... .................... /* errno.h */ .................... #ifndef _ERRNO .................... #define _ERRNO .................... .................... #define EDOM 1 .................... #define ERANGE 2 .................... .................... int errno=0; 0E4D: CLRF 7E .................... #endif .................... .................... #include .................... /////////////////////////////////////////////////////////////////////////// .................... //// (C) Copyright 1996,2003 Custom Computer Services //// .................... //// This source code may only be used by licensed users of the CCS C //// .................... //// compiler. This source code may only be distributed to other //// .................... //// licensed users of the CCS C compiler. No other use, reproduction //// .................... //// or distribution is permitted without written permission. //// .................... //// Derivative programs created using this software in object code //// .................... //// form are not restricted in any way. //// .................... /////////////////////////////////////////////////////////////////////////// .................... .................... /* float.h */ .................... .................... .................... #ifndef _FLOAT .................... #define _FLOAT .................... /* Float properties */ .................... .................... #define FLT_RADIX 2 .................... #define FLT_MANT_DIG 24 // # of bits in mantissa .................... .................... #define FLT_DIG 6 // # of decimal digits of precision .................... .................... #define FLT_MIN_EXP (-125) // min binary exponent .................... #define FLT_MIN_10_EXP (-37) // min decimal exponent .................... #define FLT_MAX_EXP 128 // max binary exponent .................... #define FLT_MAX_10_EXP 38 // max decimal exponent .................... #define FLT_MAX 3.402823466e+38F // max value .................... #define FLT_EPSILON 1.192092896e-07F // smallest such that 1.0+FLT_EPSILON != 1.0 .................... #define FLT_MIN 1.175494351e-38F // min positive value .................... .................... .................... /* Double properties */ .................... #define DBL_MANT_DIG 53 // # of bits in mantissa .................... .................... #define DBL_DIG 15 // # of decimal digits of precision .................... .................... #define DBL_MIN_EXP (-1021) // min binary exponent .................... #define DBL_MIN_10_EXP (-307) // min decimal exponent .................... #define DBL_MAX_EXP 1024 // max binary exponent .................... #define DBL_MAX_10_EXP 308 // max decimal exponent .................... #define DBL_MAX 1.79769313486231e+308F // max value .................... #define DBL_EPSILON 2.2204460492503131e-16F // smallest such that 1.0+FLT_EPSILON != 1.0 .................... #define DBL_MIN 2.22507385850721e-308F // min positive value .................... .................... /*Long double properties */ .................... .................... #define LDBL_MANT_DIG 64 // # of bits in mantissa .................... .................... #define LDBL_DIG 18 // # of decimal digits of precision .................... .................... #define LDBL_MIN_EXP (-16381) // min binary exponent .................... #define LDBL_MIN_10_EXP (-4931) // min decimal exponent .................... #define LDBL_MAX_EXP 16384 // max binary exponent .................... #define LDBL_MAX_10_EXP 4932 // max decimal exponent .................... #define LDBL_MAX 1.18973149535723176502e+4932F // max value .................... #define LDBL_EPSILON 1.084202172485504434e-019F // smallest such that 1.0+FLT_EPSILON != 1.0 .................... #define LDBL_MIN 3.36210314311209350626e-4932F // min positive value .................... #endif .................... .................... .................... .................... #include .................... /////////////////////////////////////////////////////////////////////////// .................... //// (C) Copyright 1996,2007 Custom Computer Services //// .................... //// This source code may only be used by licensed users of the CCS C //// .................... //// compiler. This source code may only be distributed to other //// .................... //// licensed users of the CCS C compiler. No other use, reproduction //// .................... //// or distribution is permitted without written permission. //// .................... //// Derivative programs created using this software in object code //// .................... //// form are not restricted in any way. //// .................... /////////////////////////////////////////////////////////////////////////// .................... .................... /* limits.h */ .................... .................... .................... #ifndef _LIMITS .................... #define _LIMITS .................... #define _C2 1 .................... .................... #define CHAR_BIT 1 /* short */ .................... .................... /* unsigned char properties*/ .................... /* treated as unsigned integer*/ .................... #define CHAR_MAX 255 /*unsigned integer maximum*/ .................... #define CHAR_MIN 0 /*unsigned integer minimum*/ .................... .................... /*signed char properties */ .................... #define SCHAR_MAX 127 /*signed integer maximum*/ .................... #define SCHAR_MIN (-127-_C2) /*signed integer minimum*/ .................... .................... /* multibyte properties */ .................... /* multibyte not supported */ .................... #define MB_LEN_MAX 1 .................... .................... #if defined(__PCD__) .................... /* signed short properties */ .................... #define SHRT_MAX 127 /* signed short maximum*/ .................... #define SHRT_MIN (-127-_C2) /* signed short minimum*/ .................... .................... /* signed int properties */ .................... #define INT_MAX 32767 /* signed integer minimum*/ .................... #define INT_MIN (-32767-_C2) /*signed integer maximum*/ .................... .................... /* signed long properties */ .................... #define LONG_MAX 2147483647 /* signed long maximum*/ .................... #define LONG_MIN (-2147483647-_C2) /* signed long minimum*/ .................... .................... /* unsigned properties */ .................... #define UCHAR_MAX 255 /*unsigned integer maximum*/ .................... #define USHRT_MAX 255 /*unsigned short integer maximum*/ .................... #define UINT_MAX 65535 /*unsigned integer maximum*/ .................... #define ULONG_MAX 4294967295 /*unsigned long integer maximum*/ .................... #else .................... /* signed short properties */ .................... #define SHRT_MAX 1 /* signed short maximum*/ .................... #define SHRT_MIN 0 /* signed short minimum*/ .................... .................... #define INT_MAX 127 /* signed integer minimum*/ .................... #define INT_MIN (-127-_C2) /*signed integer maximum*/ .................... .................... /* signed long properties */ .................... #define LONG_MAX 32767 /* signed long maximum*/ .................... #define LONG_MIN (-32767-_C2) /* signed long minimum*/ .................... .................... /* unsigned properties */ .................... #define USHRT_MAX 1 /*unsigned short integer maximum*/ .................... #define UCHAR_MAX 255 /*unsigned integer maximum*/ .................... #define UINT_MAX 255 /*unsigned integer maximum*/ .................... #define ULONG_MAX 65535/*unsigned long integer maximum*/ .................... #endif .................... .................... #endif .................... .................... #include .................... //////////////////////////////////////////////////////////////////////////// .................... //// (C) Copyright 1996,2008 Custom Computer Services //// .................... //// This source code may only be used by licensed users of the CCS C //// .................... //// compiler. This source code may only be distributed to other //// .................... //// licensed users of the CCS C compiler. No other use, reproduction //// .................... //// or distribution is permitted without written permission. //// .................... //// Derivative programs created using this software in object code //// .................... //// form are not restricted in any way. //// .................... //////////////////////////////////////////////////////////////////////////// .................... //// //// .................... //// History: //// .................... //// * 9/20/2001 : Improvments are made to sin/cos code. //// .................... //// The code now is small, much faster, //// .................... //// and more accurate. //// .................... //// * 2/21/2007 : Compiler handles & operator differently and does .................... //// not return generic (int8 *) so type cast is done //// .................... //// //// .................... //////////////////////////////////////////////////////////////////////////// .................... .................... #ifndef MATH_H .................... #define MATH_H .................... .................... #ifdef PI .................... #undef PI .................... #endif .................... #define PI 3.1415926535897932 .................... .................... .................... #define SQRT2 1.4142135623730950 .................... .................... //float const ps[4] = {5.9304945, 21.125224, 8.9403076, 0.29730279}; .................... //float const qs[4] = {1.0000000, 15.035723, 17.764134, 2.4934718}; .................... .................... ///////////////////////////// Round Functions ////////////////////////////// .................... .................... float32 CEIL_FLOOR(float32 x, unsigned int8 n) .................... { .................... float32 y, res; .................... unsigned int16 l; .................... int1 s; .................... .................... s = 0; * 05A5: BSF 03.5 05A6: BCF 68.0 .................... y = x; 05A7: MOVF 5C,W 05A8: MOVWF 61 05A9: MOVF 5B,W 05AA: MOVWF 60 05AB: MOVF 5A,W 05AC: MOVWF 5F 05AD: MOVF 59,W 05AE: MOVWF 5E .................... .................... if (x < 0) 05AF: MOVF 5C,W 05B0: MOVWF 6C 05B1: MOVF 5B,W 05B2: MOVWF 6B 05B3: MOVF 5A,W 05B4: MOVWF 6A 05B5: MOVF 59,W 05B6: MOVWF 69 05B7: BCF 03.5 05B8: BSF 03.6 05B9: CLRF 13 05BA: CLRF 12 05BB: CLRF 11 05BC: CLRF 10 05BD: BCF 03.6 05BE: CALL 353 05BF: BTFSS 03.0 05C0: GOTO 5C7 .................... { .................... s = 1; 05C1: BSF 03.5 05C2: BSF 68.0 .................... y = -y; 05C3: MOVF 5F,W 05C4: XORLW 80 05C5: MOVWF 5F 05C6: BCF 03.5 .................... } .................... .................... if (y <= 32768.0) 05C7: BSF 03.5 05C8: MOVF 61,W 05C9: MOVWF 6C 05CA: MOVF 60,W 05CB: MOVWF 6B 05CC: MOVF 5F,W 05CD: MOVWF 6A 05CE: MOVF 5E,W 05CF: MOVWF 69 05D0: BCF 03.5 05D1: BSF 03.6 05D2: CLRF 13 05D3: CLRF 12 05D4: CLRF 11 05D5: MOVLW 8E 05D6: MOVWF 10 05D7: BCF 03.6 05D8: CALL 353 05D9: BTFSC 03.0 05DA: GOTO 5DD 05DB: BTFSS 03.2 05DC: GOTO 5FA .................... res = (float32)(unsigned int16)y; 05DD: BSF 03.5 05DE: MOVF 61,W 05DF: MOVWF 6C 05E0: MOVF 60,W 05E1: MOVWF 6B 05E2: MOVF 5F,W 05E3: MOVWF 6A 05E4: MOVF 5E,W 05E5: MOVWF 69 05E6: BCF 03.5 05E7: CALL 3A8 05E8: MOVF 79,W 05E9: BSF 03.5 05EA: MOVWF 6E 05EB: MOVF 78,W 05EC: MOVWF 6D 05ED: BCF 03.5 05EE: CALL 3C9 05EF: MOVF 7A,W 05F0: BSF 03.5 05F1: MOVWF 65 05F2: MOVF 79,W 05F3: MOVWF 64 05F4: MOVF 78,W 05F5: MOVWF 63 05F6: MOVF 77,W 05F7: MOVWF 62 .................... .................... else if (y < 10000000.0) 05F8: GOTO 6F4 05F9: BCF 03.5 05FA: BSF 03.5 05FB: MOVF 61,W 05FC: MOVWF 6C 05FD: MOVF 60,W 05FE: MOVWF 6B 05FF: MOVF 5F,W 0600: MOVWF 6A 0601: MOVF 5E,W 0602: MOVWF 69 0603: MOVLW 80 0604: BCF 03.5 0605: BSF 03.6 0606: MOVWF 13 0607: MOVLW 96 0608: MOVWF 12 0609: MOVLW 18 060A: MOVWF 11 060B: MOVLW 96 060C: MOVWF 10 060D: BCF 03.6 060E: CALL 353 060F: BTFSS 03.0 0610: GOTO 6EB .................... { .................... l = (unsigned int16)(y/32768.0); 0611: BSF 03.5 0612: MOVF 61,W 0613: MOVWF 6C 0614: MOVF 60,W 0615: MOVWF 6B 0616: MOVF 5F,W 0617: MOVWF 6A 0618: MOVF 5E,W 0619: MOVWF 69 061A: BCF 03.5 061B: BSF 03.6 061C: CLRF 13 061D: CLRF 12 061E: CLRF 11 061F: MOVLW 8E 0620: MOVWF 10 0621: BCF 03.6 0622: CALL 260 0623: MOVF 7A,W 0624: BSF 03.5 0625: MOVWF 6C 0626: MOVF 79,W 0627: MOVWF 6B 0628: MOVF 78,W 0629: MOVWF 6A 062A: MOVF 77,W 062B: MOVWF 69 062C: BCF 03.5 062D: CALL 3A8 062E: MOVF 79,W 062F: BSF 03.5 0630: MOVWF 67 0631: MOVF 78,W 0632: MOVWF 66 .................... y = 32768.0*(y/32768.0 - (float32)l); 0633: MOVF 61,W 0634: MOVWF 6C 0635: MOVF 60,W 0636: MOVWF 6B 0637: MOVF 5F,W 0638: MOVWF 6A 0639: MOVF 5E,W 063A: MOVWF 69 063B: BCF 03.5 063C: BSF 03.6 063D: CLRF 13 063E: CLRF 12 063F: CLRF 11 0640: MOVLW 8E 0641: MOVWF 10 0642: BCF 03.6 0643: CALL 260 0644: MOVF 77,W 0645: BSF 03.5 0646: MOVWF 69 0647: MOVF 78,W 0648: MOVWF 6A 0649: MOVF 79,W 064A: MOVWF 6B 064B: MOVF 7A,W 064C: MOVWF 6C 064D: MOVF 67,W 064E: MOVWF 6E 064F: MOVF 66,W 0650: MOVWF 6D 0651: BCF 03.5 0652: CALL 3C9 0653: BSF 03.1 0654: BSF 03.5 0655: MOVF 6C,W 0656: BCF 03.5 0657: BSF 03.6 0658: MOVWF 13 0659: BSF 03.5 065A: BCF 03.6 065B: MOVF 6B,W 065C: BCF 03.5 065D: BSF 03.6 065E: MOVWF 12 065F: BSF 03.5 0660: BCF 03.6 0661: MOVF 6A,W 0662: BCF 03.5 0663: BSF 03.6 0664: MOVWF 11 0665: BSF 03.5 0666: BCF 03.6 0667: MOVF 69,W 0668: BCF 03.5 0669: BSF 03.6 066A: MOVWF 10 066B: MOVF 7A,W 066C: MOVWF 17 066D: MOVF 79,W 066E: MOVWF 16 066F: MOVF 78,W 0670: MOVWF 15 0671: MOVF 77,W 0672: MOVWF 14 0673: BCF 03.6 0674: CALL 3E8 0675: BSF 03.6 0676: CLRF 13 0677: CLRF 12 0678: CLRF 11 0679: MOVLW 8E 067A: MOVWF 10 067B: MOVF 7A,W 067C: MOVWF 17 067D: MOVF 79,W 067E: MOVWF 16 067F: MOVF 78,W 0680: MOVWF 15 0681: MOVF 77,W 0682: MOVWF 14 0683: BCF 03.6 0684: CALL 52E 0685: MOVF 7A,W 0686: BSF 03.5 0687: MOVWF 61 0688: MOVF 79,W 0689: MOVWF 60 068A: MOVF 78,W 068B: MOVWF 5F 068C: MOVF 77,W 068D: MOVWF 5E .................... res = 32768.0*(float32)l; 068E: MOVF 67,W 068F: MOVWF 6E 0690: MOVF 66,W 0691: MOVWF 6D 0692: BCF 03.5 0693: CALL 3C9 0694: BSF 03.6 0695: CLRF 13 0696: CLRF 12 0697: CLRF 11 0698: MOVLW 8E 0699: MOVWF 10 069A: MOVF 7A,W 069B: MOVWF 17 069C: MOVF 79,W 069D: MOVWF 16 069E: MOVF 78,W 069F: MOVWF 15 06A0: MOVF 77,W 06A1: MOVWF 14 06A2: BCF 03.6 06A3: CALL 52E 06A4: MOVF 7A,W 06A5: BSF 03.5 06A6: MOVWF 65 06A7: MOVF 79,W 06A8: MOVWF 64 06A9: MOVF 78,W 06AA: MOVWF 63 06AB: MOVF 77,W 06AC: MOVWF 62 .................... res += (float32)(unsigned int16)y; 06AD: MOVF 61,W 06AE: MOVWF 6C 06AF: MOVF 60,W 06B0: MOVWF 6B 06B1: MOVF 5F,W 06B2: MOVWF 6A 06B3: MOVF 5E,W 06B4: MOVWF 69 06B5: BCF 03.5 06B6: CALL 3A8 06B7: MOVF 79,W 06B8: BSF 03.5 06B9: MOVWF 6E 06BA: MOVF 78,W 06BB: MOVWF 6D 06BC: BCF 03.5 06BD: CALL 3C9 06BE: BCF 03.1 06BF: BSF 03.5 06C0: MOVF 65,W 06C1: BCF 03.5 06C2: BSF 03.6 06C3: MOVWF 13 06C4: BSF 03.5 06C5: BCF 03.6 06C6: MOVF 64,W 06C7: BCF 03.5 06C8: BSF 03.6 06C9: MOVWF 12 06CA: BSF 03.5 06CB: BCF 03.6 06CC: MOVF 63,W 06CD: BCF 03.5 06CE: BSF 03.6 06CF: MOVWF 11 06D0: BSF 03.5 06D1: BCF 03.6 06D2: MOVF 62,W 06D3: BCF 03.5 06D4: BSF 03.6 06D5: MOVWF 10 06D6: MOVF 7A,W 06D7: MOVWF 17 06D8: MOVF 79,W 06D9: MOVWF 16 06DA: MOVF 78,W 06DB: MOVWF 15 06DC: MOVF 77,W 06DD: MOVWF 14 06DE: BCF 03.6 06DF: CALL 3E8 06E0: MOVF 7A,W 06E1: BSF 03.5 06E2: MOVWF 65 06E3: MOVF 79,W 06E4: MOVWF 64 06E5: MOVF 78,W 06E6: MOVWF 63 06E7: MOVF 77,W 06E8: MOVWF 62 .................... } .................... .................... else 06E9: GOTO 6F4 06EA: BCF 03.5 .................... res = y; 06EB: BSF 03.5 06EC: MOVF 61,W 06ED: MOVWF 65 06EE: MOVF 60,W 06EF: MOVWF 64 06F0: MOVF 5F,W 06F1: MOVWF 63 06F2: MOVF 5E,W 06F3: MOVWF 62 .................... .................... y = y - (float32)(unsigned int16)y; 06F4: MOVF 61,W 06F5: MOVWF 6C 06F6: MOVF 60,W 06F7: MOVWF 6B 06F8: MOVF 5F,W 06F9: MOVWF 6A 06FA: MOVF 5E,W 06FB: MOVWF 69 06FC: BCF 03.5 06FD: CALL 3A8 06FE: MOVF 79,W 06FF: BSF 03.5 0700: MOVWF 6E 0701: MOVF 78,W 0702: MOVWF 6D 0703: BCF 03.5 0704: CALL 3C9 0705: BSF 03.1 0706: BSF 03.5 0707: MOVF 61,W 0708: BCF 03.5 0709: BSF 03.6 070A: MOVWF 13 070B: BSF 03.5 070C: BCF 03.6 070D: MOVF 60,W 070E: BCF 03.5 070F: BSF 03.6 0710: MOVWF 12 0711: BSF 03.5 0712: BCF 03.6 0713: MOVF 5F,W 0714: BCF 03.5 0715: BSF 03.6 0716: MOVWF 11 0717: BSF 03.5 0718: BCF 03.6 0719: MOVF 5E,W 071A: BCF 03.5 071B: BSF 03.6 071C: MOVWF 10 071D: MOVF 7A,W 071E: MOVWF 17 071F: MOVF 79,W 0720: MOVWF 16 0721: MOVF 78,W 0722: MOVWF 15 0723: MOVF 77,W 0724: MOVWF 14 0725: BCF 03.6 0726: CALL 3E8 0727: MOVF 7A,W 0728: BSF 03.5 0729: MOVWF 61 072A: MOVF 79,W 072B: MOVWF 60 072C: MOVF 78,W 072D: MOVWF 5F 072E: MOVF 77,W 072F: MOVWF 5E .................... .................... if (s) 0730: BTFSS 68.0 0731: GOTO 735 .................... res = -res; 0732: MOVF 63,W 0733: XORLW 80 0734: MOVWF 63 .................... .................... if (y != 0) 0735: MOVF 61,W 0736: MOVWF 6C 0737: MOVF 60,W 0738: MOVWF 6B 0739: MOVF 5F,W 073A: MOVWF 6A 073B: MOVF 5E,W 073C: MOVWF 69 073D: BCF 03.5 073E: BSF 03.6 073F: CLRF 13 0740: CLRF 12 0741: CLRF 11 0742: CLRF 10 0743: BCF 03.6 0744: CALL 353 0745: BTFSC 03.2 0746: GOTO 7A0 .................... { .................... if (s == 1 && n == 0) 0747: BSF 03.5 0748: BTFSS 68.0 0749: GOTO 774 074A: MOVF 5D,F 074B: BTFSS 03.2 074C: GOTO 774 .................... res -= 1.0; 074D: BSF 03.1 074E: MOVF 65,W 074F: BCF 03.5 0750: BSF 03.6 0751: MOVWF 13 0752: BSF 03.5 0753: BCF 03.6 0754: MOVF 64,W 0755: BCF 03.5 0756: BSF 03.6 0757: MOVWF 12 0758: BSF 03.5 0759: BCF 03.6 075A: MOVF 63,W 075B: BCF 03.5 075C: BSF 03.6 075D: MOVWF 11 075E: BSF 03.5 075F: BCF 03.6 0760: MOVF 62,W 0761: BCF 03.5 0762: BSF 03.6 0763: MOVWF 10 0764: CLRF 17 0765: CLRF 16 0766: CLRF 15 0767: MOVLW 7F 0768: MOVWF 14 0769: BCF 03.6 076A: CALL 3E8 076B: MOVF 7A,W 076C: BSF 03.5 076D: MOVWF 65 076E: MOVF 79,W 076F: MOVWF 64 0770: MOVF 78,W 0771: MOVWF 63 0772: MOVF 77,W 0773: MOVWF 62 .................... .................... if (s == 0 && n == 1) 0774: BTFSC 68.0 0775: GOTO 79F 0776: DECFSZ 5D,W 0777: GOTO 79F .................... res += 1.0; 0778: BCF 03.1 0779: MOVF 65,W 077A: BCF 03.5 077B: BSF 03.6 077C: MOVWF 13 077D: BSF 03.5 077E: BCF 03.6 077F: MOVF 64,W 0780: BCF 03.5 0781: BSF 03.6 0782: MOVWF 12 0783: BSF 03.5 0784: BCF 03.6 0785: MOVF 63,W 0786: BCF 03.5 0787: BSF 03.6 0788: MOVWF 11 0789: BSF 03.5 078A: BCF 03.6 078B: MOVF 62,W 078C: BCF 03.5 078D: BSF 03.6 078E: MOVWF 10 078F: CLRF 17 0790: CLRF 16 0791: CLRF 15 0792: MOVLW 7F 0793: MOVWF 14 0794: BCF 03.6 0795: CALL 3E8 0796: MOVF 7A,W 0797: BSF 03.5 0798: MOVWF 65 0799: MOVF 79,W 079A: MOVWF 64 079B: MOVF 78,W 079C: MOVWF 63 079D: MOVF 77,W 079E: MOVWF 62 079F: BCF 03.5 .................... } .................... if (x == 0) 07A0: BSF 03.5 07A1: MOVF 5C,W 07A2: MOVWF 6C 07A3: MOVF 5B,W 07A4: MOVWF 6B 07A5: MOVF 5A,W 07A6: MOVWF 6A 07A7: MOVF 59,W 07A8: MOVWF 69 07A9: BCF 03.5 07AA: BSF 03.6 07AB: CLRF 13 07AC: CLRF 12 07AD: CLRF 11 07AE: CLRF 10 07AF: BCF 03.6 07B0: CALL 353 07B1: BTFSS 03.2 07B2: GOTO 7B9 .................... res = 0; 07B3: BSF 03.5 07B4: CLRF 65 07B5: CLRF 64 07B6: CLRF 63 07B7: CLRF 62 07B8: BCF 03.5 .................... .................... return (res); 07B9: BSF 03.5 07BA: MOVF 62,W 07BB: MOVWF 77 07BC: MOVF 63,W 07BD: MOVWF 78 07BE: MOVF 64,W 07BF: MOVWF 79 07C0: MOVF 65,W 07C1: MOVWF 7A .................... } 07C2: BCF 03.5 07C3: RETLW 00 .................... .................... // Overloaded Functions to take care for new Data types in PCD .................... // Overloaded function CEIL_FLOOR() for data type - Float48 .................... #if defined(__PCD__) .................... float48 CEIL_FLOOR(float48 x, unsigned int8 n) .................... { .................... float48 y, res; .................... unsigned int16 l; .................... int1 s; .................... .................... s = 0; .................... y = x; .................... .................... if (x < 0) .................... { .................... s = 1; .................... y = -y; .................... } .................... .................... if (y <= 32768.0) .................... res = (float48)(unsigned int16)y; .................... .................... else if (y < 10000000.0) .................... { .................... l = (unsigned int16)(y/32768.0); .................... y = 32768.0*(y/32768.0 - (float48)l); .................... res = 32768.0*(float32)l; .................... res += (float48)(unsigned int16)y; .................... } .................... .................... else .................... res = y; .................... .................... y = y - (float48)(unsigned int16)y; .................... .................... if (s) .................... res = -res; .................... .................... if (y != 0) .................... { .................... if (s == 1 && n == 0) .................... res -= 1.0; .................... .................... if (s == 0 && n == 1) .................... res += 1.0; .................... } .................... if (x == 0) .................... res = 0; .................... .................... return (res); .................... } .................... .................... .................... // Overloaded function CEIL_FLOOR() for data type - Float64 .................... float64 CEIL_FLOOR(float64 x, unsigned int8 n) .................... { .................... float64 y, res; .................... unsigned int16 l; .................... int1 s; .................... .................... s = 0; .................... y = x; .................... .................... if (x < 0) .................... { .................... s = 1; .................... y = -y; .................... } .................... .................... if (y <= 32768.0) .................... res = (float64)(unsigned int16)y; .................... .................... else if (y < 10000000.0) .................... { .................... l = (unsigned int16)(y/32768.0); .................... y = 32768.0*(y/32768.0 - (float64)l); .................... res = 32768.0*(float64)l; .................... res += (float64)(unsigned int16)y; .................... } .................... .................... else .................... res = y; .................... .................... y = y - (float64)(unsigned int16)y; .................... .................... if (s) .................... res = -res; .................... .................... if (y != 0) .................... { .................... if (s == 1 && n == 0) .................... res -= 1.0; .................... .................... if (s == 0 && n == 1) .................... res += 1.0; .................... } .................... if (x == 0) .................... res = 0; .................... .................... return (res); .................... } .................... #endif .................... .................... //////////////////////////////////////////////////////////////////////////// .................... // float floor(float x) .................... //////////////////////////////////////////////////////////////////////////// .................... // Description : rounds down the number x. .................... // Date : N/A .................... // .................... float32 floor(float32 x) .................... { .................... return CEIL_FLOOR(x, 0); * 0A05: MOVF 58,W 0A06: MOVWF 5C 0A07: MOVF 57,W 0A08: MOVWF 5B 0A09: MOVF 56,W 0A0A: MOVWF 5A 0A0B: MOVF 55,W 0A0C: MOVWF 59 0A0D: CLRF 5D 0A0E: BCF 0A.3 0A0F: BCF 03.5 0A10: CALL 5A5 0A11: BSF 0A.3 .................... } .................... // Following 2 functions are overloaded functions of floor() for PCD .................... // Overloaded function floor() for data type - Float48 .................... #if defined(__PCD__) .................... float48 floor(float48 x) .................... { .................... return CEIL_FLOOR(x, 0); .................... } .................... .................... // Overloaded function floor() for data type - Float64 .................... float64 floor(float64 x) .................... { .................... return CEIL_FLOOR(x, 0); .................... } .................... #endif .................... .................... .................... //////////////////////////////////////////////////////////////////////////// .................... // float ceil(float x) .................... //////////////////////////////////////////////////////////////////////////// .................... // Description : rounds up the number x. .................... // Date : N/A .................... // .................... float32 ceil(float32 x) .................... { .................... return CEIL_FLOOR(x, 1); * 09ED: MOVF 58,W 09EE: MOVWF 5C 09EF: MOVF 57,W 09F0: MOVWF 5B 09F1: MOVF 56,W 09F2: MOVWF 5A 09F3: MOVF 55,W 09F4: MOVWF 59 09F5: MOVLW 01 09F6: MOVWF 5D 09F7: BCF 0A.3 09F8: BCF 03.5 09F9: CALL 5A5 09FA: BSF 0A.3 .................... } .................... // Following 2 functions are overloaded functions of ceil() for PCD .................... // Overloaded function ceil() for data type - Float48 .................... #if defined(__PCD__) .................... float48 ceil(float48 x) .................... { .................... return CEIL_FLOOR(x, 1); .................... } .................... .................... // Overloaded function ceil() for data type - Float64 .................... float64 ceil(float64 x) .................... { .................... return CEIL_FLOOR(x, 1); .................... } .................... #endif .................... .................... //////////////////////////////////////////////////////////////////////////// .................... // float fabs(float x) .................... //////////////////////////////////////////////////////////////////////////// .................... // Description : Computes the absolute value of floating point number x .................... // Returns : returns the absolute value of x .................... // Date : N/A .................... // .................... #define fabs abs .................... .................... //////////////////////////////////////////////////////////////////////////// .................... // float fmod(float x) .................... //////////////////////////////////////////////////////////////////////////// .................... // Description : Computes the floating point remainder of x/y .................... // Returns : returns the value of x= i*y, for some integer i such that, if y .................... // is non zero, the result has the same isgn of x na dmagnitude less than the .................... // magnitude of y. If y is zero then a domain error occurs. .................... // Date : N/A .................... // .................... .................... float fmod(float32 x,float32 y) .................... { .................... float32 i; .................... if (y!=0.0) .................... { .................... i=(x/y < 0.0)? ceil(x/y): floor(x/y); .................... return(x-(i*y)); .................... } .................... else .................... { .................... #ifdef _ERRNO .................... { .................... errno=EDOM; .................... } .................... #endif .................... } .................... } .................... //Overloaded function for fmod() for PCD .................... // Overloaded function fmod() for data type - Float48 .................... #if defined(__PCD__) .................... float48 fmod(float48 x,float48 y) .................... { .................... float48 i; .................... if (y!=0.0) .................... { .................... i=(x/y < 0.0)? ceil(x/y): floor(x/y); .................... return(x-(i*y)); .................... } .................... else .................... { .................... #ifdef _ERRNO .................... { .................... errno=EDOM; .................... } .................... #endif .................... } .................... } .................... // Overloaded function fmod() for data type - Float64 .................... float64 fmod(float64 x,float64 y) .................... { .................... float64 i; .................... if (y!=0.0) .................... { .................... i=(x/y < 0.0)? ceil(x/y): floor(x/y); .................... return(x-(i*y)); .................... } .................... else .................... { .................... #ifdef _ERRNO .................... { .................... errno=EDOM; .................... } .................... #endif .................... } .................... } .................... #endif .................... //////////////////// Exponential and logarithmic functions //////////////////// .................... //////////////////////////////////////////////////////////////////////////// .................... // float exp(float x) .................... //////////////////////////////////////////////////////////////////////////// .................... // Description : returns the value (e^x) .................... // Date : N/A .................... // .................... #define LN2 0.6931471805599453 .................... .................... float const pe[6] = {0.000207455774, 0.00127100575, 0.00965065093, .................... 0.0554965651, 0.240227138, 0.693147172}; .................... .................... .................... float32 exp(float32 x) .................... { .................... float32 y, res, r; .................... #if defined(__PCD__) .................... int8 data1; .................... #endif .................... signed int8 n; .................... int1 s; .................... #ifdef _ERRNO .................... if(x > 88.722838) .................... { .................... errno=ERANGE; .................... return(0); .................... } .................... #endif .................... n = (signed int16)(x/LN2); .................... s = 0; .................... y = x; .................... .................... if (x < 0) .................... { .................... s = 1; .................... n = -n; .................... y = -y; .................... } .................... .................... res = 0.0; .................... #if !defined(__PCD__) .................... *((unsigned int8 *)(&res)) = n + 0x7F; .................... #endif .................... .................... #if defined(__PCD__) // Takes care of IEEE format for PCD .................... data1 = n+0x7F; .................... if(bit_test(data1,0)) .................... bit_set(*(((unsigned int8 *)(&res)+2)),7); .................... rotate_right(&data1,1); .................... bit_clear(data1,7); .................... *(((unsigned int8 *)(&res)+3)) = data1; .................... #endif .................... .................... y = y/LN2 - (float32)n; .................... .................... r = pe[0]*y + pe[1]; .................... r = r*y + pe[2]; .................... r = r*y + pe[3]; .................... r = r*y + pe[4]; .................... r = r*y + pe[5]; .................... .................... res = res*(1.0 + y*r); .................... .................... if (s) .................... res = 1.0/res; .................... return(res); .................... } .................... .................... .................... //Overloaded function for exp() for PCD .................... // Overloaded function exp() for data type - Float48 .................... #if defined(__PCD__) .................... float48 exp(float48 x) .................... { .................... float48 y, res, r; .................... int8 data1; .................... signed int8 n; .................... int1 s; .................... #ifdef _ERRNO .................... if(x > 88.722838) .................... { .................... errno=ERANGE; .................... return(0); .................... } .................... #endif .................... n = (signed int16)(x/LN2); .................... s = 0; .................... y = x; .................... .................... if (x < 0) .................... { .................... s = 1; .................... n = -n; .................... y = -y; .................... } .................... .................... res = 0.0; .................... .................... data1 = n+0x7F; .................... if(bit_test(data1,0)) .................... bit_set(*(((unsigned int8 *)(&res)+4)),7); .................... rotate_right(&data1,1); .................... bit_clear(data1,7); .................... *(((unsigned int8 *)(&res)+5)) = data1; .................... .................... y = y/LN2 - (float48)n; .................... .................... r = pe[0]*y + pe[1]; .................... r = r*y + pe[2]; .................... r = r*y + pe[3]; .................... r = r*y + pe[4]; .................... r = r*y + pe[5]; .................... .................... res = res*(1.0 + y*r); .................... .................... if (s) .................... res = 1.0/res; .................... return(res); .................... } .................... .................... // Overloaded function exp() for data type - Float64 .................... float64 exp(float64 x) .................... { .................... float64 y, res, r; .................... unsigned int16 data1, data2; .................... unsigned int16 *p; .................... signed int16 n; .................... int1 s; .................... #ifdef _ERRNO .................... if(x > 709.7827128) .................... { .................... errno=ERANGE; .................... return(0); .................... } .................... #endif .................... n = (signed int16)(x/LN2); .................... s = 0; .................... y = x; .................... .................... if (x < 0) .................... { .................... s = 1; .................... n = -n; .................... y = -y; .................... } .................... .................... res = 0.0; .................... .................... #if !defined(__PCD__) .................... *((unsigned int16 *)(&res)) = n + 0x7F; .................... #endif .................... p= (((unsigned int16 *)(&res))+3); .................... data1 = *p; .................... data2 = *p; .................... data1 = n + 0x3FF; .................... data1 = data1 <<4; .................... if(bit_test(data2,15)) .................... bit_set(data1,15); .................... data2 = data2 & 0x000F; .................... data1 ^= data2; .................... .................... *(((unsigned int16 *)(&res)+3)) = data1; .................... .................... .................... y = y/LN2 - (float64)n; .................... .................... r = pe[0]*y + pe[1]; .................... r = r*y + pe[2]; .................... r = r*y + pe[3]; .................... r = r*y + pe[4]; .................... r = r*y + pe[5]; .................... .................... res = res*(1.0 + y*r); .................... .................... if (s) .................... res = 1.0/res; .................... return(res); .................... } .................... .................... #ENDIF .................... .................... .................... /************************************************************/ .................... .................... float32 const pl[4] = {0.45145214, -9.0558803, 26.940971, -19.860189}; .................... float32 const ql[4] = {1.0000000, -8.1354259, 16.780517, -9.9300943}; .................... .................... //////////////////////////////////////////////////////////////////////////// .................... // float log(float x) .................... //////////////////////////////////////////////////////////////////////////// .................... // Description : returns the the natural log of x .................... // Date : N/A .................... // .................... float32 log(float32 x) .................... { .................... float32 y, res, r, y2; .................... #if defined(__PCD__) .................... unsigned int8 data1,data2; .................... #endif .................... signed int8 n; .................... #ifdef _ERRNO .................... if(x <0) .................... { .................... errno=EDOM; .................... } .................... if(x ==0) .................... { .................... errno=ERANGE; .................... return(0); .................... } .................... #endif .................... y = x; .................... .................... if (y != 1.0) .................... { .................... #if !defined(__PCD__) .................... *((unsigned int8 *)(&y)) = 0x7E; .................... #endif .................... .................... #if defined(__PCD__) // Takes care of IEEE format .................... data2 = *(((unsigned int8 *)(&y))+3); .................... *(((unsigned int8 *)(&y))+3) = 0x3F; .................... data1 = *(((unsigned int8 *)(&y))+2); .................... bit_clear(data1,7); .................... *(((unsigned int8 *)(&y))+2) = data1; .................... if(bit_test(data2,7)) .................... bit_set(*(((unsigned int8 *)(&y))+3),7); .................... #endif .................... .................... y = (y - 1.0)/(y + 1.0); .................... .................... y2=y*y; .................... .................... res = pl[0]*y2 + pl[1]; .................... res = res*y2 + pl[2]; .................... res = res*y2 + pl[3]; .................... .................... r = ql[0]*y2 + ql[1]; .................... r = r*y2 + ql[2]; .................... r = r*y2 + ql[3]; .................... .................... res = y*res/r; .................... #if !defined(__PCD__) .................... n = *((unsigned int8 *)(&x)) - 0x7E; .................... #endif .................... #if defined(__PCD__) .................... data1 = *(((unsigned int8 *)(&x)+3)); .................... rotate_left(&data1,1); .................... data2 = *(((unsigned int8 *)(&x)+2)); .................... if(bit_test (data2,7)) .................... bit_set(data1,0); .................... n = data1 - 0x7E; .................... #endif .................... .................... if (n<0) .................... r = -(float32)-n; .................... else .................... r = (float32)n; .................... .................... res += r*LN2; .................... } .................... .................... else .................... res = 0.0; .................... .................... return(res); .................... } .................... .................... //Overloaded function for log() for PCD .................... // Overloaded function log() for data type - Float48 .................... #if defined(__PCD__) .................... float48 log(float48 x) .................... { .................... float48 y, res, r, y2; .................... unsigned int8 data1,data2; .................... signed int8 n; .................... #ifdef _ERRNO .................... if(x <0) .................... { .................... errno=EDOM; .................... } .................... if(x ==0) .................... { .................... errno=ERANGE; .................... return(0); .................... } .................... #endif .................... y = x; .................... .................... if (y != 1.0) .................... { .................... .................... #if !defined(__PCD__) .................... *((unsigned int8 *)(&y)) = 0x7E; .................... #endif .................... data2 = *(((unsigned int8 *)(&y))+5); .................... *(((unsigned int8 *)(&y))+5) = 0x3F; .................... data1 = *(((unsigned int8 *)(&y))+4); .................... bit_clear(data1,7); .................... *(((unsigned int8 *)(&y))+4) = data1; .................... .................... if(bit_test(data2,7)) .................... bit_set(*(((unsigned int8 *)(&y))+4),7); .................... y = (y - 1.0)/(y + 1.0); .................... .................... y2=y*y; .................... .................... res = pl[0]*y2 + pl[1]; .................... res = res*y2 + pl[2]; .................... res = res*y2 + pl[3]; .................... .................... r = ql[0]*y2 + ql[1]; .................... r = r*y2 + ql[2]; .................... r = r*y2 + ql[3]; .................... .................... res = y*res/r; .................... .................... data1 = *(((unsigned int8 *)(&x)+5)); .................... rotate_left(&data1,1); .................... data2 = *(((unsigned int8 *)(&x)+4)); .................... if(bit_test (data2,7)) .................... bit_set(data1,0); .................... .................... n = data1 - 0x7E; .................... .................... if (n<0) .................... r = -(float48)-n; .................... else .................... r = (float48)n; .................... .................... res += r*LN2; .................... } .................... .................... else .................... res = 0.0; .................... .................... return(res); .................... } .................... .................... // Overloaded function log() for data type - Float48 .................... #if defined(__PCD__) .................... float32 const pl_64[4] = {0.45145214, -9.0558803, 26.940971, -19.860189}; .................... float32 const ql_64[4] = {1.0000000, -8.1354259, 16.780517, -9.9300943}; .................... #endif .................... float64 log(float64 x) .................... { .................... float64 y, res, r, y2; .................... unsigned int16 data1,data2; .................... unsigned int16 *p; .................... signed int16 n; .................... #ifdef _ERRNO .................... if(x <0) .................... { .................... errno=EDOM; .................... } .................... if(x ==0) .................... { .................... errno=ERANGE; .................... return(0); .................... } .................... #endif .................... y = x; .................... .................... if (y != 1.0) .................... { .................... #if !defined(__PCD__) .................... *((unsigned int8 *)(&y)) = 0x7E; .................... #endif .................... p= (((unsigned int16 *)(&y))+3); .................... data1 = *p; .................... data2 = *p; .................... data1 = 0x3FE; .................... data1 = data1 <<4; .................... if(bit_test (data2,15)) .................... bit_set(data1,15); .................... data2 = data2 & 0x000F; .................... data1 ^=data2; .................... .................... *p = data1; .................... .................... y = (y - 1.0)/(y + 1.0); .................... .................... y2=y*y; .................... .................... res = pl_64[0]*y2 + pl_64[1]; .................... res = res*y2 + pl_64[2]; .................... res = res*y2 + pl_64[3]; .................... .................... r = ql_64[0]*y2 + ql_64[1]; .................... r = r*y2 + ql_64[2]; .................... r = r*y2 + ql_64[3]; .................... .................... res = y*res/r; .................... .................... p= (((unsigned int16 *)(&x))+3); .................... data1 = *p; .................... bit_clear(data1,15); .................... data1 = data1 >>4; .................... n = data1 - 0x3FE; .................... .................... .................... if (n<0) .................... r = -(float64)-n; .................... else .................... r = (float64)n; .................... .................... res += r*LN2; .................... } .................... .................... else .................... res = 0.0; .................... .................... return(res); .................... } .................... #endif .................... .................... .................... #define LN10 2.3025850929940456 .................... .................... //////////////////////////////////////////////////////////////////////////// .................... // float log10(float x) .................... //////////////////////////////////////////////////////////////////////////// .................... // Description : returns the the log base 10 of x .................... // Date : N/A .................... // .................... float32 log10(float32 x) .................... { .................... float32 r; .................... .................... r = log(x); .................... r = r/LN10; .................... return(r); .................... } .................... .................... //Overloaded functions for log10() for PCD .................... // Overloaded function log10() for data type - Float48 .................... #if defined(__PCD__) .................... float48 log10(float48 x) .................... { .................... float48 r; .................... .................... r = log(x); .................... r = r/LN10; .................... return(r); .................... } .................... .................... // Overloaded function log10() for data type - Float64 .................... float64 log10(float64 x) .................... { .................... float64 r; .................... .................... r = log(x); .................... r = r/LN10; .................... return(r); .................... } .................... #endif .................... //////////////////////////////////////////////////////////////////////////// .................... // float modf(float x) .................... //////////////////////////////////////////////////////////////////////////// .................... // Description :breaks the argument value int integral and fractional parts, .................... // ach of which have the same sign as the argument. It stores the integral part .................... // as a float in the object pointed to by the iptr .................... // Returns : returns the signed fractional part of value. .................... // Date : N/A .................... // .................... .................... float32 modf(float32 value,float32 *iptr) .................... { .................... *iptr=(value < 0.0)? ceil(value): floor(value); * 09CA: MOVF 52,W 09CB: MOVWF 7A 09CC: MOVF 51,W 09CD: MOVWF 53 09CE: MOVF 7A,W 09CF: MOVWF 54 09D0: MOVF 50,W 09D1: MOVWF 6C 09D2: MOVF 4F,W 09D3: MOVWF 6B 09D4: MOVF 4E,W 09D5: MOVWF 6A 09D6: MOVF 4D,W 09D7: MOVWF 69 09D8: BCF 03.5 09D9: BSF 03.6 09DA: CLRF 13 09DB: CLRF 12 09DC: CLRF 11 09DD: CLRF 10 09DE: BCF 0A.3 09DF: BCF 03.6 09E0: CALL 353 09E1: BSF 0A.3 09E2: BTFSS 03.0 09E3: GOTO 1FC 09E4: BSF 03.5 09E5: MOVF 50,W 09E6: MOVWF 58 09E7: MOVF 4F,W 09E8: MOVWF 57 09E9: MOVF 4E,W 09EA: MOVWF 56 09EB: MOVF 4D,W 09EC: MOVWF 55 * 09FB: GOTO 212 09FC: BSF 03.5 09FD: MOVF 50,W 09FE: MOVWF 58 09FF: MOVF 4F,W 0A00: MOVWF 57 0A01: MOVF 4E,W 0A02: MOVWF 56 0A03: MOVF 4D,W 0A04: MOVWF 55 * 0A12: BSF 03.5 0A13: MOVF 53,W 0A14: MOVWF 04 0A15: BCF 03.7 0A16: BTFSC 54.0 0A17: BSF 03.7 0A18: MOVF 77,W 0A19: MOVWF 00 0A1A: INCF 04,F 0A1B: MOVF 78,W 0A1C: MOVWF 00 0A1D: INCF 04,F 0A1E: MOVF 79,W 0A1F: MOVWF 00 0A20: INCF 04,F 0A21: MOVF 7A,W 0A22: MOVWF 00 .................... return(value - *iptr); 0A23: MOVF 52,W 0A24: MOVWF 7A 0A25: MOVF 51,W 0A26: MOVWF 04 0A27: BCF 03.7 0A28: BTFSC 52.0 0A29: BSF 03.7 0A2A: MOVF 00,W 0A2B: MOVWF 77 0A2C: INCF 04,F 0A2D: MOVF 00,W 0A2E: MOVWF 78 0A2F: INCF 04,F 0A30: MOVF 00,W 0A31: MOVWF 79 0A32: INCF 04,F 0A33: MOVF 00,W 0A34: MOVWF 7A 0A35: CLRF 54 0A36: MOVF 04,W 0A37: MOVWF 53 0A38: BCF 54.0 0A39: BTFSC 03.7 0A3A: BSF 54.0 0A3B: BSF 03.1 0A3C: MOVF 50,W 0A3D: BCF 03.5 0A3E: BSF 03.6 0A3F: MOVWF 13 0A40: BSF 03.5 0A41: BCF 03.6 0A42: MOVF 4F,W 0A43: BCF 03.5 0A44: BSF 03.6 0A45: MOVWF 12 0A46: BSF 03.5 0A47: BCF 03.6 0A48: MOVF 4E,W 0A49: BCF 03.5 0A4A: BSF 03.6 0A4B: MOVWF 11 0A4C: BSF 03.5 0A4D: BCF 03.6 0A4E: MOVF 4D,W 0A4F: BCF 03.5 0A50: BSF 03.6 0A51: MOVWF 10 0A52: MOVF 7A,W 0A53: MOVWF 17 0A54: MOVF 79,W 0A55: MOVWF 16 0A56: MOVF 78,W 0A57: MOVWF 15 0A58: MOVF 77,W 0A59: MOVWF 14 0A5A: BCF 0A.3 0A5B: BCF 03.6 0A5C: CALL 3E8 0A5D: BSF 0A.3 0A5E: BSF 03.5 0A5F: MOVF 53,W 0A60: MOVWF 04 0A61: BCF 03.7 0A62: BTFSC 54.0 0A63: BSF 03.7 .................... } .................... //Overloaded functions for modf() for PCD .................... // Overloaded function modf() for data type - Float48 .................... #if defined(__PCD__) .................... float48 modf(float48 value,float48 *iptr) .................... { .................... *iptr=(value < 0.0)? ceil(value): floor(value); .................... return(value - *iptr); .................... } .................... // Overloaded function modf() for data type - Float64 .................... float64 modf(float64 value,float64 *iptr) .................... { .................... *iptr=(value < 0.0)? ceil(value): floor(value); .................... return(value - *iptr); .................... } .................... #endif .................... .................... //////////////////////////////////////////////////////////////////////////// .................... // float pwr(float x,float y) .................... //////////////////////////////////////////////////////////////////////////// .................... // Description : returns the value (x^y) .................... // Date : N/A .................... // .................... float32 pwr(float32 x,float32 y) .................... { .................... if(x>=0) .................... return( exp(y*log(x)) ); .................... else .................... return( -exp(y*log(-x)) ); .................... } .................... //Overloaded functions for pwr() for PCD .................... // Overloaded function pwr() for data type - Float48 .................... #if defined(__PCD__) .................... float48 pwr(float48 x,float48 y) .................... { .................... if(x>=0) .................... return( exp(y*log(x)) ); .................... else .................... return( -exp(y*log(-x)) ); .................... } .................... // Overloaded function pwr() for data type - Float64 .................... float64 pwr(float64 x,float64 y) .................... { .................... if(x>=0) .................... return( exp(y*log(x)) ); .................... else .................... return( -exp(y*log(-x)) ); .................... } .................... #endif .................... .................... //////////////////// Power functions //////////////////// .................... .................... //////////////////////////////////////////////////////////////////////////// .................... // float pow(float x,float y) .................... //////////////////////////////////////////////////////////////////////////// .................... // Description : returns the value (x^y) .................... // Date : N/A .................... // .................... float32 pow(float32 x,float32 y) .................... { .................... if(x>=0) .................... return( exp(y*log(x)) ); .................... else .................... return( -exp(y*log(-x)) ); .................... } .................... //Overloaded functions for pow() for PCD .................... // Overloaded function for pow() data type - Float48 .................... #if defined(__PCD__) .................... float48 pow(float48 x,float48 y) .................... { .................... if(x>=0) .................... return( exp(y*log(x)) ); .................... else .................... return( -exp(y*log(-x)) ); .................... } .................... .................... // Overloaded function pow() for data type - Float64 .................... float64 pow(float64 x,float64 y) .................... { .................... if(x>=0) .................... return( exp(y*log(x)) ); .................... else .................... return( -exp(y*log(-x)) ); .................... } .................... #endif .................... .................... //////////////////////////////////////////////////////////////////////////// .................... // float sqrt(float x) .................... //////////////////////////////////////////////////////////////////////////// .................... // Description : returns the square root of x .................... // Date : N/A .................... // .................... float32 sqrt(float32 x) .................... { .................... float32 y, res; .................... #if defined(__PCD__) .................... unsigned int16 data1,data2; .................... #endif .................... BYTE *p; .................... .................... #ifdef _ERRNO .................... if(x < 0) .................... { .................... errno=EDOM; .................... } .................... #endif .................... .................... if( x<=0.0) .................... return(0.0); .................... .................... y=x; .................... .................... #if !defined(__PCD__) .................... p=&y; .................... (*p)=(BYTE)((((unsigned int16)(*p)) + 127) >> 1); .................... #endif .................... .................... #if defined(__PCD__) .................... p = (((unsigned int8 *)(&y))+3); .................... data1 = *(((unsigned int8 *)(&y))+3); .................... data2 = *(((unsigned int8 *)(&y))+2); .................... rotate_left(&data1,1); .................... if(bit_test(data2,7)) .................... bit_set(data1,0); .................... data1 = ((data1+127) >>1); .................... bit_clear(data2,7); .................... if(bit_test(data1,0)) .................... bit_set(data2,7); .................... data1 = data1 >>1; .................... *(((unsigned int8 *)(&y))+3) = data1; .................... *(((unsigned int8 *)(&y))+2) = data2; .................... .................... #endif .................... .................... do { .................... res=y; .................... y+=(x/y); .................... .................... #if !defined(__PCD__) .................... (*p)--; .................... #endif .................... .................... #if defined(__PCD__) .................... data1 = *(((unsigned int8 *)(&y))+3); .................... data2 = *(((unsigned int8 *)(&y))+2); .................... rotate_left(&data1,1); .................... if(bit_test(data2,7)) .................... bit_set(data1,0); .................... data1--; .................... bit_clear(data2,7); .................... if(bit_test(data1,0)) .................... bit_set(data2,7); .................... data1 = data1 >>1; .................... *(((unsigned int8 *)(&y))+3) = data1; .................... *(((unsigned int8 *)(&y))+2) = data2; .................... .................... #endif .................... } while(res != y); .................... .................... return(res); .................... } .................... //Overloaded functions for sqrt() for PCD .................... // Overloaded function sqrt() for data type - Float48 .................... #if defined(__PCD__) .................... float48 sqrt(float48 x) .................... { .................... float48 y, res; .................... unsigned int16 data1,data2; .................... BYTE *p; .................... .................... #ifdef _ERRNO .................... if(x < 0) .................... { .................... errno=EDOM; .................... } .................... #endif .................... .................... if( x<=0.0) .................... return(0.0); .................... .................... y=x; .................... .................... #if !defined(__PCD__) .................... p=&y; .................... (*p)=(BYTE)((((unsigned int16)(*p)) + 127) >> 1); .................... #endif .................... .................... #if defined(__PCD__) .................... p = (((unsigned int8 *)(&y))+5); .................... data1 = *(((unsigned int8 *)(&y))+5); .................... data2 = *(((unsigned int8 *)(&y))+4); .................... rotate_left(&data1,1); .................... if(bit_test(data2,7)) .................... bit_set(data1,0); .................... data1 = ((data1+127) >>1); .................... bit_clear(data2,7); .................... if(bit_test(data1,0)) .................... bit_set(data2,7); .................... data1 = data1 >>1; .................... *(((unsigned int8 *)(&y))+5) = data1; .................... *(((unsigned int8 *)(&y))+4) = data2; .................... .................... #endif .................... .................... do { .................... res=y; .................... y+=(x/y); .................... .................... #if !defined(__PCD__) .................... (*p)--; .................... #endif .................... .................... data1 = *(((unsigned int8 *)(&y))+5); .................... data2 = *(((unsigned int8 *)(&y))+4); .................... rotate_left(&data1,1); .................... if(bit_test(data2,7)) .................... bit_set(data1,0); .................... data1--; .................... bit_clear(data2,7); .................... if(bit_test(data1,0)) .................... bit_set(data2,7); .................... data1 = data1 >>1; .................... *(((unsigned int8 *)(&y))+5) = data1; .................... *(((unsigned int8 *)(&y))+4) = data2; .................... .................... } while(res != y); .................... .................... return(res); .................... } .................... .................... // Overloaded function sqrt() for data type - Float64 .................... float64 sqrt(float64 x) .................... { .................... float64 y, res; .................... unsigned int16 *p; .................... unsigned int16 temp1,temp2; .................... .................... #ifdef _ERRNO .................... if(x < 0) .................... { .................... errno=EDOM; .................... } .................... #endif .................... .................... if( x<=0.0) .................... return(0.0); .................... .................... y=x; .................... p= (((unsigned int16 *)(&y))+3); .................... temp1 = *p; .................... temp2 = *p; .................... bit_clear(temp1,15); .................... temp1 = (temp1>>4)+1023; .................... temp1 = temp1 >> 1; .................... temp1 = (temp1<<4) & 0xFFF0; .................... if(bit_test(temp2,15)) .................... bit_set(temp1,15); .................... temp2 = temp2 & 0x000F; .................... temp1 ^= temp2; .................... .................... (*p) = temp1; .................... .................... do { .................... res=y; .................... y+=(x/y); .................... temp1 = *p; .................... temp2 = *p; .................... bit_clear(temp1,15); .................... temp1 = (temp1>>4); .................... temp1--; .................... temp1 = (temp1<<4) & 0xFFF0; .................... if(bit_test(temp2,15)) .................... bit_set(temp1,15); .................... temp2 = temp2 & 0x000F; .................... temp1 ^= temp2; .................... (*p) = temp1; .................... .................... } while(res != y); .................... .................... return(res); .................... } .................... #endif .................... .................... ////////////////////////////// Trig Functions ////////////////////////////// .................... #ifdef PI_DIV_BY_TWO .................... #undef PI_DIV_BY_TWO .................... #endif .................... #define PI_DIV_BY_TWO 1.5707963267948966 .................... #ifdef TWOBYPI .................... #undef TWOBYPI .................... #define TWOBYPI 0.6366197723675813 .................... #endif .................... //////////////////////////////////////////////////////////////////////////// .................... // float cos(float x) .................... //////////////////////////////////////////////////////////////////////////// .................... // Description : returns the cosine value of the angle x, which is in radian .................... // Date : 9/20/2001 .................... // .................... float32 cos(float32 x) .................... { .................... float32 y, t, t2 = 1.0; .................... unsigned int8 quad, i; .................... float32 frac; .................... float32 p[4] = { .................... -0.499999993585, .................... 0.041666636258, .................... -0.0013888361399, .................... 0.00002476016134 .................... }; .................... .................... if (x < 0) x = -x; // absolute value of input .................... .................... quad = (unsigned int8)(x / PI_DIV_BY_TWO); // quadrant .................... frac = (x / PI_DIV_BY_TWO) - quad; // fractional part of input .................... quad = quad % 4; // quadrant (0 to 3) .................... .................... if (quad == 0 || quad == 2) .................... t = frac * PI_DIV_BY_TWO; .................... else if (quad == 1) .................... t = (1-frac) * PI_DIV_BY_TWO; .................... else // should be 3 .................... t = (frac-1) * PI_DIV_BY_TWO; .................... .................... y = 0.999999999781; .................... t = t * t; .................... for (i = 0; i <= 3; i++) .................... { .................... t2 = t2 * t; .................... y = y + p[i] * t2; .................... } .................... .................... if (quad == 2 || quad == 1) .................... y = -y; // correct sign .................... .................... return (y); .................... } .................... .................... .................... //Overloaded functions for cos() for PCD .................... // Overloaded function cos() for data type - Float48 .................... #if defined(__PCD__) .................... float48 cos(float48 x) .................... { .................... float48 y, t, t2 = 1.0; .................... unsigned int8 quad, i; .................... float48 frac; .................... float48 p[4] = { .................... -0.499999993585, .................... 0.041666636258, .................... -0.0013888361399, .................... 0.00002476016134 .................... }; .................... .................... if (x < 0) x = -x; // absolute value of input .................... .................... quad = (unsigned int8)(x / PI_DIV_BY_TWO); // quadrant .................... frac = (x / PI_DIV_BY_TWO) - quad; // fractional part of input .................... quad = quad % 4; // quadrant (0 to 3) .................... .................... if (quad == 0 || quad == 2) .................... t = frac * PI_DIV_BY_TWO; .................... else if (quad == 1) .................... t = (1-frac) * PI_DIV_BY_TWO; .................... else // should be 3 .................... t = (frac-1) * PI_DIV_BY_TWO; .................... .................... y = 0.999999999781; .................... t = t * t; .................... for (i = 0; i <= 3; i++) .................... { .................... t2 = t2 * t; .................... y = y + p[i] * t2; .................... } .................... .................... if (quad == 2 || quad == 1) .................... y = -y; // correct sign .................... .................... return (y); .................... } .................... .................... // Overloaded function cos() for data type - Float48 .................... float64 cos(float64 x) .................... { .................... float64 y, t, t2 = 1.0; .................... unsigned int8 quad, i; .................... float64 frac; .................... float64 p[4] = { .................... -0.499999993585, .................... 0.041666636258, .................... -0.0013888361399, .................... 0.00002476016134 .................... }; .................... .................... if (x < 0) x = -x; // absolute value of input .................... .................... quad = (unsigned int8)(x / PI_DIV_BY_TWO); // quadrant .................... frac = (x / PI_DIV_BY_TWO) - quad; // fractional part of input .................... quad = quad % 4; // quadrant (0 to 3) .................... .................... if (quad == 0 || quad == 2) .................... t = frac * PI_DIV_BY_TWO; .................... else if (quad == 1) .................... t = (1-frac) * PI_DIV_BY_TWO; .................... else // should be 3 .................... t = (frac-1) * PI_DIV_BY_TWO; .................... .................... y = 0.999999999781; .................... t = t * t; .................... for (i = 0; i <= 3; i++) .................... { .................... t2 = t2 * t; .................... y = y + p[i] * t2; .................... } .................... .................... if (quad == 2 || quad == 1) .................... y = -y; // correct sign .................... .................... return (y); .................... } .................... .................... #endif .................... .................... //////////////////////////////////////////////////////////////////////////// .................... // float sin(float x) .................... //////////////////////////////////////////////////////////////////////////// .................... // Description : returns the sine value of the angle x, which is in radian .................... // Date : 9/20/2001 .................... // .................... float32 sin(float32 x) .................... { .................... return cos(x - PI_DIV_BY_TWO); .................... } .................... .................... //Overloaded functions for sin() for PCD .................... // Overloaded function sin() for data type - Float48 .................... #if defined(__PCD__) .................... float48 sin(float48 x) .................... { .................... return cos(x - PI_DIV_BY_TWO); .................... } .................... .................... // Overloaded function sin() for data type - Float48 .................... float64 sin(float64 x) .................... { .................... return cos(x - PI_DIV_BY_TWO); .................... } .................... #endif .................... .................... //////////////////////////////////////////////////////////////////////////// .................... // float tan(float x) .................... //////////////////////////////////////////////////////////////////////////// .................... // Description : returns the tangent value of the angle x, which is in radian .................... // Date : 9/20/2001 .................... // .................... float32 tan(float32 x) .................... { .................... float32 c, s; .................... .................... c = cos(x); .................... if (c == 0.0) .................... return (1.0e+36); .................... .................... s = sin(x); .................... return(s/c); .................... } .................... //Overloaded functions for tan() for PCD .................... // Overloaded function tan() for data type - Float48 .................... #if defined(__PCD__) .................... float48 tan(float48 x) .................... { .................... float48 c, s; .................... .................... c = cos(x); .................... if (c == 0.0) .................... return (1.0e+36); .................... .................... s = sin(x); .................... return(s/c); .................... } .................... .................... // Overloaded function tan() for data type - Float48 .................... float64 tan(float64 x) .................... { .................... float64 c, s; .................... .................... c = cos(x); .................... if (c == 0.0) .................... return (1.0e+36); .................... .................... s = sin(x); .................... return(s/c); .................... } .................... #endif .................... .................... float32 const pas[3] = {0.49559947, -4.6145309, 5.6036290}; .................... float32 const qas[3] = {1.0000000, -5.5484666, 5.6036290}; .................... .................... float32 ASIN_COS(float32 x, unsigned int8 n) .................... { .................... float32 y, res, r, y2; .................... int1 s; .................... #ifdef _ERRNO .................... if(x <-1 || x > 1) .................... { .................... errno=EDOM; .................... } .................... #endif .................... s = 0; .................... y = x; .................... .................... if (x < 0) .................... { .................... s = 1; .................... y = -y; .................... } .................... .................... if (y > 0.5) .................... { .................... y = sqrt((1.0 - y)/2.0); .................... n += 2; .................... } .................... .................... y2=y*y; .................... .................... res = pas[0]*y2 + pas[1]; .................... res = res*y2 + pas[2]; .................... .................... r = qas[0]*y2 + qas[1]; .................... r = r*y2 + qas[2]; .................... .................... res = y*res/r; .................... .................... if (n & 2) // |x| > 0.5 .................... res = PI_DIV_BY_TWO - 2.0*res; .................... if (s) .................... res = -res; .................... if (n & 1) // take arccos .................... res = PI_DIV_BY_TWO - res; .................... .................... return(res); .................... } .................... .................... //Overloaded functions for ASIN_COS() for PCD .................... // Overloaded function ASIN_COS() for data type - Float48 .................... #if defined(__PCD__) .................... float48 ASIN_COS(float48 x, unsigned int8 n) .................... { .................... float48 y, res, r, y2; .................... int1 s; .................... #ifdef _ERRNO .................... if(x <-1 || x > 1) .................... { .................... errno=EDOM; .................... } .................... #endif .................... s = 0; .................... y = x; .................... .................... if (x < 0) .................... { .................... s = 1; .................... y = -y; .................... } .................... .................... if (y > 0.5) .................... { .................... y = sqrt((1.0 - y)/2.0); .................... n += 2; .................... } .................... .................... y2=y*y; .................... .................... res = pas[0]*y2 + pas[1]; .................... res = res*y2 + pas[2]; .................... .................... r = qas[0]*y2 + qas[1]; .................... r = r*y2 + qas[2]; .................... .................... res = y*res/r; .................... .................... if (n & 2) // |x| > 0.5 .................... res = PI_DIV_BY_TWO - 2.0*res; .................... if (s) .................... res = -res; .................... if (n & 1) // take arccos .................... res = PI_DIV_BY_TWO - res; .................... .................... return(res); .................... } .................... .................... // Overloaded function ASIN_COS() for data type - Float64 .................... float64 ASIN_COS(float64 x, unsigned int8 n) .................... { .................... float64 y, res, r, y2; .................... int1 s; .................... #ifdef _ERRNO .................... if(x <-1 || x > 1) .................... { .................... errno=EDOM; .................... } .................... #endif .................... s = 0; .................... y = x; .................... .................... if (x < 0) .................... { .................... s = 1; .................... y = -y; .................... } .................... .................... if (y > 0.5) .................... { .................... y = sqrt((1.0 - y)/2.0); .................... n += 2; .................... } .................... .................... y2=y*y; .................... .................... res = pas[0]*y2 + pas[1]; .................... res = res*y2 + pas[2]; .................... .................... r = qas[0]*y2 + qas[1]; .................... r = r*y2 + qas[2]; .................... .................... res = y*res/r; .................... .................... if (n & 2) // |x| > 0.5 .................... res = PI_DIV_BY_TWO - 2.0*res; .................... if (s) .................... res = -res; .................... if (n & 1) // take arccos .................... res = PI_DIV_BY_TWO - res; .................... .................... return(res); .................... } .................... #endif .................... .................... //////////////////////////////////////////////////////////////////////////// .................... // float asin(float x) .................... //////////////////////////////////////////////////////////////////////////// .................... // Description : returns the arcsine value of the value x. .................... // Date : N/A .................... // .................... float32 asin(float32 x) .................... { .................... float32 r; .................... .................... r = ASIN_COS(x, 0); .................... return(r); .................... } .................... //Overloaded functions for asin() for PCD .................... // Overloaded function asin() for data type - Float48 .................... #if defined(__PCD__) .................... float48 asin(float48 x) .................... { .................... float48 r; .................... .................... r = ASIN_COS(x, 0); .................... return(r); .................... } .................... .................... // Overloaded function asin() for data type - Float64 .................... float64 asin(float64 x) .................... { .................... float64 r; .................... .................... r = ASIN_COS(x, 0); .................... return(r); .................... } .................... #endif .................... .................... //////////////////////////////////////////////////////////////////////////// .................... // float acos(float x) .................... //////////////////////////////////////////////////////////////////////////// .................... // Description : returns the arccosine value of the value x. .................... // Date : N/A .................... // .................... float32 acos(float32 x) .................... { .................... float32 r; .................... .................... r = ASIN_COS(x, 1); .................... return(r); .................... } .................... //Overloaded functions for acos() for PCD .................... // Overloaded function acos() for data type - Float48 .................... #if defined(__PCD__) .................... float48 acos(float48 x) .................... { .................... float48 r; .................... .................... r = ASIN_COS(x, 1); .................... return(r); .................... } .................... .................... // Overloaded function acos() for data type - Float64 .................... float64 acos(float64 x) .................... { .................... float64 r; .................... .................... r = ASIN_COS(x, 1); .................... return(r); .................... } .................... #endif .................... .................... float32 const pat[4] = {0.17630401, 5.6710795, 22.376096, 19.818457}; .................... float32 const qat[4] = {1.0000000, 11.368190, 28.982246, 19.818457}; .................... .................... //////////////////////////////////////////////////////////////////////////// .................... // float atan(float x) .................... //////////////////////////////////////////////////////////////////////////// .................... // Description : returns the arctangent value of the value x. .................... // Date : N/A .................... // .................... float32 atan(float32 x) .................... { .................... float32 y, res, r; .................... int1 s, flag; .................... .................... s = 0; .................... flag = 0; .................... y = x; .................... .................... if (x < 0) .................... { .................... s = 1; .................... y = -y; .................... } .................... .................... if (y > 1.0) .................... { .................... y = 1.0/y; .................... flag = 1; .................... } .................... .................... res = pat[0]*y*y + pat[1]; .................... res = res*y*y + pat[2]; .................... res = res*y*y + pat[3]; .................... .................... r = qat[0]*y*y + qat[1]; .................... r = r*y*y + qat[2]; .................... r = r*y*y + qat[3]; .................... .................... res = y*res/r; .................... .................... .................... if (flag) // for |x| > 1 .................... res = PI_DIV_BY_TWO - res; .................... if (s) .................... res = -res; .................... .................... return(res); .................... } .................... //Overloaded functions for atan() for PCD .................... // Overloaded function atan() for data type - Float48 .................... #if defined(__PCD__) .................... float48 atan(float48 x) .................... { .................... float48 y, res, r; .................... int1 s, flag; .................... .................... s = 0; .................... flag = 0; .................... y = x; .................... .................... if (x < 0) .................... { .................... s = 1; .................... y = -y; .................... } .................... .................... if (y > 1.0) .................... { .................... y = 1.0/y; .................... flag = 1; .................... } .................... .................... res = pat[0]*y*y + pat[1]; .................... res = res*y*y + pat[2]; .................... res = res*y*y + pat[3]; .................... .................... r = qat[0]*y*y + qat[1]; .................... r = r*y*y + qat[2]; .................... r = r*y*y + qat[3]; .................... .................... res = y*res/r; .................... .................... .................... if (flag) // for |x| > 1 .................... res = PI_DIV_BY_TWO - res; .................... if (s) .................... res = -res; .................... .................... return(res); .................... } .................... .................... // Overloaded function atan() for data type - Float64 .................... float64 atan(float64 x) .................... { .................... float64 y, res, r; .................... int1 s, flag; .................... .................... s = 0; .................... flag = 0; .................... y = x; .................... .................... if (x < 0) .................... { .................... s = 1; .................... y = -y; .................... } .................... .................... if (y > 1.0) .................... { .................... y = 1.0/y; .................... flag = 1; .................... } .................... .................... res = pat[0]*y*y + pat[1]; .................... res = res*y*y + pat[2]; .................... res = res*y*y + pat[3]; .................... .................... r = qat[0]*y*y + qat[1]; .................... r = r*y*y + qat[2]; .................... r = r*y*y + qat[3]; .................... .................... res = y*res/r; .................... .................... .................... if (flag) // for |x| > 1 .................... res = PI_DIV_BY_TWO - res; .................... if (s) .................... res = -res; .................... .................... return(res); .................... } .................... #endif .................... .................... ///////////////////////////////////////////////////////////////////////////// .................... // float atan2(float y, float x) .................... ///////////////////////////////////////////////////////////////////////////// .................... // Description :computes the principal value of arc tangent of y/x, using the .................... // signs of both the arguments to determine the quadrant of the return value .................... // Returns : returns the arc tangent of y/x. .................... // Date : N/A .................... // .................... .................... float32 atan2(float32 y,float32 x) .................... { .................... float32 z; .................... int1 sign; .................... unsigned int8 quad; .................... sign=0; .................... quad=0; //quadrant .................... quad=((y<=0.0)?((x<=0.0)?3:4):((x<0.0)?2:1)); .................... if(y<0.0) .................... { .................... sign=1; .................... y=-y; .................... } .................... if(x<0.0) .................... { .................... x=-x; .................... } .................... if (x==0.0) .................... { .................... if(y==0.0) .................... { .................... #ifdef _ERRNO .................... { .................... errno=EDOM; .................... } .................... #endif .................... } .................... else .................... { .................... if(sign) .................... { .................... return (-(PI_DIV_BY_TWO)); .................... } .................... else .................... { .................... return (PI_DIV_BY_TWO); .................... } .................... } .................... } .................... else .................... { .................... z=y/x; .................... switch(quad) .................... { .................... case 1: .................... { .................... return atan(z); .................... break; .................... } .................... case 2: .................... { .................... // return (atan(z)+PI_DIV_BY_TWO); //2L3122 .................... return (PI-atan(z)); .................... break; .................... } .................... case 3: .................... { .................... return (atan(z)-PI); .................... break; .................... } .................... case 4: .................... { .................... return (-atan(z)); .................... break; .................... } .................... } .................... } .................... } .................... .................... //Overloaded functions for atan2() for PCD .................... // Overloaded function atan2() for data type - Float48 .................... #if defined(__PCD__) .................... float48 atan2(float48 y,float48 x) .................... { .................... float48 z; .................... int1 sign; .................... unsigned int8 quad; .................... sign=0; .................... quad=0; //quadrant .................... quad=((y<=0.0)?((x<=0.0)?3:4):((x<0.0)?2:1)); .................... if(y<0.0) .................... { .................... sign=1; .................... y=-y; .................... } .................... if(x<0.0) .................... { .................... x=-x; .................... } .................... if (x==0.0) .................... { .................... if(y==0.0) .................... { .................... #ifdef _ERRNO .................... { .................... errno=EDOM; .................... } .................... #endif .................... } .................... else .................... { .................... if(sign) .................... { .................... return (-(PI_DIV_BY_TWO)); .................... } .................... else .................... { .................... return (PI_DIV_BY_TWO); .................... } .................... } .................... } .................... else .................... { .................... z=y/x; .................... switch(quad) .................... { .................... case 1: .................... { .................... return atan(z); .................... break; .................... } .................... case 2: .................... { .................... // return (atan(z)+PI_DIV_BY_TWO); //2L3122 .................... return (PI-atan(z)); .................... break; .................... } .................... case 3: .................... { .................... return (atan(z)-PI); .................... break; .................... } .................... case 4: .................... { .................... return (-atan(z)); .................... break; .................... } .................... } .................... } .................... } .................... .................... // Overloaded function atan2() for data type - Float64 .................... float64 atan2(float64 y,float64 x) .................... { .................... float64 z; .................... int1 sign; .................... unsigned int8 quad; .................... sign=0; .................... quad=0; //quadrant .................... quad=((y<=0.0)?((x<=0.0)?3:4):((x<0.0)?2:1)); .................... if(y<0.0) .................... { .................... sign=1; .................... y=-y; .................... } .................... if(x<0.0) .................... { .................... x=-x; .................... } .................... if (x==0.0) .................... { .................... if(y==0.0) .................... { .................... #ifdef _ERRNO .................... { .................... errno=EDOM; .................... } .................... #endif .................... } .................... else .................... { .................... if(sign) .................... { .................... return (-(PI_DIV_BY_TWO)); .................... } .................... else .................... { .................... return (PI_DIV_BY_TWO); .................... } .................... } .................... } .................... else .................... { .................... z=y/x; .................... switch(quad) .................... { .................... case 1: .................... { .................... return atan(z); .................... break; .................... } .................... case 2: .................... { .................... // return (atan(z)+PI_DIV_BY_TWO); //2L3122 .................... return (PI-atan(z)); .................... break; .................... } .................... case 3: .................... { .................... return (atan(z)-PI); .................... break; .................... } .................... case 4: .................... { .................... return (-atan(z)); .................... break; .................... } .................... } .................... } .................... } .................... #endif .................... .................... //////////////////// Hyperbolic functions //////////////////// .................... .................... //////////////////////////////////////////////////////////////////////////// .................... // float cosh(float x) .................... //////////////////////////////////////////////////////////////////////////// .................... // Description : Computes the hyperbolic cosine value of x .................... // Returns : returns the hyperbolic cosine value of x .................... // Date : N/A .................... // .................... .................... float32 cosh(float32 x) .................... { .................... return ((exp(x)+exp(-x))/2); .................... } .................... //Overloaded functions for cosh() for PCD .................... // Overloaded function cosh() for data type - Float48 .................... #if defined(__PCD__) .................... float48 cosh(float48 x) .................... { .................... return ((exp(x)+exp(-x))/2); .................... } .................... .................... // Overloaded function cosh() for data type - Float64 .................... float64 cosh(float64 x) .................... { .................... return ((exp(x)+exp(-x))/2); .................... } .................... #endif .................... .................... //////////////////////////////////////////////////////////////////////////// .................... // float sinh(float x) .................... //////////////////////////////////////////////////////////////////////////// .................... // Description : Computes the hyperbolic sine value of x .................... // Returns : returns the hyperbolic sine value of x .................... // Date : N/A .................... // .................... .................... float32 sinh(float32 x) .................... { .................... .................... return ((exp(x) - exp(-x))/2); .................... } .................... //Overloaded functions for sinh() for PCD .................... // Overloaded function sinh() for data type - Float48 .................... #if defined(__PCD__) .................... float48 sinh(float48 x) .................... { .................... .................... return ((exp(x) - exp(-x))/2); .................... } .................... .................... // Overloaded function sinh() for data type - Float48 .................... float64 sinh(float64 x) .................... { .................... .................... return ((exp(x) - exp(-x))/2); .................... } .................... #endif .................... .................... //////////////////////////////////////////////////////////////////////////// .................... // float tanh(float x) .................... //////////////////////////////////////////////////////////////////////////// .................... // Description : Computes the hyperbolic tangent value of x .................... // Returns : returns the hyperbolic tangent value of x .................... // Date : N/A .................... // .................... .................... float32 tanh(float32 x) .................... { .................... return(sinh(x)/cosh(x)); .................... } .................... //Overloaded functions for tanh() for PCD .................... // Overloaded function tanh() for data type - Float48 .................... #if defined(__PCD__) .................... float48 tanh(float48 x) .................... { .................... return(sinh(x)/cosh(x)); .................... } .................... .................... // Overloaded function tanh() for data type - Float64 .................... float64 tanh(float64 x) .................... { .................... return(sinh(x)/cosh(x)); .................... } .................... #endif .................... .................... //////////////////////////////////////////////////////////////////////////// .................... // float frexp(float x, signed int *exp) .................... //////////////////////////////////////////////////////////////////////////// .................... // Description : breaks a floating point number into a normalized fraction and an integral .................... // power of 2. It stores the integer in the signed int object pointed to by exp. .................... // Returns : returns the value x, such that x is a double with magnitude in the interval .................... // [1/2,1) or zero, and value equals x times 2 raised to the power *exp.If value is zero, .................... // both parts of the result are zero. .................... // Date : N/A .................... // .................... .................... #define LOG2 .30102999566398119521 .................... float32 frexp(float32 x, signed int8 *exp) .................... { .................... float32 res; .................... int1 sign = 0; .................... if(x == 0.0) .................... { .................... *exp=0; .................... return (0.0); .................... } .................... if(x < 0.0) .................... { .................... x=-x; .................... sign=1; .................... } .................... if (x > 1.0) .................... { .................... *exp=(ceil(log10(x)/LOG2)); .................... res=x/(pow(2, *exp)); .................... if (res == 1) .................... { .................... *exp=*exp+1; .................... res=.5; .................... } .................... } .................... else .................... { .................... if(x < 0.5) .................... { .................... *exp=-1; .................... res=x*2; .................... } .................... else .................... { .................... *exp=0; .................... res=x; .................... } .................... } .................... if(sign) .................... { .................... res=-res; .................... } .................... return res; .................... } .................... .................... //Overloaded functions for frexp() for PCD .................... // Overloaded function frexp() for data type - Float48 .................... #if defined(__PCD__) .................... float48 frexp(float48 x, signed int8 *exp) .................... { .................... float48 res; .................... int1 sign = 0; .................... if(x == 0.0) .................... { .................... *exp=0; .................... return (0.0); .................... } .................... if(x < 0.0) .................... { .................... x=-x; .................... sign=1; .................... } .................... if (x > 1.0) .................... { .................... *exp=(ceil(log10(x)/LOG2)); .................... res=x/(pow(2, *exp)); .................... if (res == 1) .................... { .................... *exp=*exp+1; .................... res=.5; .................... } .................... } .................... else .................... { .................... if(x < 0.5) .................... { .................... *exp=-1; .................... res=x*2; .................... } .................... else .................... { .................... *exp=0; .................... res=x; .................... } .................... } .................... if(sign) .................... { .................... res=-res; .................... } .................... return res; .................... } .................... .................... // Overloaded function frexp() for data type - Float64 .................... float64 frexp(float64 x, signed int8 *exp) .................... { .................... float64 res; .................... int1 sign = 0; .................... if(x == 0.0) .................... { .................... *exp=0; .................... return (0.0); .................... } .................... if(x < 0.0) .................... { .................... x=-x; .................... sign=1; .................... } .................... if (x > 1.0) .................... { .................... *exp=(ceil(log10(x)/LOG2)); .................... res=x/(pow(2, *exp)); .................... if (res == 1) .................... { .................... *exp=*exp+1; .................... res=.5; .................... } .................... } .................... else .................... { .................... if(x < 0.5) .................... { .................... *exp=-1; .................... res=x*2; .................... } .................... else .................... { .................... *exp=0; .................... res=x; .................... } .................... } .................... if(sign) .................... { .................... res=-res; .................... } .................... return res; .................... } .................... #endif .................... .................... ////////////////////////////////////////////////////////////////////////////// .................... // float ldexp(float x, signed int *exp) .................... ////////////////////////////////////////////////////////////////////////////// .................... // Description : multiplies a floating point number by an integral power of 2. .................... // Returns : returns the value of x times 2 raised to the power exp. .................... // Date : N/A .................... // .................... .................... float32 ldexp(float32 value, signed int8 exp) .................... { .................... return (value * pow(2,exp)); .................... } .................... //Overloaded functions for ldexp() for PCD .................... // Overloaded function ldexp() for data type - Float48 .................... .................... #if defined(__PCD__) .................... float48 ldexp(float48 value, signed int8 exp) .................... { .................... return (value * pow(2,exp)); .................... } .................... // Overloaded function ldexp() for data type - Float64 .................... float64 ldexp(float64 value, signed int8 exp) .................... { .................... return (value * pow(2,exp)); .................... } .................... #endif .................... .................... #endif .................... .................... //#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); * 07D7: BSF 03.5 07D8: SWAPF 3E,W 07D9: MOVWF 77 07DA: MOVLW F0 07DB: ANDWF 77,F 07DC: MOVF 77,W 07DD: ANDLW E0 07DE: MOVWF 41 07DF: MOVF 3E,W 07E0: ANDLW 01 07E1: IORWF 41,W 07E2: MOVWF 3F .................... //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 ); 07E3: BTFSC 3E.2 07E4: GOTO 7E9 07E5: BCF 03.5 07E6: BCF 08.6 07E7: GOTO 7EB 07E8: BSF 03.5 07E9: BCF 03.5 07EA: BSF 08.6 .................... output_bit( PIN_D7, data & 0x08 ); 07EB: BSF 03.5 07EC: BTFSC 3E.3 07ED: GOTO 7F2 07EE: BCF 03.5 07EF: BCF 08.7 07F0: GOTO 7F4 07F1: BSF 03.5 07F2: BCF 03.5 07F3: BSF 08.7 .................... output_e( (data & 0x03 ) ); 07F4: BSF 03.5 07F5: MOVF 3E,W 07F6: ANDLW 03 07F7: MOVWF 40 07F8: BCF 03.5 07F9: MOVWF 09 .................... output_c( temp ); 07FA: BSF 03.5 07FB: MOVF 3F,W 07FC: BCF 03.5 07FD: MOVWF 07 .................... //rc7,6,5,0 .................... } 07FE: RETLW 00 .................... .................... void write_stepper_phase( int8 data ) .................... { .................... output_d( (data & 0x0f ) << 2 ) ; //display * 07C4: BSF 03.5 07C5: MOVF 3E,W 07C6: ANDLW 0F 07C7: MOVWF 77 07C8: RLF 77,W 07C9: MOVWF 40 07CA: RLF 40,F 07CB: MOVLW FC 07CC: ANDWF 40,F 07CD: MOVF 40,W 07CE: BCF 03.5 07CF: MOVWF 08 .................... output_a( data & 0x0f ); 07D0: BSF 03.5 07D1: MOVF 3E,W 07D2: ANDLW 0F 07D3: MOVWF 3F 07D4: BCF 03.5 07D5: MOVWF 05 .................... } 07D6: RETLW 00 .................... #define write_encoder_bits( data ) output_e( (data & 0x03 ) ) .................... .................... #ifdef ENABLE_HW_ENCODER .................... #int_RB .................... void RB_isr() .................... { .................... BYTE byTemp; .................... byTemp=(input_b() & 0x30); * 00CB: MOVF 06,W 00CC: ANDLW 30 00CD: BSF 03.6 00CE: MOVWF 29 .................... if (OldPortB != byTemp ) 00CF: MOVF 29,W 00D0: BCF 03.6 00D1: SUBWF 42,W 00D2: BTFSC 03.2 00D3: GOTO 1B1 .................... { .................... OldPortB = byTemp; 00D4: BSF 03.6 00D5: MOVF 29,W 00D6: BCF 03.6 00D7: MOVWF 42 .................... write_encoder_bits( byTemp ); 00D8: BSF 03.6 00D9: MOVF 29,W 00DA: ANDLW 03 00DB: MOVWF 2A 00DC: BCF 03.6 00DD: MOVWF 09 .................... if (RotaryEncoder.A == OldEncoder.A) 00DE: MOVF 06,W 00DF: XORWF 43,W 00E0: ANDLW 10 00E1: BTFSS 03.2 00E2: GOTO 149 .................... { .................... if (RotaryEncoder.B == RotaryEncoder.A) 00E3: CLRF 77 00E4: BTFSC 06.4 00E5: BSF 77.5 00E6: MOVF 06,W 00E7: XORWF 77,W 00E8: ANDLW 20 00E9: BTFSS 03.2 00EA: GOTO 116 .................... { .................... if( encoderCount == 0 ) 00EB: MOVF 44,F 00EC: BTFSS 03.2 00ED: GOTO 10D 00EE: MOVF 45,F 00EF: BTFSS 03.2 00F0: GOTO 10D 00F1: MOVF 46,F 00F2: BTFSS 03.2 00F3: GOTO 10D 00F4: MOVF 47,F 00F5: BTFSS 03.2 00F6: GOTO 10D .................... encoderCount = encoderRollover -1; 00F7: MOVLW 01 00F8: SUBWF 4C,W 00F9: MOVWF 44 00FA: MOVF 4D,W 00FB: MOVWF 45 00FC: MOVLW 00 00FD: BTFSS 03.0 00FE: MOVLW 01 00FF: SUBWF 45,F 0100: MOVF 4E,W 0101: MOVWF 46 0102: MOVLW 00 0103: BTFSS 03.0 0104: MOVLW 01 0105: SUBWF 46,F 0106: MOVF 4F,W 0107: MOVWF 47 0108: MOVLW 00 0109: BTFSS 03.0 010A: MOVLW 01 010B: SUBWF 47,F .................... else 010C: GOTO 115 .................... --encoderCount; 010D: MOVLW FF 010E: ADDWF 44,F 010F: BTFSS 03.0 0110: ADDWF 45,F 0111: BTFSS 03.0 0112: ADDWF 46,F 0113: BTFSS 03.0 0114: ADDWF 47,F .................... } .................... else 0115: GOTO 148 .................... { .................... if( encoderCount == encoderRollover -1 ) 0116: MOVLW 01 0117: SUBWF 4C,W 0118: MOVWF 77 0119: MOVF 4D,W 011A: MOVWF 78 011B: MOVLW 00 011C: BTFSS 03.0 011D: MOVLW 01 011E: SUBWF 78,F 011F: MOVF 4E,W 0120: MOVWF 79 0121: MOVLW 00 0122: BTFSS 03.0 0123: MOVLW 01 0124: SUBWF 79,F 0125: MOVF 4F,W 0126: MOVWF 7A 0127: MOVLW 00 0128: BTFSS 03.0 0129: MOVLW 01 012A: SUBWF 7A,F 012B: MOVF 77,W 012C: SUBWF 44,W 012D: BTFSS 03.2 012E: GOTO 140 012F: MOVF 78,W 0130: SUBWF 45,W 0131: BTFSS 03.2 0132: GOTO 140 0133: MOVF 79,W 0134: SUBWF 46,W 0135: BTFSS 03.2 0136: GOTO 140 0137: MOVF 7A,W 0138: SUBWF 47,W 0139: BTFSS 03.2 013A: GOTO 140 .................... encoderCount = 0; 013B: CLRF 47 013C: CLRF 46 013D: CLRF 45 013E: CLRF 44 .................... else 013F: GOTO 148 .................... ++encoderCount; 0140: MOVLW 01 0141: ADDWF 44,F 0142: BTFSC 03.0 0143: INCF 45,F 0144: BTFSC 03.2 0145: INCF 46,F 0146: BTFSC 03.2 0147: INCF 47,F .................... } .................... } .................... else 0148: GOTO 1B1 .................... { .................... OldEncoder.A = RotaryEncoder.A; 0149: BCF 43.4 014A: BTFSC 06.4 014B: BSF 43.4 .................... if (RotaryEncoder.B == RotaryEncoder.A) 014C: CLRF 77 014D: BTFSC 06.4 014E: BSF 77.5 014F: MOVF 06,W 0150: XORWF 77,W 0151: ANDLW 20 0152: BTFSS 03.2 0153: GOTO 187 .................... { .................... if( encoderCount == encoderRollover -1 ) 0154: MOVLW 01 0155: SUBWF 4C,W 0156: MOVWF 77 0157: MOVF 4D,W 0158: MOVWF 78 0159: MOVLW 00 015A: BTFSS 03.0 015B: MOVLW 01 015C: SUBWF 78,F 015D: MOVF 4E,W 015E: MOVWF 79 015F: MOVLW 00 0160: BTFSS 03.0 0161: MOVLW 01 0162: SUBWF 79,F 0163: MOVF 4F,W 0164: MOVWF 7A 0165: MOVLW 00 0166: BTFSS 03.0 0167: MOVLW 01 0168: SUBWF 7A,F 0169: MOVF 77,W 016A: SUBWF 44,W 016B: BTFSS 03.2 016C: GOTO 17E 016D: MOVF 78,W 016E: SUBWF 45,W 016F: BTFSS 03.2 0170: GOTO 17E 0171: MOVF 79,W 0172: SUBWF 46,W 0173: BTFSS 03.2 0174: GOTO 17E 0175: MOVF 7A,W 0176: SUBWF 47,W 0177: BTFSS 03.2 0178: GOTO 17E .................... encoderCount = 0; 0179: CLRF 47 017A: CLRF 46 017B: CLRF 45 017C: CLRF 44 .................... else 017D: GOTO 186 .................... ++encoderCount; 017E: MOVLW 01 017F: ADDWF 44,F 0180: BTFSC 03.0 0181: INCF 45,F 0182: BTFSC 03.2 0183: INCF 46,F 0184: BTFSC 03.2 0185: INCF 47,F .................... } .................... else 0186: GOTO 1B1 .................... { .................... if( encoderCount == 0 ) 0187: MOVF 44,F 0188: BTFSS 03.2 0189: GOTO 1A9 018A: MOVF 45,F 018B: BTFSS 03.2 018C: GOTO 1A9 018D: MOVF 46,F 018E: BTFSS 03.2 018F: GOTO 1A9 0190: MOVF 47,F 0191: BTFSS 03.2 0192: GOTO 1A9 .................... encoderCount = encoderRollover -1; 0193: MOVLW 01 0194: SUBWF 4C,W 0195: MOVWF 44 0196: MOVF 4D,W 0197: MOVWF 45 0198: MOVLW 00 0199: BTFSS 03.0 019A: MOVLW 01 019B: SUBWF 45,F 019C: MOVF 4E,W 019D: MOVWF 46 019E: MOVLW 00 019F: BTFSS 03.0 01A0: MOVLW 01 01A1: SUBWF 46,F 01A2: MOVF 4F,W 01A3: MOVWF 47 01A4: MOVLW 00 01A5: BTFSS 03.0 01A6: MOVLW 01 01A7: SUBWF 47,F .................... else 01A8: GOTO 1B1 .................... --encoderCount; 01A9: MOVLW FF 01AA: ADDWF 44,F 01AB: BTFSS 03.0 01AC: ADDWF 45,F 01AD: BTFSS 03.0 01AE: ADDWF 46,F 01AF: BTFSS 03.0 01B0: ADDWF 47,F .................... } .................... } .................... } .................... // Handle more PORTB stuff here, like a button, or something. .................... output_bit( PIN_D1, !(input(PIN_D1)) ); 01B1: BTFSS 08.1 01B2: GOTO 1B5 01B3: BCF 08.1 01B4: GOTO 1B6 01B5: BSF 08.1 .................... } .................... #endif .................... .................... #ifdef ENABLE_STEPPER 01B6: BCF 0B.0 01B7: BCF 0A.3 01B8: BCF 0A.4 01B9: GOTO 029 .................... #int_TIMER1 .................... void TIMER1_isr() .................... { .................... static int32 tempphase; * 0E4E: BSF 03.5 0E4F: CLRF 2A 0E50: CLRF 2B 0E51: CLRF 2C 0E52: CLRF 2D .................... .................... //Use this to keep track of step times. TMR1 counts from XXX to 65536 where it rolls over. .................... if (t1LoopCount > 0 ) * 01BA: MOVF 7C,F 01BB: BTFSS 03.2 01BC: GOTO 1C0 01BD: MOVF 7D,F 01BE: BTFSC 03.2 01BF: GOTO 1C5 .................... { .................... t1LoopCount--; 01C0: MOVF 7C,W 01C1: BTFSC 03.2 01C2: DECF 7D,F 01C3: DECF 7C,F .................... } .................... else 01C4: GOTO 21E .................... { .................... tempphase += phaseaccum; 01C5: MOVF 71,W 01C6: BSF 03.5 01C7: ADDWF 2A,F 01C8: MOVF 72,W 01C9: BTFSC 03.0 01CA: INCFSZ 72,W 01CB: ADDWF 2B,F 01CC: MOVF 73,W 01CD: BTFSC 03.0 01CE: INCFSZ 73,W 01CF: ADDWF 2C,F 01D0: MOVF 74,W 01D1: BTFSC 03.0 01D2: INCFSZ 74,W 01D3: ADDWF 2D,F .................... if ( (tempphase & 0xFFFF0000 ) >0 ) 01D4: BCF 03.5 01D5: BSF 03.6 01D6: CLRF 29 01D7: CLRF 2A 01D8: BSF 03.5 01D9: BCF 03.6 01DA: MOVF 2C,W 01DB: BCF 03.5 01DC: BSF 03.6 01DD: MOVWF 2B 01DE: BSF 03.5 01DF: BCF 03.6 01E0: MOVF 2D,W 01E1: BCF 03.5 01E2: BSF 03.6 01E3: MOVWF 2C 01E4: MOVF 29,F 01E5: BTFSS 03.2 01E6: GOTO 1F0 01E7: MOVF 2A,F 01E8: BTFSS 03.2 01E9: GOTO 1F0 01EA: MOVF 2B,F 01EB: BTFSS 03.2 01EC: GOTO 1F0 01ED: MOVF 2C,F 01EE: BTFSC 03.2 01EF: GOTO 214 .................... { .................... tempphase = tempphase - 0xFFFF; 01F0: MOVLW FF 01F1: BSF 03.5 01F2: BCF 03.6 01F3: SUBWF 2A,F 01F4: BTFSC 03.0 01F5: SUBWF 2B,F 01F6: MOVLW 00 01F7: BTFSS 03.0 01F8: MOVLW 01 01F9: SUBWF 2C,F 01FA: MOVLW 00 01FB: BTFSS 03.0 01FC: MOVLW 01 01FD: SUBWF 2D,F .................... set_timer1( t1ResetCount -1 ); 01FE: MOVLW 01 01FF: BCF 03.5 0200: SUBWF 6E,W 0201: BSF 03.6 0202: MOVWF 29 0203: BCF 03.6 0204: MOVF 6F,W 0205: BSF 03.6 0206: MOVWF 2A 0207: MOVLW 00 0208: BTFSS 03.0 0209: MOVLW 01 020A: SUBWF 2A,F 020B: MOVF 2A,W 020C: BCF 03.6 020D: MOVWF 0F 020E: BSF 03.6 020F: MOVF 29,W 0210: BCF 03.6 0211: MOVWF 0E .................... } .................... else 0212: GOTO 219 0213: BSF 03.6 .................... set_timer1( t1ResetCount ); 0214: BCF 03.6 0215: MOVF 6F,W 0216: MOVWF 0F 0217: MOVF 6E,W 0218: MOVWF 0E .................... .................... //restart our counting .................... t1LoopCount = t1ResetLoopCount; 0219: MOVF 76,W 021A: MOVWF 7D 021B: MOVF 75,W 021C: MOVWF 7C .................... .................... //Set TMR1 interrupt flag for main loop processing .................... interruptStatus |= TMR1_TIMEDOUT; 021D: BSF 29.0 .................... } .................... #if defined DEBUG .................... interruptStatus |= TMR1_TIMEDOUT; .................... #endif .................... } .................... #endif .................... 021E: BCF 0C.0 021F: BCF 0A.3 0220: BCF 0A.4 0221: GOTO 029 .................... #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 ) 0222: BSF 03.5 0223: MOVF 24,F 0224: BTFSS 03.2 0225: GOTO 229 0226: MOVF 25,F 0227: BTFSC 03.2 0228: GOTO 22E .................... t2LoopCount--; 0229: MOVF 24,W 022A: BTFSC 03.2 022B: DECF 25,F 022C: DECF 24,F .................... else 022D: GOTO 25B .................... { .................... set_timer2( t2ResetCount); 022E: MOVF 28,W 022F: BCF 03.5 0230: MOVWF 11 .................... t2Loopcount = t2ResetLoopCount; 0231: BSF 03.5 0232: MOVF 27,W 0233: MOVWF 25 0234: MOVF 26,W 0235: MOVWF 24 .................... output_bit(PIN_D0, !input(PIN_D0)); 0236: BCF 03.5 0237: BTFSS 08.0 0238: GOTO 23B 0239: BCF 08.0 023A: GOTO 23C 023B: BSF 08.0 .................... #ifdef ENABLE_HW_ENCODER .................... encoderDelta = encoderCount - encoderOld; 023C: MOVF 54,W 023D: SUBWF 44,W 023E: MOVWF 58 023F: MOVF 45,W 0240: MOVWF 59 0241: MOVF 55,W 0242: BTFSS 03.0 0243: INCFSZ 55,W 0244: SUBWF 59,F 0245: MOVF 46,W 0246: MOVWF 5A 0247: MOVF 56,W 0248: BTFSS 03.0 0249: INCFSZ 56,W 024A: SUBWF 5A,F 024B: MOVF 47,W 024C: MOVWF 5B 024D: MOVF 57,W 024E: BTFSS 03.0 024F: INCFSZ 57,W 0250: SUBWF 5B,F .................... encoderOld = encoderCount; 0251: MOVF 47,W 0252: MOVWF 57 0253: MOVF 46,W 0254: MOVWF 56 0255: MOVF 45,W 0256: MOVWF 55 0257: MOVF 44,W 0258: MOVWF 54 .................... #endif .................... interruptStatus |= TMR2_TIMEDOUT; 0259: BSF 29.1 025A: BSF 03.5 .................... } .................... } .................... .................... /* .................... #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. 025B: BCF 03.5 025C: BCF 0C.1 025D: BCF 0A.3 025E: BCF 0A.4 025F: GOTO 029 .................... 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}; * 0816: BSF 03.5 0817: MOVF 39,W 0818: BCF 03.5 0819: MOVWF 5E 081A: XORLW 00 081B: BTFSC 03.2 081C: GOTO 02A 081D: XORLW 01 081E: BTFSC 03.2 081F: GOTO 02F 0820: XORLW 03 0821: BTFSC 03.2 0822: GOTO 034 0823: XORLW 01 0824: BTFSC 03.2 0825: GOTO 039 0826: XORLW 07 0827: BTFSC 03.2 0828: GOTO 039 0829: GOTO 03E .................... case UNIPOLAR_A: stepTable = fullStepTable1;break; 082A: MOVLW 03 082B: MOVWF 5C 082C: MOVLW 06 082D: MOVWF 5D 082E: GOTO 044 .................... case UNIPOLAR_B: stepTable = fullStepTable2;break; 082F: MOVLW 01 0830: MOVWF 5C 0831: MOVLW 04 0832: MOVWF 5D 0833: GOTO 044 .................... case UNIPOLAR_HALF: stepTable = halfStepTable;break; 0834: MOVLW 01 0835: MOVWF 5C 0836: MOVLW 05 0837: MOVWF 5D 0838: GOTO 044 .................... case BIPOLAR_A: .................... case BIPOLAR_B: stepTable = bipolarStepTable;break; 0839: MOVLW 0A 083A: MOVWF 5C 083B: MOVLW 06 083C: MOVWF 5D 083D: GOTO 044 .................... default:stepTable = halfStepTable;break; 083E: MOVLW 01 083F: MOVWF 5C 0840: MOVLW 05 0841: MOVWF 5D 0842: GOTO 044 .................... break; 0843: GOTO 044 .................... } .................... .................... stepperStatus = status; 0844: BSF 03.5 0845: MOVF 3C,W 0846: BCF 03.5 0847: MOVWF 5F .................... stepperStatusTarget = status; 0848: BSF 03.5 0849: MOVF 3C,W 084A: BCF 03.5 084B: MOVWF 60 .................... stepperStepCount = 0; 084C: CLRF 61 .................... stepperStepCountCurrent = steps; 084D: BSF 03.5 084E: MOVF 3A,W 084F: BCF 03.5 0850: MOVWF 62 .................... stepperStepCountTarget = steps; 0851: BSF 03.5 0852: MOVF 3A,W 0853: BCF 03.5 0854: MOVWF 63 .................... stepperMicrostepCount = 0; 0855: CLRF 64 .................... #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) .................... { 0856: BSF 03.5 0857: MOVF 3B,W 0858: XORLW 02 0859: BCF 03.5 085A: BTFSC 03.2 085B: GOTO 06F 085C: XORLW 06 085D: BTFSC 03.2 085E: GOTO 06F 085F: XORLW 0C 0860: BTFSC 03.2 0861: GOTO 06F 0862: XORLW 18 0863: BTFSC 03.2 0864: GOTO 06F 0865: XORLW 30 0866: BTFSC 03.2 0867: GOTO 06F 0868: XORLW 60 0869: BTFSC 03.2 086A: GOTO 06F 086B: XORLW 40 086C: BTFSC 03.2 086D: GOTO 07B 086E: GOTO 07B .................... case 2: .................... case 4: .................... case 8: .................... case 16: .................... case 32: .................... case 64: .................... stepperMicrostepsPerStep = microsteps/2; 086F: BCF 03.0 0870: BSF 03.5 0871: RRF 3B,W 0872: BCF 03.5 0873: MOVWF 67 .................... stepperMicrostepsPerStepTarget = stepperMicrostepsPerStep; 0874: MOVF 67,W 0875: MOVWF 68 .................... stepperMicrostepCountIncrement = 1;//MAXMICROSTEPSPERSTEP/microsteps; 0876: MOVLW 01 0877: MOVWF 65 .................... stepperMicrostepCountIncrementTarget = stepperMicrostepCountIncrement; 0878: MOVF 65,W 0879: MOVWF 66 .................... break; 087A: GOTO 080 .................... case 0: .................... default: .................... stepperMicrostepsPerStep = 0; 087B: CLRF 67 .................... stepperMicrostepsPerStepTarget = 0; 087C: CLRF 68 .................... stepperMicrostepCountIncrement = 0; 087D: CLRF 65 .................... stepperMicrostepCountIncrementTarget = 0; 087E: CLRF 66 .................... break; 087F: GOTO 080 .................... } .................... .................... //initialise outputs. .................... //update PWM mask control - need to think about which one is changing .................... if ( (stepperStatus & STEPPER_DIRN_MASK ) == STEPPER_DIRN_FORWARD ) 0880: MOVF 5F,W 0881: ANDLW 02 0882: SUBLW 02 0883: BTFSS 03.2 0884: GOTO 0C6 .................... { .................... stepMask = stepTable[stepperStepCount]; 0885: MOVF 61,W 0886: ADDWF 5C,W 0887: MOVWF 04 0888: BCF 03.7 0889: BTFSC 5D.0 088A: BSF 03.7 088B: MOVF 00,W 088C: MOVWF 6C .................... pwmMask = (stepTable[stepperStepCount+1 %STEPSPERPHASE] ^ stepTable[stepperStepCount]); 088D: MOVLW 01 088E: ADDWF 61,W 088F: ADDWF 5C,W 0890: MOVWF 04 0891: BCF 03.7 0892: BTFSC 5D.0 0893: BSF 03.7 0894: MOVF 00,W 0895: BSF 03.5 0896: MOVWF 3F 0897: BCF 03.5 0898: MOVF 61,W 0899: ADDWF 5C,W 089A: MOVWF 04 089B: BCF 03.7 089C: BTFSC 5D.0 089D: BSF 03.7 089E: MOVF 00,W 089F: BSF 03.5 08A0: XORWF 3F,W 08A1: MOVWF 3D .................... pwmMask &= stepTable[stepperStepCount+1 %STEPSPERPHASE]; 08A2: MOVLW 01 08A3: BCF 03.5 08A4: ADDWF 61,W 08A5: ADDWF 5C,W 08A6: MOVWF 04 08A7: BCF 03.7 08A8: BTFSC 5D.0 08A9: BSF 03.7 08AA: MOVF 00,W 08AB: BSF 03.5 08AC: ANDWF 3D,F .................... stepMask = stepTable[stepperStepCount] | stepTable[(stepperStepCount+1)%STEPSPERPHASE]; 08AD: BCF 03.5 08AE: MOVF 61,W 08AF: ADDWF 5C,W 08B0: MOVWF 04 08B1: BCF 03.7 08B2: BTFSC 5D.0 08B3: BSF 03.7 08B4: MOVF 00,W 08B5: BSF 03.5 08B6: MOVWF 3E 08B7: MOVLW 01 08B8: BCF 03.5 08B9: ADDWF 61,W 08BA: ANDLW 07 08BB: ADDWF 5C,W 08BC: MOVWF 04 08BD: BCF 03.7 08BE: BTFSC 5D.0 08BF: BSF 03.7 08C0: MOVF 00,W 08C1: BSF 03.5 08C2: IORWF 3E,W 08C3: BCF 03.5 08C4: MOVWF 6C .................... } .................... else 08C5: GOTO 0FE .................... { .................... pwmMask = (stepTable[(stepperStepCount-1) %STEPSPERPHASE] ^ stepTable[stepperStepCount]); 08C6: MOVLW 01 08C7: SUBWF 61,W 08C8: ANDLW 07 08C9: ADDWF 5C,W 08CA: MOVWF 04 08CB: BCF 03.7 08CC: BTFSC 5D.0 08CD: BSF 03.7 08CE: MOVF 00,W 08CF: BSF 03.5 08D0: MOVWF 40 08D1: BCF 03.5 08D2: MOVF 61,W 08D3: ADDWF 5C,W 08D4: MOVWF 04 08D5: BCF 03.7 08D6: BTFSC 5D.0 08D7: BSF 03.7 08D8: MOVF 00,W 08D9: BSF 03.5 08DA: XORWF 40,W 08DB: MOVWF 3D .................... pwmMask &= stepTable[stepperStepCount]; 08DC: BCF 03.5 08DD: MOVF 61,W 08DE: ADDWF 5C,W 08DF: MOVWF 04 08E0: BCF 03.7 08E1: BTFSC 5D.0 08E2: BSF 03.7 08E3: MOVF 00,W 08E4: BSF 03.5 08E5: ANDWF 3D,F .................... stepMask = stepTable[stepperStepCount] | stepTable[(stepperStepCount-1)%STEPSPERPHASE]; 08E6: BCF 03.5 08E7: MOVF 61,W 08E8: ADDWF 5C,W 08E9: MOVWF 04 08EA: BCF 03.7 08EB: BTFSC 5D.0 08EC: BSF 03.7 08ED: MOVF 00,W 08EE: BSF 03.5 08EF: MOVWF 3E 08F0: MOVLW 01 08F1: BCF 03.5 08F2: SUBWF 61,W 08F3: ANDLW 07 08F4: ADDWF 5C,W 08F5: MOVWF 04 08F6: BCF 03.7 08F7: BTFSC 5D.0 08F8: BSF 03.7 08F9: MOVF 00,W 08FA: BSF 03.5 08FB: IORWF 3E,W 08FC: BCF 03.5 08FD: MOVWF 6C .................... } .................... .................... write_stepper_phase( stepMask ); 08FE: MOVF 6C,W 08FF: BSF 03.5 0900: MOVWF 3E 0901: BCF 0A.3 0902: BCF 03.5 0903: CALL 7C4 0904: BSF 0A.3 .................... write_stepper_mask( pwmMask ); 0905: BSF 03.5 0906: MOVF 3D,W 0907: MOVWF 3E 0908: BCF 0A.3 0909: BCF 03.5 090A: CALL 7D7 090B: BSF 0A.3 .................... setMicrostepPWM( pwmTable[stepperMicrostepCount] ); 090C: BCF 03.0 090D: RLF 64,W 090E: BSF 03.5 090F: MOVWF 3E 0910: INCF 3E,W 0911: BCF 0A.3 0912: BCF 03.5 0913: CALL 045 0914: BSF 0A.3 0915: MOVWF 7A 0916: BSF 03.5 0917: MOVF 3E,W 0918: BCF 0A.3 0919: BCF 03.5 091A: CALL 045 091B: BSF 0A.3 091C: BSF 03.5 091D: MOVWF 3F 091E: MOVF 7A,W 091F: MOVWF 40 0920: MOVWF 42 0921: MOVF 3F,W 0922: MOVWF 41 0923: BCF 03.5 0924: CALL 000 .................... } 0925: RETLW 00 .................... .................... 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); * 0938: BCF 03.5 0939: BCF 07.2 093A: MOVLW 0C 093B: MOVWF 17 .................... //20KHz at 20MHz .................... setup_timer_2(T2_DIV_BY_4,64,16); 093C: MOVLW 78 093D: MOVWF 78 093E: IORLW 05 093F: MOVWF 12 0940: MOVLW 40 0941: BSF 03.5 0942: MOVWF 12 .................... .................... //PWM at zero duty to start .................... set_pwm1_duty( 0x000); 0943: BCF 03.5 0944: CLRF 15 .................... } .................... .................... 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 * 094D: MOVLW 95 094E: MOVWF 44 094F: MOVLW BF 0950: MOVWF 43 0951: MOVLW 56 0952: MOVWF 42 0953: MOVLW 74 0954: MOVWF 41 .................... intbase = (float)(incrementTime/1e6)/Tosc; //scaled clocks per interval 0955: MOVF 3C,W 0956: MOVWF 50 0957: MOVF 3B,W 0958: MOVWF 4F 0959: MOVF 3A,W 095A: MOVWF 4E 095B: MOVF 39,W 095C: MOVWF 4D * 0977: MOVF 7A,W 0978: MOVWF 6C 0979: MOVF 79,W 097A: MOVWF 6B 097B: MOVF 78,W 097C: MOVWF 6A 097D: MOVF 77,W 097E: MOVWF 69 097F: BCF 03.5 0980: BSF 03.6 0981: CLRF 13 0982: MOVLW 24 0983: MOVWF 12 0984: MOVLW 74 0985: MOVWF 11 0986: MOVLW 92 0987: MOVWF 10 0988: BCF 0A.3 0989: BCF 03.6 098A: CALL 260 098B: BSF 0A.3 098C: MOVF 77,W 098D: BSF 03.5 098E: MOVWF 4D 098F: MOVF 78,W 0990: MOVWF 4E 0991: MOVF 79,W 0992: MOVWF 4F 0993: MOVF 7A,W 0994: MOVWF 50 0995: MOVWF 6C 0996: MOVF 4F,W 0997: MOVWF 6B 0998: MOVF 4E,W 0999: MOVWF 6A 099A: MOVF 4D,W 099B: MOVWF 69 099C: MOVF 44,W 099D: BCF 03.5 099E: BSF 03.6 099F: MOVWF 13 09A0: BSF 03.5 09A1: BCF 03.6 09A2: MOVF 43,W 09A3: BCF 03.5 09A4: BSF 03.6 09A5: MOVWF 12 09A6: BSF 03.5 09A7: BCF 03.6 09A8: MOVF 42,W 09A9: BCF 03.5 09AA: BSF 03.6 09AB: MOVWF 11 09AC: BSF 03.5 09AD: BCF 03.6 09AE: MOVF 41,W 09AF: BCF 03.5 09B0: BSF 03.6 09B1: MOVWF 10 09B2: BCF 0A.3 09B3: BCF 03.6 09B4: CALL 260 09B5: BSF 0A.3 09B6: MOVF 7A,W 09B7: BSF 03.5 09B8: MOVWF 40 09B9: MOVF 79,W 09BA: MOVWF 3F 09BB: MOVF 78,W 09BC: MOVWF 3E 09BD: MOVF 77,W 09BE: MOVWF 3D .................... fTemp2 = modf( intbase, &fTemp1); 09BF: MOVF 40,W 09C0: MOVWF 50 09C1: MOVF 3F,W 09C2: MOVWF 4F 09C3: MOVF 3E,W 09C4: MOVWF 4E 09C5: MOVF 3D,W 09C6: MOVWF 4D 09C7: CLRF 52 09C8: MOVLW C5 09C9: MOVWF 51 * 0A64: MOVF 7A,W 0A65: MOVWF 4C 0A66: MOVF 79,W 0A67: MOVWF 4B 0A68: MOVF 78,W 0A69: MOVWF 4A 0A6A: MOVF 77,W 0A6B: MOVWF 49 .................... t2LoopCount = (int16) fTemp1; 0A6C: MOVF 48,W 0A6D: MOVWF 6C 0A6E: MOVF 47,W 0A6F: MOVWF 6B 0A70: MOVF 46,W 0A71: MOVWF 6A 0A72: MOVF 45,W 0A73: MOVWF 69 0A74: BCF 0A.3 0A75: BCF 03.5 0A76: CALL 3A8 0A77: BSF 0A.3 0A78: MOVF 79,W 0A79: BSF 03.5 0A7A: MOVWF 25 0A7B: MOVF 78,W 0A7C: MOVWF 24 .................... t2ResetLoopCount = t2LoopCount; 0A7D: MOVF 25,W 0A7E: MOVWF 27 0A7F: MOVF 24,W 0A80: MOVWF 26 .................... fTemp2 = (fTemp2 * 65536); 0A81: MOVF 4C,W 0A82: BCF 03.5 0A83: BSF 03.6 0A84: MOVWF 13 0A85: BSF 03.5 0A86: BCF 03.6 0A87: MOVF 4B,W 0A88: BCF 03.5 0A89: BSF 03.6 0A8A: MOVWF 12 0A8B: BSF 03.5 0A8C: BCF 03.6 0A8D: MOVF 4A,W 0A8E: BCF 03.5 0A8F: BSF 03.6 0A90: MOVWF 11 0A91: BSF 03.5 0A92: BCF 03.6 0A93: MOVF 49,W 0A94: BCF 03.5 0A95: BSF 03.6 0A96: MOVWF 10 0A97: CLRF 17 0A98: CLRF 16 0A99: CLRF 15 0A9A: MOVLW 8F 0A9B: MOVWF 14 0A9C: BCF 0A.3 0A9D: BCF 03.6 0A9E: CALL 52E 0A9F: BSF 0A.3 0AA0: MOVF 7A,W 0AA1: BSF 03.5 0AA2: MOVWF 4C 0AA3: MOVF 79,W 0AA4: MOVWF 4B 0AA5: MOVF 78,W 0AA6: MOVWF 4A 0AA7: MOVF 77,W 0AA8: MOVWF 49 .................... t2ResetCount = (int16) fTemp2; 0AA9: MOVF 4C,W 0AAA: MOVWF 6C 0AAB: MOVF 4B,W 0AAC: MOVWF 6B 0AAD: MOVF 4A,W 0AAE: MOVWF 6A 0AAF: MOVF 49,W 0AB0: MOVWF 69 0AB1: BCF 0A.3 0AB2: BCF 03.5 0AB3: CALL 3A8 0AB4: BSF 0A.3 0AB5: MOVF 79,W 0AB6: BSF 03.5 0AB7: MOVWF 29 0AB8: MOVF 78,W 0AB9: MOVWF 28 .................... Tosc= 0; 0ABA: CLRF 44 0ABB: CLRF 43 0ABC: CLRF 42 0ABD: CLRF 41 .................... } .................... void setMicrostepPWM( int16 count ) .................... { .................... set_pwm1_duty( count ); * 0800: BSF 03.5 0801: MOVF 42,W 0802: MOVWF 79 0803: MOVF 41,W 0804: MOVWF 78 0805: RRF 79,F 0806: RRF 78,F 0807: RRF 79,F 0808: RRF 78,F 0809: RRF 79,F 080A: MOVF 78,W 080B: BCF 03.5 080C: MOVWF 15 080D: RRF 79,F 080E: RRF 79,W 080F: ANDLW 30 0810: MOVWF 77 0811: MOVF 17,W 0812: ANDLW CF 0813: IORWF 77,W 0814: MOVWF 17 .................... } 0815: RETLW 00 .................... .................... void setup_encoder( int32 rollover, int32 initial, int countsPerStep) .................... { .................... #ifdef ENABLE_HW_ENCODER .................... encoderTarget = initial; * 0ADD: MOVF 40,W 0ADE: BCF 03.5 0ADF: MOVWF 4B 0AE0: BSF 03.5 0AE1: MOVF 3F,W 0AE2: BCF 03.5 0AE3: MOVWF 4A 0AE4: BSF 03.5 0AE5: MOVF 3E,W 0AE6: BCF 03.5 0AE7: MOVWF 49 0AE8: BSF 03.5 0AE9: MOVF 3D,W 0AEA: BCF 03.5 0AEB: MOVWF 48 .................... encoderCount = initial; 0AEC: BSF 03.5 0AED: MOVF 40,W 0AEE: BCF 03.5 0AEF: MOVWF 47 0AF0: BSF 03.5 0AF1: MOVF 3F,W 0AF2: BCF 03.5 0AF3: MOVWF 46 0AF4: BSF 03.5 0AF5: MOVF 3E,W 0AF6: BCF 03.5 0AF7: MOVWF 45 0AF8: BSF 03.5 0AF9: MOVF 3D,W 0AFA: BCF 03.5 0AFB: MOVWF 44 .................... encoderRollover = rollover; 0AFC: BSF 03.5 0AFD: MOVF 3C,W 0AFE: BCF 03.5 0AFF: MOVWF 4F 0B00: BSF 03.5 0B01: MOVF 3B,W 0B02: BCF 03.5 0B03: MOVWF 4E 0B04: BSF 03.5 0B05: MOVF 3A,W 0B06: BCF 03.5 0B07: MOVWF 4D 0B08: BSF 03.5 0B09: MOVF 39,W 0B0A: BCF 03.5 0B0B: MOVWF 4C .................... encoderCountsPerStep = countsPerStep; 0B0C: CLRF 53 0B0D: CLRF 52 0B0E: CLRF 51 0B0F: BSF 03.5 0B10: MOVF 41,W 0B11: BCF 03.5 0B12: MOVWF 50 .................... encoderOld = encoderCount; 0B13: MOVF 47,W 0B14: MOVWF 57 0B15: MOVF 46,W 0B16: MOVWF 56 0B17: MOVF 45,W 0B18: MOVWF 55 0B19: MOVF 44,W 0B1A: MOVWF 54 .................... encoderDelta = 0; 0B1B: CLRF 5B 0B1C: CLRF 5A 0B1D: CLRF 59 0B1E: CLRF 58 .................... #endif .................... } .................... .................... void setupPrescaler( int scale ) .................... { .................... disable_interrupts(INT_TIMER1); * 0B22: BCF 0C.0 .................... if ( t1Prescaler == 0 ) 0B23: BCF 03.5 0B24: MOVF 6D,F 0B25: BTFSS 03.2 0B26: GOTO 32A .................... { .................... setup_TIMER_1( T1_INTERNAL | T1_DIV_BY_1 ); 0B27: MOVLW 85 0B28: MOVWF 10 .................... } .................... else if ( t1Prescaler == 1 ) 0B29: GOTO 338 0B2A: DECFSZ 6D,W 0B2B: GOTO 32F .................... { .................... setup_TIMER_1( T1_INTERNAL | T1_DIV_BY_2 ); 0B2C: MOVLW 95 0B2D: MOVWF 10 .................... } .................... else if ( t1Prescaler == 2 ) 0B2E: GOTO 338 0B2F: MOVF 6D,W 0B30: SUBLW 02 0B31: BTFSS 03.2 0B32: GOTO 336 .................... { .................... setup_TIMER_1( T1_INTERNAL | T1_DIV_BY_4 ); 0B33: MOVLW A5 0B34: MOVWF 10 .................... } .................... else //if ( t1Prescaler == 4 ) 0B35: GOTO 338 .................... { .................... setup_TIMER_1( T1_INTERNAL | T1_DIV_BY_8 ); 0B36: MOVLW B5 0B37: MOVWF 10 .................... } .................... .................... enable_interrupts(INT_TIMER1); 0B38: BSF 03.5 0B39: BSF 0C.0 .................... } .................... .................... void init_config() .................... { .................... int temp = 0; * 0926: BSF 03.5 0927: CLRF 38 .................... //Initialise hardware config .................... set_tris_a(0x00); //All outputs 0928: MOVLW 00 0929: MOVWF 05 .................... port_b_pullups(TRUE); 092A: BCF 01.7 .................... set_tris_b(0b11110000); //RB7-4 inputs 092B: MOVLW F0 092C: MOVWF 06 .................... set_tris_d( 0x00 );//All outputs 092D: MOVLW 00 092E: MOVWF 08 .................... set_tris_c( 0b00011000 ); 092F: MOVLW 18 0930: MOVWF 07 .................... output_d( 0x01 ); 0931: MOVLW 01 0932: BCF 03.5 0933: MOVWF 08 .................... set_tris_e( 0x00 );//All outputs 0934: BSF 03.5 0935: BCF 09.0 0936: BCF 09.1 0937: BCF 09.2 .................... //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 ); * 0945: BSF 03.5 0946: CLRF 3C 0947: MOVLW 01 0948: MOVWF 3B 0949: MOVLW 86 094A: MOVWF 3A 094B: MOVLW A0 094C: MOVWF 39 .................... .................... #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; * 0ABE: MOVLW 04 0ABF: BCF 03.5 0AC0: MOVWF 6D .................... phaseAccum = 89; 0AC1: CLRF 74 0AC2: CLRF 73 0AC3: CLRF 72 0AC4: MOVLW 59 0AC5: MOVWF 71 .................... t1ResetCount = 45241; 0AC6: MOVLW B0 0AC7: MOVWF 6F 0AC8: MOVLW B9 0AC9: MOVWF 6E .................... t1LoopCount = 0; 0ACA: CLRF 7D 0ACB: CLRF 7C .................... t1ResetLoopCount = 32000; 0ACC: MOVLW 7D 0ACD: MOVWF 76 0ACE: CLRF 75 .................... #endif .................... .................... #ifdef ENABLE_HW_ENCODER .................... setup_encoder( 360*4*500, 0L, 1); 0ACF: BSF 03.5 0AD0: CLRF 3C 0AD1: MOVLW 0A 0AD2: MOVWF 3B 0AD3: MOVLW FC 0AD4: MOVWF 3A 0AD5: MOVLW 80 0AD6: MOVWF 39 0AD7: CLRF 40 0AD8: CLRF 3F 0AD9: CLRF 3E 0ADA: CLRF 3D 0ADB: MOVLW 01 0ADC: MOVWF 41 .................... #endif .................... .................... #ifdef ENABLE_STEPPER .................... setupPrescaler( t1Prescaler ); * 0B1F: MOVF 6D,W 0B20: BSF 03.5 0B21: MOVWF 39 .................... //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 ); * 0B3A: CLRF 39 0B3B: MOVLW 30 0B3C: MOVWF 3A 0B3D: CLRF 3B 0B3E: MOVLW 82 0B3F: MOVWF 3C 0B40: BCF 03.5 0B41: CALL 016 .................... //Sidereal rate, includes setting up the TMR1 prescaler. .................... //239.3444 secs per rev .................... //timebase = 239344000; .................... setup_timebase( t1ResetCount, t1LoopCount ); 0B42: MOVF 6F,W 0B43: BSF 03.5 0B44: MOVWF 3A 0B45: BCF 03.5 0B46: MOVF 6E,W 0B47: BSF 03.5 0B48: MOVWF 39 0B49: MOVF 7D,W 0B4A: MOVWF 3C 0B4B: MOVF 7C,W 0B4C: MOVWF 3B .................... #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 ); * 0B5B: BCF 03.5 0B5C: MOVF 6F,W 0B5D: MOVWF 0F 0B5E: MOVF 6E,W 0B5F: MOVWF 0E .................... set_timer2( t2ResetCount); 0B60: BSF 03.5 0B61: MOVF 28,W 0B62: BCF 03.5 0B63: MOVWF 11 .................... .................... #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); 0B64: BSF 0B.3 .................... enable_interrupts(INT_TIMER1); //remove stepper for debug of cmd sequence. 0B65: BSF 03.5 0B66: BSF 0C.0 .................... enable_interrupts(INT_TIMER2); //remove encoder delta timer. 0B67: BSF 0C.1 .................... enable_interrupts(INT_CCP1); 0B68: BSF 0C.2 .................... enable_interrupts(INT_SSP); 0B69: BSF 0C.3 .................... //Finally the global enable .................... enable_interrupts(GLOBAL); 0B6A: MOVLW C0 0B6B: BCF 03.5 0B6C: IORWF 0B,F .................... output_bit(PIN_D0,1); 0B6D: BSF 08.0 .................... } 0B6E: BSF 0A.3 0B6F: BCF 0A.4 0B70: GOTO 65E (RETURN) .................... .................... void setup_timebase( int16 newResetCount, int16 newLoopCount ) .................... { .................... disable_interrupts(INT_TIMER1); * 0B4D: BCF 0C.0 .................... t1ResetLoopCount = newLoopCount; 0B4E: MOVF 3C,W 0B4F: MOVWF 76 0B50: MOVF 3B,W 0B51: MOVWF 75 .................... t1ResetCount = newResetCount; 0B52: MOVF 3A,W 0B53: BCF 03.5 0B54: MOVWF 6F 0B55: BSF 03.5 0B56: MOVF 39,W 0B57: BCF 03.5 0B58: MOVWF 6E .................... enable_interrupts(INT_TIMER1); 0B59: BSF 03.5 0B5A: BSF 0C.0 .................... } .................... .................... void main() .................... { * 0DED: MOVF 03,W 0DEE: ANDLW 1F 0DEF: MOVWF 03 0DF0: MOVLW 50 0DF1: MOVWF 77 0DF2: MOVLW 20 0DF3: MOVWF 04 0DF4: BCF 03.7 0DF5: CLRF 00 0DF6: INCF 04,F 0DF7: DECFSZ 77,F 0DF8: GOTO 5F5 0DF9: CLRF 71 0DFA: CLRF 72 0DFB: CLRF 73 0DFC: CLRF 74 0DFD: CLRF 75 0DFE: CLRF 76 0DFF: CLRF 78 0E00: CLRF 79 0E01: CLRF 7A 0E02: CLRF 7B 0E03: CLRF 7C 0E04: CLRF 7D 0E05: CLRF 7E 0E06: MOVLW 50 0E07: MOVWF 77 0E08: MOVLW A0 0E09: MOVWF 04 0E0A: BCF 03.7 0E0B: CLRF 00 0E0C: INCF 04,F 0E0D: DECFSZ 77,F 0E0E: GOTO 60B 0E0F: MOVLW 60 0E10: MOVWF 77 0E11: MOVLW 10 0E12: MOVWF 04 0E13: BSF 03.7 0E14: CLRF 00 0E15: INCF 04,F 0E16: DECFSZ 77,F 0E17: GOTO 614 0E18: BCF 03.7 0E19: MOVLW 55 0E1A: MOVWF 77 0E1B: MOVLW 90 0E1C: MOVWF 04 0E1D: BSF 03.7 0E1E: CLRF 00 0E1F: INCF 04,F 0E20: DECFSZ 77,F 0E21: GOTO 61E 0E22: BCF 03.7 0E23: BSF 03.5 0E24: BSF 03.6 0E25: CLRF 6E 0E26: BCF 03.7 0E27: BCF 03.5 0E28: BCF 03.6 0E29: CLRF 20 0E2A: CLRF 04 0E2B: BCF 03.7 0E2C: MOVLW 1F 0E2D: ANDWF 03,F 0E2E: BSF 28.3 0E2F: MOVF 28,W 0E30: BSF 03.5 0E31: MOVWF 07 0E32: BCF 03.5 0E33: BSF 28.4 0E34: MOVF 28,W 0E35: BSF 03.5 0E36: MOVWF 07 0E37: MOVLW D8 0E38: MOVWF 13 0E39: MOVLW 36 0E3A: BCF 03.5 0E3B: MOVWF 14 0E3C: BSF 03.5 0E3D: BSF 11.0 0E3E: BSF 1F.0 0E3F: BSF 1F.1 0E40: BSF 1F.2 0E41: BCF 1F.3 0E42: MOVLW 07 0E43: MOVWF 1C .................... int i = 0; * 0E53: CLRF 2E .................... int temp; .................... int16 dwTemp = 0; 0E54: CLRF 30 0E55: CLRF 31 .................... long wTemp = 0l; 0E56: CLRF 32 0E57: CLRF 33 .................... int32 qwTemp = 0; 0E58: CLRF 34 0E59: CLRF 35 0E5A: CLRF 36 0E5B: CLRF 37 .................... .................... init_config(); 0E5C: BCF 03.5 0E5D: GOTO 126 .................... //readEESettings(); .................... .................... //Event based processing .................... While ( TRUE ) .................... { .................... #ifdef ENABLE_STEPPER .................... if ( interruptStatus & TMR1_TIMEDOUT ) 0E5E: BTFSS 29.0 0E5F: GOTO 76E .................... { .................... step_motor(); 0E60: GOTO 3AF .................... .................... //Make any changes requested by the main loop .................... //Do mode-based processing .................... if ( (stepperStatus & STEPPER_MODE_MASK) == STEPPER_MODE_MOTION ) 0E61: MOVF 5F,W 0E62: ANDLW 60 0E63: BTFSS 03.2 0E64: GOTO 6BD .................... { .................... //Enable change flagged so slow to stop or start at slowest rate .................... temp = (stepperStatus ^ stepperStatusTarget) & STEPPER_ENABLE_MASK; 0E65: MOVF 5F,W 0E66: XORWF 60,W 0E67: ANDLW 80 0E68: BSF 03.5 0E69: MOVWF 2F .................... if ( temp != 0 ) 0E6A: MOVF 2F,F 0E6B: BTFSC 03.2 0E6C: GOTO 68F .................... { .................... //Turn on motor .................... if( stepperStatusTarget & STEPPER_ENABLE_MASK ) 0E6D: BCF 03.5 0E6E: BTFSS 60.7 0E6F: GOTO 67F .................... { .................... //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 ); 0E70: BSF 03.5 0E71: CLRF 39 0E72: BCF 03.5 0E73: MOVF 63,W 0E74: BSF 03.5 0E75: MOVWF 3A 0E76: BCF 03.5 0E77: MOVF 68,W 0E78: BSF 03.5 0E79: MOVWF 3B 0E7A: MOVLW 82 0E7B: MOVWF 3C 0E7C: BCF 03.5 0E7D: CALL 016 .................... } .................... else 0E7E: GOTO 68D .................... //slow current rate to stop. .................... //Turn off motor .................... { .................... // if faster than min step rate, slow to stepperMicrostepCountIncrementTarget .................... if( stepperMicrostepCountIncrement > stepperMicrostepCountIncrementTarget ) 0E7F: BTFSC 65.7 0E80: GOTO 689 0E81: MOVF 65,W 0E82: SUBWF 66,W 0E83: BTFSC 03.0 0E84: GOTO 689 .................... { .................... stepperMicrostepCountIncrement >>= 1; 0E85: BCF 03.0 0E86: RRF 65,W 0E87: MOVWF 65 .................... } .................... //Have slowed as far as we can so turn off .................... else 0E88: GOTO 68D .................... { .................... //mix in the new dirn flag to the stepperStatus. .................... stepperStatus &= (!STEPPER_ENABLE_MASK); 0E89: CLRF 5F .................... stepperStatus |= stepperStatusTarget & STEPPER_ENABLE_MASK; 0E8A: MOVF 60,W 0E8B: ANDLW 80 0E8C: IORWF 5F,F .................... } .................... } .................... } .................... //Handle rate adjustments .................... //Direction change flagged so slow to slowest speed to change .................... else if ( (stepperStatus ^ stepperStatusTarget)& STEPPER_DIRN_MASK ) 0E8D: GOTO 6BC 0E8E: BSF 03.5 0E8F: BCF 03.5 0E90: MOVF 5F,W 0E91: XORWF 60,W 0E92: ANDLW 02 0E93: BTFSC 03.2 0E94: GOTO 6A8 .................... { .................... if ( stepperMicrostepCount == 0 ) 0E95: MOVF 64,F 0E96: BTFSS 03.2 0E97: GOTO 6A7 .................... { .................... if( stepperMicrostepCountIncrement > stepperMicrostepCountIncrementTarget ) 0E98: BTFSC 65.7 0E99: GOTO 6A2 0E9A: MOVF 65,W 0E9B: SUBWF 66,W 0E9C: BTFSC 03.0 0E9D: GOTO 6A2 .................... { .................... stepperMicrostepCountIncrement >>= 1; 0E9E: BCF 03.0 0E9F: RRF 65,W 0EA0: MOVWF 65 .................... } .................... else //Have slowed as far as we can 0EA1: GOTO 6A7 .................... { .................... //mix in the new dirn flag to the stepperStatus. .................... stepperStatus ^= STEPPER_DIRN_MASK; 0EA2: MOVLW 02 0EA3: XORWF 5F,F .................... stepperStatus |= (stepperStatusTarget & STEPPER_DIRN_MASK); 0EA4: MOVF 60,W 0EA5: ANDLW 02 0EA6: IORWF 5F,F .................... } .................... } .................... } .................... else if ( stepperMicrostepCountIncrement != stepperMicrostepCountIncrementTarget ) 0EA7: GOTO 6BC 0EA8: MOVF 66,W 0EA9: SUBWF 65,W 0EAA: BTFSC 03.2 0EAB: GOTO 6BC .................... { .................... if ( stepperMicrostepCount == 0 ) 0EAC: MOVF 64,F 0EAD: BTFSS 03.2 0EAE: GOTO 6BC .................... { .................... if( stepperMicrostepCountIncrement > stepperMicrostepCountIncrementTarget ) 0EAF: BTFSC 65.7 0EB0: GOTO 6B9 0EB1: MOVF 65,W 0EB2: SUBWF 66,W 0EB3: BTFSC 03.0 0EB4: GOTO 6B9 .................... { .................... //decrease .................... stepperMicrostepCountIncrement >>= 1; 0EB5: BCF 03.0 0EB6: RRF 65,W 0EB7: MOVWF 65 .................... } .................... else 0EB8: GOTO 6BC .................... { .................... //increase .................... stepperMicrostepCountIncrement <<= 1; 0EB9: BCF 03.0 0EBA: RLF 65,W 0EBB: MOVWF 65 .................... } .................... } .................... } .................... } .................... //This mode requires normal operation to handle stepper stepping. .................... else if ( (stepperStatus & STEPPER_MODE_MASK) == STEPPER_MODE_POSITION_CONSTANT ) 0EBC: GOTO 76A 0EBD: MOVF 5F,W 0EBE: ANDLW 60 0EBF: SUBLW 40 0EC0: BTFSS 03.2 0EC1: GOTO 763 .................... { .................... #ifdef ENABLE_HW_ENCODER .................... qwTemp = (encoderTarget - encoderCount)/encoderCountsPerStep; 0EC2: MOVF 44,W 0EC3: SUBWF 48,W 0EC4: BSF 03.5 0EC5: MOVWF 38 0EC6: BCF 03.5 0EC7: MOVF 49,W 0EC8: BSF 03.5 0EC9: MOVWF 39 0ECA: BCF 03.5 0ECB: MOVF 45,W 0ECC: BTFSS 03.0 0ECD: INCFSZ 45,W 0ECE: GOTO 6D0 0ECF: GOTO 6D3 0ED0: BSF 03.5 0ED1: SUBWF 39,F 0ED2: BCF 03.5 0ED3: MOVF 4A,W 0ED4: BSF 03.5 0ED5: MOVWF 3A 0ED6: BCF 03.5 0ED7: MOVF 46,W 0ED8: BTFSS 03.0 0ED9: INCFSZ 46,W 0EDA: GOTO 6DC 0EDB: GOTO 6DF 0EDC: BSF 03.5 0EDD: SUBWF 3A,F 0EDE: BCF 03.5 0EDF: MOVF 4B,W 0EE0: BSF 03.5 0EE1: MOVWF 3B 0EE2: BCF 03.5 0EE3: MOVF 47,W 0EE4: BTFSS 03.0 0EE5: INCFSZ 47,W 0EE6: GOTO 6E8 0EE7: GOTO 6EB 0EE8: BSF 03.5 0EE9: SUBWF 3B,F 0EEA: BCF 03.5 0EEB: BSF 03.5 0EEC: MOVF 3B,W 0EED: MOVWF 3F 0EEE: MOVF 3A,W 0EEF: MOVWF 3E 0EF0: MOVF 39,W 0EF1: MOVWF 3D 0EF2: MOVF 38,W 0EF3: MOVWF 3C 0EF4: BCF 03.5 0EF5: MOVF 53,W 0EF6: BSF 03.5 0EF7: MOVWF 43 0EF8: BCF 03.5 0EF9: MOVF 52,W 0EFA: BSF 03.5 0EFB: MOVWF 42 0EFC: BCF 03.5 0EFD: MOVF 51,W 0EFE: BSF 03.5 0EFF: MOVWF 41 0F00: BCF 03.5 0F01: MOVF 50,W 0F02: BSF 03.5 0F03: MOVWF 40 0F04: BCF 03.5 0F05: GOTO 5A7 0F06: MOVF 7A,W 0F07: BSF 03.5 0F08: MOVWF 37 0F09: MOVF 79,W 0F0A: MOVWF 36 0F0B: MOVF 78,W 0F0C: MOVWF 35 0F0D: MOVF 77,W 0F0E: MOVWF 34 .................... #else .................... qwTemp = (stepperPositionTarget - stepperPositionCount); .................... #endif .................... temp = abs(qwTemp) >> 8; 0F0F: MOVF 35,W 0F10: MOVWF 2F .................... if ( qwTemp == 0 ) 0F11: MOVF 34,F 0F12: BTFSS 03.2 0F13: GOTO 721 0F14: MOVF 35,F 0F15: BTFSS 03.2 0F16: GOTO 721 0F17: MOVF 36,F 0F18: BTFSS 03.2 0F19: GOTO 721 0F1A: MOVF 37,F 0F1B: BTFSS 03.2 0F1C: GOTO 721 .................... { .................... stepperStatus &= !STEPPER_ENABLE_MASK; 0F1D: BCF 03.5 0F1E: CLRF 5F .................... } .................... else if ( qwTemp > 0 ) 0F1F: GOTO 760 0F20: BSF 03.5 0F21: MOVF 34,F 0F22: BTFSS 03.2 0F23: GOTO 72D 0F24: MOVF 35,F 0F25: BTFSS 03.2 0F26: GOTO 72D 0F27: MOVF 36,F 0F28: BTFSS 03.2 0F29: GOTO 72D 0F2A: MOVF 37,F 0F2B: BTFSC 03.2 0F2C: GOTO 761 .................... { .................... if( temp == 0 ) 0F2D: MOVF 2F,F 0F2E: BTFSS 03.2 0F2F: GOTO 74B .................... { .................... stepperMicrostepCountIncrement = stepperMicrostepCountIncrementTarget; 0F30: BCF 03.5 0F31: MOVF 66,W 0F32: MOVWF 65 .................... stepperMicroStepsPerStep = stepperMicrostepsPerStepTarget; 0F33: MOVF 68,W 0F34: MOVWF 67 .................... stepperStatus &= !STEPPER_DIRN_MASK; 0F35: CLRF 5F .................... stepperStatus |= (qwTemp>0)? STEPPER_DIRN_FORWARD:STEPPER_DIRN_REVERSE; 0F36: BSF 03.5 0F37: MOVF 34,F 0F38: BTFSS 03.2 0F39: GOTO 743 0F3A: MOVF 35,F 0F3B: BTFSS 03.2 0F3C: GOTO 743 0F3D: MOVF 36,F 0F3E: BTFSS 03.2 0F3F: GOTO 743 0F40: MOVF 37,F 0F41: BTFSC 03.2 0F42: GOTO 745 0F43: MOVLW 02 0F44: GOTO 746 0F45: MOVLW 00 0F46: BCF 03.5 0F47: IORWF 5F,F .................... stepperStatus |= STEPPER_ENABLE_MASK; 0F48: BSF 5F.7 .................... } .................... else 0F49: GOTO 760 0F4A: BSF 03.5 .................... { .................... if( stepperMicrostepCountIncrement < stepperMicrostepCountIncrementTarget ) 0F4B: BCF 03.5 0F4C: BTFSC 65.7 0F4D: GOTO 752 0F4E: MOVF 66,W 0F4F: SUBWF 65,W 0F50: BTFSC 03.0 0F51: GOTO 760 .................... stepperMicrostepCountIncrement <<= temp; 0F52: MOVF 65,W 0F53: MOVWF 77 0F54: BSF 03.5 0F55: MOVF 2F,W 0F56: MOVWF 78 0F57: BTFSC 03.2 0F58: GOTO 75D 0F59: BCF 03.0 0F5A: RLF 77,F 0F5B: DECFSZ 78,F 0F5C: GOTO 759 0F5D: MOVF 77,W 0F5E: BCF 03.5 0F5F: MOVWF 65 0F60: BSF 03.5 .................... } .................... } .................... } .................... else if ( (stepperStatus & STEPPER_MODE_MASK) == STEPPER_MODE_POSITION_RAMPED ) 0F61: GOTO 76B 0F62: BCF 03.5 0F63: MOVF 5F,W 0F64: ANDLW 60 0F65: SUBLW 60 0F66: BTFSC 03.2 .................... { .................... } .................... else 0F67: GOTO 76A .................... { .................... //unknown status - major error. .................... reset_cpu(); 0F68: CLRF 0A 0F69: GOTO 000 0F6A: BSF 03.5 .................... } .................... //Clear flag .................... interruptStatus ^= TMR1_TIMEDOUT; 0F6B: MOVLW 01 0F6C: BCF 03.5 0F6D: XORWF 29,F .................... } .................... #endif //ENABLE_STEPPER .................... if ( interruptStatus & TMR2_TIMEDOUT ) 0F6E: BTFSS 29.1 0F6F: GOTO 7A9 .................... { .................... //Log the error from where we expect to be and change movement rate as required. .................... #ifdef ENABLE_HW_ENCODER .................... if ( (encoderTarget - encoderCount) > 0 ) 0F70: MOVF 44,W 0F71: SUBWF 48,W 0F72: BSF 03.5 0F73: MOVWF 38 0F74: BCF 03.5 0F75: MOVF 49,W 0F76: BSF 03.5 0F77: MOVWF 39 0F78: BCF 03.5 0F79: MOVF 45,W 0F7A: BTFSS 03.0 0F7B: INCFSZ 45,W 0F7C: GOTO 77E 0F7D: GOTO 781 0F7E: BSF 03.5 0F7F: SUBWF 39,F 0F80: BCF 03.5 0F81: MOVF 4A,W 0F82: BSF 03.5 0F83: MOVWF 3A 0F84: BCF 03.5 0F85: MOVF 46,W 0F86: BTFSS 03.0 0F87: INCFSZ 46,W 0F88: GOTO 78A 0F89: GOTO 78D 0F8A: BSF 03.5 0F8B: SUBWF 3A,F 0F8C: BCF 03.5 0F8D: MOVF 4B,W 0F8E: BSF 03.5 0F8F: MOVWF 3B 0F90: BCF 03.5 0F91: MOVF 47,W 0F92: BTFSS 03.0 0F93: INCFSZ 47,W 0F94: GOTO 796 0F95: GOTO 799 0F96: BSF 03.5 0F97: SUBWF 3B,F 0F98: BCF 03.5 0F99: BSF 03.5 0F9A: MOVF 38,F 0F9B: BTFSS 03.2 0F9C: GOTO 7A5 0F9D: MOVF 39,F 0F9E: BTFSS 03.2 0F9F: GOTO 7A5 0FA0: MOVF 3A,F 0FA1: BTFSS 03.2 0FA2: GOTO 7A5 0FA3: MOVF 3B,F 0FA4: BTFSS 03.2 .................... { .................... //increase speed .................... } .................... else 0FA5: GOTO 7A6 .................... { .................... //decrease speed. .................... } .................... #else .................... if ( ( stepperPositionTarget - stepperPositionCount ) > 0 ) .................... { .................... } .................... else .................... { .................... } .................... #endif .................... interruptStatus ^= TMR2_TIMEDOUT; 0FA6: MOVLW 02 0FA7: BCF 03.5 0FA8: XORWF 29,F .................... } .................... .................... } 0FA9: GOTO 65E .................... } .................... .................... void step_motor() .................... { 0FAA: GOTO 7AA .................... int index; .................... if( stepperStatus & STEPPER_ENABLE_MASK ) * 0BAF: BTFSS 5F.7 0BB0: GOTO 5A4 .................... { .................... //Set the next step regardless - handle longer issues later .................... stepperMicrostepCount += stepperMicrostepCountIncrement; 0BB1: MOVF 65,W 0BB2: ADDWF 64,W 0BB3: MOVWF 64 .................... /* 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 ) 0BB4: MOVF 64,F 0BB5: BTFSC 03.2 0BB6: GOTO 3BB 0BB7: MOVF 67,W 0BB8: SUBWF 64,W 0BB9: BTFSS 03.0 0BBA: GOTO 572 .................... { .................... //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 0BBB: MOVF 64,F 0BBC: BTFSC 03.2 0BBD: GOTO 3C2 0BBE: MOVF 68,W 0BBF: SUBLW 01 0BC0: BTFSS 03.0 0BC1: GOTO 4AE .................... { .................... stepperMicroStepCount = 0; 0BC2: CLRF 64 .................... stepperMicrostepCountIncrement = stepperMicrostepCountIncrementTarget; 0BC3: MOVF 66,W 0BC4: MOVWF 65 .................... if( (stepperStatus & STEPPER_DIRN_MASK ) == STEPPER_DIRN_FORWARD ) 0BC5: MOVF 5F,W 0BC6: ANDLW 02 0BC7: SUBLW 02 0BC8: BTFSS 03.2 0BC9: GOTO 43C .................... { .................... stepperStepCount++; 0BCA: INCF 61,F .................... stepperStepCount %= STEPSPERPHASE; 0BCB: MOVLW 07 0BCC: ANDWF 61,F .................... #ifdef ENABLE_STEPPER_CTR .................... stepperPositionCount++; .................... #endif .................... //output new step phase .................... stepMask = stepTable[stepperStepCount]; 0BCD: MOVF 61,W 0BCE: ADDWF 5C,W 0BCF: MOVWF 04 0BD0: BCF 03.7 0BD1: BTFSC 5D.0 0BD2: BSF 03.7 0BD3: MOVF 00,W 0BD4: MOVWF 6C .................... stepMask |= stepTable[ (stepperStepCount+1)%STEPSPERPHASE]; 0BD5: MOVLW 01 0BD6: ADDWF 61,W 0BD7: ANDLW 07 0BD8: ADDWF 5C,W 0BD9: MOVWF 04 0BDA: BCF 03.7 0BDB: BTFSC 5D.0 0BDC: BSF 03.7 0BDD: MOVF 00,W 0BDE: IORWF 6C,F .................... write_stepper_phase( stepMask ); 0BDF: MOVF 6C,W 0BE0: BSF 03.5 0BE1: MOVWF 3E 0BE2: BCF 0A.3 0BE3: BCF 03.5 0BE4: CALL 7C4 0BE5: BSF 0A.3 .................... //update PWM mask control - need to think about which one is changing .................... pwmMask = stepTable[((stepperStepCount+1) %STEPSPERPHASE)] ; 0BE6: MOVLW 01 0BE7: ADDWF 61,W 0BE8: ANDLW 07 0BE9: ADDWF 5C,W 0BEA: MOVWF 04 0BEB: BCF 03.7 0BEC: BTFSC 5D.0 0BED: BSF 03.7 0BEE: MOVF 00,W 0BEF: MOVWF 69 .................... pwmMask ^= stepTable[stepperStepCount]; 0BF0: MOVF 61,W 0BF1: ADDWF 5C,W 0BF2: MOVWF 04 0BF3: BCF 03.7 0BF4: BTFSC 5D.0 0BF5: BSF 03.7 0BF6: MOVF 00,W 0BF7: XORWF 69,F .................... pwmMask &= stepTable[((stepperStepCount+1) %STEPSPERPHASE)]; 0BF8: MOVLW 01 0BF9: ADDWF 61,W 0BFA: ANDLW 07 0BFB: ADDWF 5C,W 0BFC: MOVWF 04 0BFD: BCF 03.7 0BFE: BTFSC 5D.0 0BFF: BSF 03.7 0C00: MOVF 00,W 0C01: ANDWF 69,F .................... write_stepper_mask( pwmMask ); 0C02: MOVF 69,W 0C03: BSF 03.5 0C04: MOVWF 3E 0C05: BCF 0A.3 0C06: BCF 03.5 0C07: CALL 7D7 0C08: BSF 0A.3 .................... //update PWM value .................... index = ABS(MAXMICROSTEPSPERSTEP/stepperMicrostepsPerStep) * stepperMicrostepCount; 0C09: MOVLW 40 0C0A: BSF 03.5 0C0B: MOVWF 39 0C0C: BCF 03.5 0C0D: MOVF 67,W 0C0E: BSF 03.5 0C0F: MOVWF 3A 0C10: BCF 03.5 0C11: CALL 371 0C12: MOVF 78,W 0C13: BSF 03.5 0C14: MOVWF 39 0C15: MOVF 39,W 0C16: MOVWF 3A 0C17: BCF 03.5 0C18: MOVF 64,W 0C19: BSF 03.5 0C1A: MOVWF 3B 0C1B: BCF 03.5 0C1C: CALL 388 0C1D: MOVF 78,W 0C1E: BSF 03.5 0C1F: MOVWF 38 .................... pwmVal = pwmTable[index]; 0C20: BCF 03.0 0C21: RLF 38,W 0C22: MOVWF 39 0C23: INCF 39,W 0C24: BCF 0A.3 0C25: BCF 03.5 0C26: CALL 045 0C27: BSF 0A.3 0C28: MOVWF 7A 0C29: BSF 03.5 0C2A: MOVF 39,W 0C2B: BCF 0A.3 0C2C: BCF 03.5 0C2D: CALL 045 0C2E: BSF 0A.3 0C2F: MOVWF 6A 0C30: MOVF 7A,W 0C31: MOVWF 6B .................... setMicrostepPWM( pwmVal ); 0C32: MOVF 6B,W 0C33: BSF 03.5 0C34: MOVWF 42 0C35: BCF 03.5 0C36: MOVF 6A,W 0C37: BSF 03.5 0C38: MOVWF 41 0C39: BCF 03.5 0C3A: CALL 000 .................... } .................... else //Reverse 0C3B: GOTO 4AD .................... { .................... stepperStepCount--; 0C3C: DECF 61,F .................... stepperStepCount %= STEPSPERPHASE; 0C3D: MOVLW 07 0C3E: ANDWF 61,F .................... #if defined ENABLE_STEPPER_CTR .................... stepperPositionCount--; .................... #endif .................... //output new step phase .................... stepMask = stepTable[stepperStepCount]; 0C3F: MOVF 61,W 0C40: ADDWF 5C,W 0C41: MOVWF 04 0C42: BCF 03.7 0C43: BTFSC 5D.0 0C44: BSF 03.7 0C45: MOVF 00,W 0C46: MOVWF 6C .................... stepMask |= stepTable[(stepperStepCount-1)%STEPSPERPHASE]; 0C47: MOVLW 01 0C48: SUBWF 61,W 0C49: ANDLW 07 0C4A: ADDWF 5C,W 0C4B: MOVWF 04 0C4C: BCF 03.7 0C4D: BTFSC 5D.0 0C4E: BSF 03.7 0C4F: MOVF 00,W 0C50: IORWF 6C,F .................... write_stepper_phase( stepMask ); 0C51: MOVF 6C,W 0C52: BSF 03.5 0C53: MOVWF 3E 0C54: BCF 0A.3 0C55: BCF 03.5 0C56: CALL 7C4 0C57: BSF 0A.3 .................... //update PWM mask control - need to think about which one is changing .................... pwmMask = stepTable[((stepperStepCount-1) %STEPSPERPHASE)]; 0C58: MOVLW 01 0C59: SUBWF 61,W 0C5A: ANDLW 07 0C5B: ADDWF 5C,W 0C5C: MOVWF 04 0C5D: BCF 03.7 0C5E: BTFSC 5D.0 0C5F: BSF 03.7 0C60: MOVF 00,W 0C61: MOVWF 69 .................... pwmMask ^= stepTable[stepperStepCount]; 0C62: MOVF 61,W 0C63: ADDWF 5C,W 0C64: MOVWF 04 0C65: BCF 03.7 0C66: BTFSC 5D.0 0C67: BSF 03.7 0C68: MOVF 00,W 0C69: XORWF 69,F .................... pwmMask &= stepTable[((stepperStepCount-1) %STEPSPERPHASE)]; 0C6A: MOVLW 01 0C6B: SUBWF 61,W 0C6C: ANDLW 07 0C6D: ADDWF 5C,W 0C6E: MOVWF 04 0C6F: BCF 03.7 0C70: BTFSC 5D.0 0C71: BSF 03.7 0C72: MOVF 00,W 0C73: ANDWF 69,F .................... write_stepper_mask( pwmMask ); 0C74: MOVF 69,W 0C75: BSF 03.5 0C76: MOVWF 3E 0C77: BCF 0A.3 0C78: BCF 03.5 0C79: CALL 7D7 0C7A: BSF 0A.3 .................... //update PWM value .................... index = ABS(MAXMICROSTEPSPERSTEP/stepperMicrostepsPerStep) * stepperMicrostepCount; 0C7B: MOVLW 40 0C7C: BSF 03.5 0C7D: MOVWF 39 0C7E: BCF 03.5 0C7F: MOVF 67,W 0C80: BSF 03.5 0C81: MOVWF 3A 0C82: BCF 03.5 0C83: CALL 371 0C84: MOVF 78,W 0C85: BSF 03.5 0C86: MOVWF 39 0C87: MOVF 39,W 0C88: MOVWF 3A 0C89: BCF 03.5 0C8A: MOVF 64,W 0C8B: BSF 03.5 0C8C: MOVWF 3B 0C8D: BCF 03.5 0C8E: CALL 388 0C8F: MOVF 78,W 0C90: BSF 03.5 0C91: MOVWF 38 .................... pwmVal = pwmTable[index]; 0C92: BCF 03.0 0C93: RLF 38,W 0C94: MOVWF 39 0C95: INCF 39,W 0C96: BCF 0A.3 0C97: BCF 03.5 0C98: CALL 045 0C99: BSF 0A.3 0C9A: MOVWF 7A 0C9B: BSF 03.5 0C9C: MOVF 39,W 0C9D: BCF 0A.3 0C9E: BCF 03.5 0C9F: CALL 045 0CA0: BSF 0A.3 0CA1: MOVWF 6A 0CA2: MOVF 7A,W 0CA3: MOVWF 6B .................... setMicrostepPWM( pwmVal ); 0CA4: MOVF 6B,W 0CA5: BSF 03.5 0CA6: MOVWF 42 0CA7: BCF 03.5 0CA8: MOVF 6A,W 0CA9: BSF 03.5 0CAA: MOVWF 41 0CAB: BCF 03.5 0CAC: CALL 000 .................... } .................... } .................... //Half step point .................... else // if ( stepperMicroStepCount >= MAXMICROSTEPSPERSTEP ) 0CAD: GOTO 571 .................... { .................... stepperMicrostepCountIncrement = -1 * stepperMicrostepCountIncrement; 0CAE: MOVLW FF 0CAF: BSF 03.5 0CB0: MOVWF 3A 0CB1: BCF 03.5 0CB2: MOVF 65,W 0CB3: BSF 03.5 0CB4: MOVWF 3B 0CB5: BCF 03.5 0CB6: CALL 388 0CB7: MOVF 78,W 0CB8: MOVWF 65 .................... stepperMicroStepCount = stepperMicroStepsPerStep; 0CB9: MOVF 67,W 0CBA: MOVWF 64 .................... if( (stepperStatus & STEPPER_DIRN_MASK) == STEPPER_DIRN_FORWARD ) 0CBB: MOVF 5F,W 0CBC: ANDLW 02 0CBD: SUBLW 02 0CBE: BTFSS 03.2 0CBF: GOTO 519 .................... { .................... //update PWM mask control - need to think about which one is changing .................... pwmMask = (stepTable[(stepperStepCount+1)%STEPSPERPHASE] ^ stepTable[stepperStepCount]); 0CC0: MOVLW 01 0CC1: ADDWF 61,W 0CC2: ANDLW 07 0CC3: ADDWF 5C,W 0CC4: MOVWF 04 0CC5: BCF 03.7 0CC6: BTFSC 5D.0 0CC7: BSF 03.7 0CC8: MOVF 00,W 0CC9: BSF 03.5 0CCA: MOVWF 3B 0CCB: BCF 03.5 0CCC: MOVF 61,W 0CCD: ADDWF 5C,W 0CCE: MOVWF 04 0CCF: BCF 03.7 0CD0: BTFSC 5D.0 0CD1: BSF 03.7 0CD2: MOVF 00,W 0CD3: BSF 03.5 0CD4: XORWF 3B,W 0CD5: BCF 03.5 0CD6: MOVWF 69 .................... pwmMask &= stepTable[stepperStepCount]; 0CD7: MOVF 61,W 0CD8: ADDWF 5C,W 0CD9: MOVWF 04 0CDA: BCF 03.7 0CDB: BTFSC 5D.0 0CDC: BSF 03.7 0CDD: MOVF 00,W 0CDE: ANDWF 69,F .................... write_stepper_mask( pwmMask ); 0CDF: MOVF 69,W 0CE0: BSF 03.5 0CE1: MOVWF 3E 0CE2: BCF 0A.3 0CE3: BCF 03.5 0CE4: CALL 7D7 0CE5: BSF 0A.3 .................... //update PWM value .................... index = ABS(MAXMICROSTEPSPERSTEP/stepperMicrostepsPerStep) * stepperMicrostepCount; 0CE6: MOVLW 40 0CE7: BSF 03.5 0CE8: MOVWF 39 0CE9: BCF 03.5 0CEA: MOVF 67,W 0CEB: BSF 03.5 0CEC: MOVWF 3A 0CED: BCF 03.5 0CEE: CALL 371 0CEF: MOVF 78,W 0CF0: BSF 03.5 0CF1: MOVWF 39 0CF2: MOVF 39,W 0CF3: MOVWF 3A 0CF4: BCF 03.5 0CF5: MOVF 64,W 0CF6: BSF 03.5 0CF7: MOVWF 3B 0CF8: BCF 03.5 0CF9: CALL 388 0CFA: MOVF 78,W 0CFB: BSF 03.5 0CFC: MOVWF 38 .................... pwmVal = pwmTable[index]; 0CFD: BCF 03.0 0CFE: RLF 38,W 0CFF: MOVWF 39 0D00: INCF 39,W 0D01: BCF 0A.3 0D02: BCF 03.5 0D03: CALL 045 0D04: BSF 0A.3 0D05: MOVWF 7A 0D06: BSF 03.5 0D07: MOVF 39,W 0D08: BCF 0A.3 0D09: BCF 03.5 0D0A: CALL 045 0D0B: BSF 0A.3 0D0C: MOVWF 6A 0D0D: MOVF 7A,W 0D0E: MOVWF 6B .................... setMicrostepPWM( pwmVal ); 0D0F: MOVF 6B,W 0D10: BSF 03.5 0D11: MOVWF 42 0D12: BCF 03.5 0D13: MOVF 6A,W 0D14: BSF 03.5 0D15: MOVWF 41 0D16: BCF 03.5 0D17: CALL 000 .................... } .................... else //Reverse 0D18: GOTO 571 .................... { .................... //update PWM mask control - need to think about which one is changing .................... pwmMask = (stepTable[(stepperStepCount-1) %STEPSPERPHASE] ^ stepTable[stepperStepCount]); 0D19: MOVLW 01 0D1A: SUBWF 61,W 0D1B: ANDLW 07 0D1C: ADDWF 5C,W 0D1D: MOVWF 04 0D1E: BCF 03.7 0D1F: BTFSC 5D.0 0D20: BSF 03.7 0D21: MOVF 00,W 0D22: BSF 03.5 0D23: MOVWF 3B 0D24: BCF 03.5 0D25: MOVF 61,W 0D26: ADDWF 5C,W 0D27: MOVWF 04 0D28: BCF 03.7 0D29: BTFSC 5D.0 0D2A: BSF 03.7 0D2B: MOVF 00,W 0D2C: BSF 03.5 0D2D: XORWF 3B,W 0D2E: BCF 03.5 0D2F: MOVWF 69 .................... pwmMask &= stepTable[stepperStepCount]; 0D30: MOVF 61,W 0D31: ADDWF 5C,W 0D32: MOVWF 04 0D33: BCF 03.7 0D34: BTFSC 5D.0 0D35: BSF 03.7 0D36: MOVF 00,W 0D37: ANDWF 69,F .................... write_stepper_mask( pwmMask ); 0D38: MOVF 69,W 0D39: BSF 03.5 0D3A: MOVWF 3E 0D3B: BCF 0A.3 0D3C: BCF 03.5 0D3D: CALL 7D7 0D3E: BSF 0A.3 .................... //update PWM value .................... index = ABS(MAXMICROSTEPSPERSTEP/stepperMicrostepsPerStep) * stepperMicrostepCount; 0D3F: MOVLW 40 0D40: BSF 03.5 0D41: MOVWF 39 0D42: BCF 03.5 0D43: MOVF 67,W 0D44: BSF 03.5 0D45: MOVWF 3A 0D46: BCF 03.5 0D47: CALL 371 0D48: MOVF 78,W 0D49: BSF 03.5 0D4A: MOVWF 39 0D4B: MOVF 39,W 0D4C: MOVWF 3A 0D4D: BCF 03.5 0D4E: MOVF 64,W 0D4F: BSF 03.5 0D50: MOVWF 3B 0D51: BCF 03.5 0D52: CALL 388 0D53: MOVF 78,W 0D54: BSF 03.5 0D55: MOVWF 38 .................... pwmVal = pwmTable[index]; 0D56: BCF 03.0 0D57: RLF 38,W 0D58: MOVWF 39 0D59: INCF 39,W 0D5A: BCF 0A.3 0D5B: BCF 03.5 0D5C: CALL 045 0D5D: BSF 0A.3 0D5E: MOVWF 7A 0D5F: BSF 03.5 0D60: MOVF 39,W 0D61: BCF 0A.3 0D62: BCF 03.5 0D63: CALL 045 0D64: BSF 0A.3 0D65: MOVWF 6A 0D66: MOVF 7A,W 0D67: MOVWF 6B .................... setMicrostepPWM( pwmVal ); 0D68: MOVF 6B,W 0D69: BSF 03.5 0D6A: MOVWF 42 0D6B: BCF 03.5 0D6C: MOVF 6A,W 0D6D: BSF 03.5 0D6E: MOVWF 41 0D6F: BCF 03.5 0D70: CALL 000 .................... } .................... } .................... } .................... //normal microstep handling .................... else 0D71: GOTO 5A4 .................... { .................... index = stepperMicrostepCount * ABS(MAXMICROSTEPSPERSTEP / stepperMicrostepsPerStep); 0D72: MOVLW 40 0D73: BSF 03.5 0D74: MOVWF 39 0D75: BCF 03.5 0D76: MOVF 67,W 0D77: BSF 03.5 0D78: MOVWF 3A 0D79: BCF 03.5 0D7A: CALL 371 0D7B: MOVF 78,W 0D7C: BSF 03.5 0D7D: MOVWF 39 0D7E: BCF 03.5 0D7F: MOVF 64,W 0D80: BSF 03.5 0D81: MOVWF 3A 0D82: MOVF 39,W 0D83: MOVWF 3B 0D84: BCF 03.5 0D85: CALL 388 0D86: MOVF 78,W 0D87: BSF 03.5 0D88: MOVWF 38 .................... pwmVal = pwmTable[index]; 0D89: BCF 03.0 0D8A: RLF 38,W 0D8B: MOVWF 39 0D8C: INCF 39,W 0D8D: BCF 0A.3 0D8E: BCF 03.5 0D8F: CALL 045 0D90: BSF 0A.3 0D91: MOVWF 7A 0D92: BSF 03.5 0D93: MOVF 39,W 0D94: BCF 0A.3 0D95: BCF 03.5 0D96: CALL 045 0D97: BSF 0A.3 0D98: MOVWF 6A 0D99: MOVF 7A,W 0D9A: MOVWF 6B .................... setMicrostepPWM( pwmVal ); 0D9B: MOVF 6B,W 0D9C: BSF 03.5 0D9D: MOVWF 42 0D9E: BCF 03.5 0D9F: MOVF 6A,W 0DA0: BSF 03.5 0DA1: MOVWF 41 0DA2: BCF 03.5 0DA3: CALL 000 .................... } .................... } .................... //Step processing complete .................... } 0DA4: BSF 0A.3 0DA5: BCF 0A.4 0DA6: GOTO 661 (RETURN) .................... Configuration Fuses: Word 1: 373A HS NOWDT NOPUT DEBUG NOPROTECT NOBROWNOUT NOLVP NOCPD NOWRT