00001 00010 #include "servoconfig2.h" 00011 00012 /* Globals */ 00013 volatile uint16 au16_servoDur2[NUM_SERVOS_USED_2]; 00014 00015 /********************************************************************/ 00027 void initialize_servo_2(void) { 00028 uint8 u8_i; 00029 for (u8_i = 0; u8_i < NUM_SERVOS_USED_2; u8_i++) { 00030 au16_servoDur2[u8_i] = usToU16Ticks(1500, SERVO_PRESCALER_2); //initialize to 1.5 ms 00031 } 00032 #if (NUM_SERVOS_USED_2 >= 1) 00033 CONFIG_SERVO_2_PORT_1(); 00034 #endif 00035 #if (NUM_SERVOS_USED_2 >= 2) 00036 CONFIG_SERVO_2_PORT_2(); 00037 #endif 00038 #if (NUM_SERVOS_USED_2 >= 3) 00039 CONFIG_SERVO_2_PORT_3(); 00040 #endif 00041 #if (NUM_SERVOS_USED_2 >= 4) 00042 CONFIG_SERVO_2_PORT_4(); 00043 #endif 00044 #if (NUM_SERVOS_USED_2 >= 5) 00045 CONFIG_SERVO_2_PORT_5(); 00046 #endif 00047 #if (NUM_SERVOS_USED_2 >= 6) 00048 CONFIG_SERVO_2_PORT_6(); 00049 #endif 00050 #if (NUM_SERVOS_USED_2 >= 7) 00051 CONFIG_SERVO_2_PORT_7(); 00052 #endif 00053 #ifdef ESOS_USE_IRQS 00054 ESOS_REGISTER_PIC24_USER_INTERRUPT(ESOS_IRQ_PIC24_T2, SERVO_IRQ_2, 00055 _T2Interrupt); 00056 ESOS_ENABLE_PIC24_USER_INTERRUPT(ESOS_IRQ_PIC24_T2); 00057 #else 00058 //continue whilst idle, 1:64 prescaler (1.6us tick), 16-bit timer, FCY source 00059 T2CON = T2_IDLE_CON + SERVO_PRESCALER_BITFIELD_2 + T2_32BIT_MODE_OFF + 00060 T2_SOURCE_INT; 00061 PR2 = au16_servoDur2[0]; //set PR2 to 1.5 ms 00062 TMR2 = 0; //clear the TMR2 register 00063 T2CONbits.TON = 1; //turn TMR2 on 00064 _T2IF = 0; //clear interrupt flag 00065 _T2IP = SERVO_IRQ_2; //choose a priority 00066 _T2IE = 1; //enable the interrupt 00067 #endif 00068 } 00069 00070 /**********************************************************************/ 00082 ESOS_USER_INTERRUPT(ESOS_IRQ_PIC24_T2) { 00083 static uint8 u8_pwmState = 0; 00084 static uint16 u16_servoDurCopy; 00085 ESOS_MARK_PIC24_USER_INTERRUPT_SERVICED(ESOS_IRQ_PIC24_T2); 00086 switch (u8_pwmState) { 00087 case 0: 00088 ASSERT(au16_servoDur2[0] > 0 && au16_servoDur2[0] < TIME_PER_SERVO_2); //make sure it is a reasonable duration 00089 SERVO_2_PORT_1 = 1; //assert the port 00090 PR2 = au16_servoDur2[0]; //set PR2 for the next interrupt 00091 u16_servoDurCopy = au16_servoDur2[0]; //save a local copy 00092 u8_pwmState++; //transition to the next state 00093 break; 00094 case 1: 00095 SERVO_2_PORT_1 = 0; //deassert the port 00096 PR2 = TIME_PER_SERVO_2 - u16_servoDurCopy; //set PR2 for 3ms - duration 00097 u8_pwmState++; //transition to the next state, or spill over into default case 00098 break; 00099 #if (NUM_SERVOS_USED_2 >= 2) 00100 case 2: 00101 ASSERT(au16_servoDur2[1] > 0 && au16_servoDur2[1] < TIME_PER_SERVO_2); //make sure it is a reasonable duration 00102 SERVO_2_PORT_2 = 1; //assert the port 00103 PR2 = au16_servoDur2[1]; //set PR2 for the next interrupt 00104 u16_servoDurCopy = au16_servoDur2[1]; //save a local copy 00105 u8_pwmState++; //transition to the next state 00106 break; 00107 case 3: 00108 SERVO_2_PORT_2 = 0; //deassert the port 00109 PR2 = TIME_PER_SERVO_2 - u16_servoDurCopy; //set PR2 for 3ms - duration 00110 u8_pwmState++; //transition to the next state, or spill over into default case 00111 break; 00112 #endif 00113 #if (NUM_SERVOS_USED_2 >= 3) 00114 case 4: 00115 ASSERT(au16_servoDur2[2] > 0 && au16_servoDur2[2] < TIME_PER_SERVO_2); //make sure it is a reasonable duration 00116 SERVO_2_PORT_3 = 1; //assert the port 00117 PR2 = au16_servoDur2[2]; //set PR2 for the next interrupt 00118 u16_servoDurCopy = au16_servoDur2[2]; //save a local copy 00119 u8_pwmState++; //transition to the next state 00120 break; 00121 case 5: 00122 SERVO_2_PORT_3 = 0; //deassert the port 00123 PR2 = TIME_PER_SERVO_2 - u16_servoDurCopy; //set PR2 for 3ms - duration 00124 u8_pwmState++; //transition to the next state, or spill over into default case 00125 break; 00126 #endif 00127 #if (NUM_SERVOS_USED_2 >= 4) 00128 case 6: 00129 ASSERT(au16_servoDur2[3] > 0 && au16_servoDur2[3] < TIME_PER_SERVO_2); //make sure it is a reasonable duration 00130 SERVO_2_PORT_4 = 1; //assert the port 00131 PR2 = au16_servoDur2[3]; //set PR2 for the next interrupt 00132 u16_servoDurCopy = au16_servoDur2[3]; //save a local copy 00133 u8_pwmState++; //transition to the next state 00134 break; 00135 case 7: 00136 SERVO_2_PORT_4 = 0; //deassert the port 00137 PR2 = TIME_PER_SERVO_2 - u16_servoDurCopy; //set PR2 for 3ms - duration 00138 u8_pwmState++; //transition to the next state, or spill over into default case 00139 break; 00140 #endif 00141 #if (NUM_SERVOS_USED_2 >= 5) 00142 case 8: 00143 ASSERT(au16_servoDur2[4] > 0 && au16_servoDur2[4] < TIME_PER_SERVO_2); //make sure it is a reasonable duration 00144 SERVO_2_PORT_5 = 1; //assert the port 00145 PR2 = au16_servoDur2[4]; //set PR2 for the next interrupt 00146 u16_servoDurCopy = au16_servoDur2[4]; //save a local copy 00147 u8_pwmState++; //transition to the next state 00148 break; 00149 case 9: 00150 SERVO_2_PORT_5 = 0; //deassert the port 00151 PR2 = TIME_PER_SERVO_2 - u16_servoDurCopy; //set PR2 for 3ms - duration 00152 u8_pwmState++; //transition to the next state, or spill over into default case 00153 break; 00154 #endif 00155 #if (NUM_SERVOS_USED_2 >= 6) 00156 case 10: 00157 ASSERT(au16_servoDur2[5] > 0 && au16_servoDur2[5] < TIME_PER_SERVO_2); //make sure it is a reasonable duration 00158 SERVO_2_PORT_6 = 1; //assert the port 00159 PR2 = au16_servoDur2[5]; //set PR2 for the next interrupt 00160 u16_servoDurCopy = au16_servoDur2[5]; //save a local copy 00161 u8_pwmState++; //transition to the next state 00162 break; 00163 case 11: 00164 SERVO_2_PORT_6 = 0; //deassert the port 00165 PR2 = TIME_PER_SERVO_2 - u16_servoDurCopy; //set PR2 for 3ms - duration 00166 u8_pwmState++; //transition to the next state, or spill over into default case 00167 break; 00168 #endif 00169 #if (NUM_SERVOS_USED_2 >= 7) 00170 case 12: 00171 ASSERT(au16_servoDur2[6] > 0 && au16_servoDur2[6] < TIME_PER_SERVO_2); //make sure it is a reasonable duration 00172 SERVO_2_PORT_7 = 1; //assert the port 00173 PR2 = au16_servoDur2[6]; //set PR2 for the next interrupt 00174 u16_servoDurCopy = au16_servoDur2[6]; //save a local copy 00175 u8_pwmState++; //transition to the next state 00176 break; 00177 case 13: 00178 SERVO_2_PORT_7 = 0; //deassert the port 00179 PR2 = TIME_PER_SERVO_2 - u16_servoDurCopy; //set PR2 for 3ms - duration 00180 u8_pwmState++; //transition to the next state, or spill over into default case 00181 break; 00182 #endif 00183 case (NUM_SERVOS_USED_2 * 2): 00184 PR2 = SERVO_PULSE_PERIOD_2 - (NUM_SERVOS_USED_2 * TIME_PER_SERVO_2); //how long should we wait until to finish the period 00185 u8_pwmState = 0; //reset state 00186 break; 00187 00188 default: // Should never enter this state 00189 ASSERT(0); 00190 break; 00191 } //end switch(u8_pwmState) 00192 }
1.5.8