//************************************************************************************ //** //** File name: C:\Users\Marius\Desktop\Servo Test\ServoMinimal.c //** Generated by: Flowcode v4.5.18.74 //** Date: Monday, August 22, 2011 19:06:12 //** Licence: Professional //** Registered to: Marius van der Walt //** //** //** http://www.matrixmultimedia.com //************************************************************************************ #define MX_PIC //Defines for microcontroller #define P16F88 #define MX_EE #define MX_EE_TYPE2 #define MX_EE_SIZE 256 #define MX_SPI #define MX_SPI_B #define MX_SPI_SDI 1 #define MX_SPI_SDO 2 #define MX_SPI_SCK 4 #define MX_UART #define MX_UART_B #define MX_UART_TX 5 #define MX_UART_RX 2 #define MX_I2C #define MX_I2C_B #define MX_I2C_SDA 1 #define MX_I2C_SCL 4 #define MX_PWM #define MX_PWM_CNT 1 #define MX_PWM_TRIS1 trisb #define MX_PWM_1 0 #define MX_PWM_TRIS1a trisb #define MX_PWM_1a 3 //Functions #define MX_CLK_SPEED 19660800 #ifdef _BOOSTC #include #endif #ifdef HI_TECH_C #include #endif //Configuration data //Internal functions #include "C:\Program Files\Matrix Multimedia\Flowcode V4\FCD\internals.h" //Macro function declarations //Variable declarations //Servo(0): //Defines: /**** Macro Substitutions **** 48 = Capture Compare High Byte Value 0 = Capture Compare Low Byte Value 0x01 = TIMER1 Configuration Register Value 0x0b = Capture Compare 1 Configuration Register Value 0x0a = Capture Compare 2 Configuration Register Value 1 = Number of Channels (1-8) 0,0,0,0,0,0,0,0 = Port Register Array ("0,0,1,1,3,2,0,1") 0,1,2,3,4,5,6,7 = Servo Pin Array ("0,1,0,1,0,0,2,2") 200,200,200,200,200,200,200,200 = Servo Trim Array ("128,128,128,128,128,128,128,128") 6291 = Number of clock cycles equivalent to 1280us ******************************/ //Common defines #define MX_SERVO_NO_CHAN 1 char MX_SERVO_PIN_ARRAY[8] = {0,1,2,3,4,5,6,7}; char MX_SERVO_TRIM[8] = {200,200,200,200,200,200,200,200}; #ifndef MX_MIAC_SYSTEM #define MX_SERVO_CCPR1H 48 #define MX_SERVO_CCPR1L 0 #define MX_SERVO_T1CON 0x01 #define MX_SERVO_CCP1CON 0x0b #define MX_SERVO_CCP2CON 0x0a //Global Variables char MX_SERVO_PORT_ARRAY[8] = {0,0,0,0,0,0,0,0}; char MX_SERVO_POSITION[8] = {0,0,0,0,0,0,0,0}; char MX_SERVO_REQUIRED[8] = {0,0,0,0,0,0,0,0}; unsigned int MX_SERVO_CLK_COUNT = 6291; char MX_SERVO_IDX = 0; unsigned long MX_SERVO_CALC = 0; #endif //Servo(0): //Macro function declarations void FCD_Servo0_TogglePort(char Port, char Pin, char State); void FCD_Servo0_ToggleTris(char Port, char Pin, char State); void FCD_Servo0_EnableServo(char Channel); void FCD_Servo0_DisableServo(char Channel); void FCD_Servo0_SetPosition(char Channel, char Position); void FCD_Servo0_SetTrim(char Channel, char Trim); char FCD_Servo0_MoveToPosition(char Channel, char Position); void FCD_Servo0_AutoMoveToPosition(char Channel, char Position, char StepSpeedms); //Servo(0): //Macro implementations void FCD_Servo0_TogglePort(char Port, char Pin, char State) { #ifndef MX_MIAC_SYSTEM if (Pin > 7) return; switch(Port) { #ifdef _BOOSTC case 0: #ifdef PORTA if(State) set_bit(porta, Pin); else clear_bit(porta, Pin); #endif #ifdef GPIO if(State) set_bit(gpio, Pin); else clear_bit(gpio, Pin); #endif break; case 1: #ifdef PORTB if(State) set_bit(portb, Pin); else clear_bit(portb, Pin); #endif break; case 2: #ifdef PORTC if(State) set_bit(portc, Pin); else clear_bit(portc, Pin); #endif break; case 3: #ifdef PORTD if(State) set_bit(portd, Pin); else clear_bit(portd, Pin); #endif break; case 4: #ifdef PORTE if(State) set_bit(porte, Pin); else clear_bit(porte, Pin); #endif break; case 5: #ifdef PORTF if(State) set_bit(portf, Pin); else clear_bit(portf, Pin); #endif break; #endif #ifdef HI_TECH_C case 0: #ifdef porta if(State) set_bit(porta, Pin); else clear_bit(porta, Pin); #endif #ifdef gpio if(State) set_bit(gpio, Pin); else clear_bit(gpio, Pin); #endif break; case 1: #ifdef portb if(State) set_bit(portb, Pin); else clear_bit(portb, Pin); #endif break; case 2: #ifdef portc if(State) set_bit(portc, Pin); else clear_bit(portc, Pin); #endif break; case 3: #ifdef portd if(State) set_bit(portd, Pin); else clear_bit(portd, Pin); #endif break; case 4: #ifdef porte if(State) set_bit(porte, Pin); else clear_bit(porte, Pin); #endif break; case 5: #ifdef portf if(State) set_bit(portf, Pin); else clear_bit(portf, Pin); #endif break; #endif } #endif } void FCD_Servo0_ToggleTris(char Port, char Pin, char State) { #ifndef MX_MIAC_SYSTEM if (Pin > 7) return; switch(Port) { #ifdef _BOOSTC case 0: #ifdef TRISA if(State) set_bit(trisa, Pin); else clear_bit(trisa, Pin); #endif #ifdef TRISIO if(State) set_bit(trisio, Pin); else clear_bit(trisio, Pin); #endif break; case 1: #ifdef TRISB if(State) set_bit(trisb, Pin); else clear_bit(trisb, Pin); #endif break; case 2: #ifdef TRISC if(State) set_bit(trisc, Pin); else clear_bit(trisc, Pin); #endif break; case 3: #ifdef TRISD if(State) set_bit(trisd, Pin); else clear_bit(trisd, Pin); #endif break; case 4: #ifdef TRISE if(State) set_bit(trise, Pin); else clear_bit(trise, Pin); #endif break; case 5: #ifdef TRISF if(State) set_bit(trisf, Pin); else clear_bit(trisf, Pin); #endif break; #endif #ifdef HI_TECH_C case 0: #ifdef trisa if(State) set_bit(trisa, Pin); else clear_bit(trisa, Pin); #endif #ifdef trisio if(State) set_bit(trisio, Pin); else clear_bit(trisio, Pin); #endif break; case 1: #ifdef trisb if(State) set_bit(trisb, Pin); else clear_bit(trisb, Pin); #endif break; case 2: #ifdef trisc if(State) set_bit(trisc, Pin); else clear_bit(trisc, Pin); #endif break; case 3: #ifdef trisd if(State) set_bit(trisd, Pin); else clear_bit(trisd, Pin); #endif break; case 4: #ifdef trise if(State) set_bit(trise, Pin); else clear_bit(trise, Pin); #endif break; case 5: #ifdef trisf if(State) set_bit(trisf, Pin); else clear_bit(trisf, Pin); #endif break; #endif } #endif } void FCD_Servo0_EnableServo(char Channel) { #ifdef MX_MIAC_SYSTEM #if (48 != MX_MIAC_MM_MIAC_BASE) unsigned int ModID = ((0 << 5) + 48) << 9; MIAC_CAN_TX[0] = Channel; MIAC_Module_WR(ModID, 641, 1,1,0); MIAC_Module_GetAck(ModID + 32, 641, 20); #endif #else if (Channel < MX_SERVO_NO_CHAN) FCD_Servo0_ToggleTris(MX_SERVO_PORT_ARRAY[Channel], MX_SERVO_PIN_ARRAY[Channel], 0); //Convert Servo pin to output to enable motor #endif } void FCD_Servo0_DisableServo(char Channel) { #ifdef MX_MIAC_SYSTEM #if (48 != MX_MIAC_MM_MIAC_BASE) unsigned int ModID = ((0 << 5) + 48) << 9; MIAC_CAN_TX[0] = Channel; MIAC_Module_WR(ModID, 642, 1,1,0); MIAC_Module_GetAck(ModID + 32, 642, 20); #endif #else if (Channel < MX_SERVO_NO_CHAN) FCD_Servo0_ToggleTris(MX_SERVO_PORT_ARRAY[Channel], MX_SERVO_PIN_ARRAY[Channel], 1); //Convert Servo pin to input to disable motor #endif } void FCD_Servo0_SetPosition(char Channel, char Position) { #ifdef MX_MIAC_SYSTEM #if (48 != MX_MIAC_MM_MIAC_BASE) unsigned int ModID = ((0 << 5) + 48) << 9; MIAC_CAN_TX[0] = Channel; MIAC_CAN_TX[1] = Position; MIAC_Module_WR(ModID, 643, 2,1,0); MIAC_Module_GetAck(ModID + 32, 643, 20); #endif #else if (Channel < 8) { MX_SERVO_POSITION[Channel] = Position; //Assign duty to motor controller logic MX_SERVO_REQUIRED[Channel] = Position; } #endif } void FCD_Servo0_SetTrim(char Channel, char Trim) { #ifdef MX_MIAC_SYSTEM #if (48 != MX_MIAC_MM_MIAC_BASE) unsigned int ModID = ((0 << 5) + 48) << 9; MIAC_CAN_TX[0] = Channel; MIAC_CAN_TX[1] = Trim; MIAC_Module_WR(ModID, 644, 2,1,0); MIAC_Module_GetAck(ModID + 32, 644, 20); #endif #else if (Channel < 8) MX_SERVO_TRIM[Channel] = Trim; //Assign duty to motor controller logic #endif } char FCD_Servo0_MoveToPosition(char Channel, char Position) { #ifdef MX_MIAC_SYSTEM #if (48 != MX_MIAC_MM_MIAC_BASE) unsigned int ModID = ((0 << 5) + 48) << 9; MIAC_CAN_TX[0] = Channel; MIAC_CAN_TX[1] = Position; MIAC_Module_WR(ModID, 645, 2,1,0); MIAC_Module_GetAck(ModID + 32, 645, 20); return(MIAC_CAN_RX[0]); #endif #else char arrived = 0; if (Channel < 8) { if (Position > MX_SERVO_POSITION[Channel]) MX_SERVO_POSITION[Channel] = MX_SERVO_POSITION[Channel] + 1; //Increment duty if (Position < MX_SERVO_POSITION[Channel]) MX_SERVO_POSITION[Channel] = MX_SERVO_POSITION[Channel] - 1; //Decrement duty if (Position == MX_SERVO_POSITION[Channel]) arrived = 1; MX_SERVO_REQUIRED[Channel] = MX_SERVO_POSITION[Channel]; } return arrived; #endif } void FCD_Servo0_AutoMoveToPosition(char Channel, char Position, char StepSpeedms) { #ifdef MX_MIAC_SYSTEM #if (48 != MX_MIAC_MM_MIAC_BASE) unsigned int ModID = ((0 << 5) + 48) << 9; MIAC_CAN_TX[0] = Channel; MIAC_CAN_TX[1] = Position; MIAC_Module_WR(ModID, 646, 2,1,0); MIAC_Module_GetAck(ModID + 32, 646, 20); #endif #else if (Channel < 8) MX_SERVO_REQUIRED[Channel] = Position; //Assign required duty #endif } //Macro implementations void main() { //Initialisation ansel = 0; cmcon = 0x07; #ifdef MX_MIAC_SYSTEM #if (48 != MX_MIAC_MM_MIAC_BASE) #define MOD_ID ((0 << 5) + 48) << 9 MIAC_CAN_TX[0] = MX_SERVO_PIN_ARRAY[0]; MIAC_CAN_TX[1] = MX_SERVO_PIN_ARRAY[1]; MIAC_CAN_TX[2] = MX_SERVO_PIN_ARRAY[2]; MIAC_CAN_TX[3] = MX_SERVO_PIN_ARRAY[3]; MIAC_CAN_TX[4] = MX_SERVO_PIN_ARRAY[4]; MIAC_CAN_TX[5] = MX_SERVO_PIN_ARRAY[5]; MIAC_Module_WR(MOD_ID, 640, 6,1,1); MIAC_Module_GetAck(MOD_ID + 32, 640, 20); MIAC_CAN_TX[0] = MX_SERVO_PIN_ARRAY[6]; MIAC_CAN_TX[1] = MX_SERVO_PIN_ARRAY[7]; MIAC_CAN_TX[2] = MX_SERVO_TRIM[0]; MIAC_CAN_TX[3] = MX_SERVO_TRIM[1]; MIAC_CAN_TX[4] = MX_SERVO_TRIM[2]; MIAC_CAN_TX[5] = MX_SERVO_TRIM[3]; MIAC_Module_WR(MOD_ID, 640, 6,0,1); MIAC_Module_GetAck(MOD_ID + 32, 640, 20); MIAC_CAN_TX[0] = MX_SERVO_TRIM[4]; MIAC_CAN_TX[1] = MX_SERVO_TRIM[5]; MIAC_CAN_TX[2] = MX_SERVO_TRIM[6]; MIAC_CAN_TX[3] = MX_SERVO_TRIM[7]; MIAC_CAN_TX[4] = MX_SERVO_NO_CHAN; MIAC_Module_WR(MOD_ID, 640, 5,0,0); MIAC_Module_GetAck(MOD_ID + 32, 640, 20); #undef MOD_ID #endif #else char temp; ccpr1h = MX_SERVO_CCPR1H; //Set capture compare setpoint ccpr1l = MX_SERVO_CCPR1L; t1con = MX_SERVO_T1CON; //Setup timer and start running ccp1con = MX_SERVO_CCP1CON; //Configure CCP1 settings st_bit(pie1, CCP1IE); //Enable CCP1 interrupt ccp2con = MX_SERVO_CCP2CON; //Configure CCP2 settings st_bit(pie2, CCP2IE); //Enable CCP2 interrupt st_bit(intcon, PEIE); //Enable Interrupts st_bit(intcon, GIE); for (temp = 0; temp < MX_SERVO_NO_CHAN; temp++) { FCD_Servo0_ToggleTris(MX_SERVO_PORT_ARRAY[temp], MX_SERVO_PIN_ARRAY[temp], 1); //Convert Servo pin to input to disable motor } #endif //Interrupt initialisation code option_reg = 0xC0; //Call Component Macro //Call Component Macro: Servo(0)::EnableServo(0) FCD_Servo0_EnableServo(0); //Call Component Macro //Call Component Macro: Servo(0)::SetPosition(0, 128) FCD_Servo0_SetPosition(0, 128); mainendloop: goto mainendloop; } void MX_INTERRUPT_MACRO(void) { #ifndef MX_MIAC_SYSTEM if (ts_bit(pir1, CCP1IF) && ts_bit(pie1, CCP1IE)) { if (MX_SERVO_IDX < MX_SERVO_NO_CHAN) { FCD_Servo0_TogglePort(MX_SERVO_PORT_ARRAY[MX_SERVO_IDX], MX_SERVO_PIN_ARRAY[MX_SERVO_IDX], 1); //Start Servo duty cycle MX_SERVO_CALC = MX_SERVO_TRIM[MX_SERVO_IDX] + MX_SERVO_POSITION[MX_SERVO_IDX]; //Calculate the required position MX_SERVO_CALC = MX_SERVO_CALC * MX_SERVO_CLK_COUNT; //Calculate the number of clock cycles required MX_SERVO_CALC = (MX_SERVO_CALC >> 8) & 0xFFFF; //Shift back to be 16-bit ccpr2h = ( MX_SERVO_CALC >> 8 ); //Setup capture compare registers ccpr2l = ( MX_SERVO_CALC & 0xFF ); } cr_bit(pir1, CCP1IF); //Clear interrupt } if (ts_bit(pir2, CCP2IF) && ts_bit(pie2, CCP2IE)) { if (MX_SERVO_IDX < MX_SERVO_NO_CHAN) //If channel is active { FCD_Servo0_TogglePort(MX_SERVO_PORT_ARRAY[MX_SERVO_IDX], MX_SERVO_PIN_ARRAY[MX_SERVO_IDX],0); //End of control pulse } MX_SERVO_IDX = (MX_SERVO_IDX + 1) & 0x07; //Increment channel if (MX_SERVO_REQUIRED[MX_SERVO_IDX] > MX_SERVO_POSITION[MX_SERVO_IDX]) MX_SERVO_POSITION[MX_SERVO_IDX] = MX_SERVO_POSITION[MX_SERVO_IDX] + 1; //Increment duty if (MX_SERVO_REQUIRED[MX_SERVO_IDX] < MX_SERVO_POSITION[MX_SERVO_IDX]) MX_SERVO_POSITION[MX_SERVO_IDX] = MX_SERVO_POSITION[MX_SERVO_IDX] - 1; //Decrement duty cr_bit(pir2, CCP2IF); //Clear interrupt } #endif }