43 #define PWM_PERIOD 20000 //in microseconds
45 void configTimer2(
void) {
46 T2CON = T2_OFF | T2_IDLE_CON | T2_GATE_OFF
58 #define SERVO2 _LATB13
59 #define SERVO3 _LATB14
61 #define MIN_PW 600 //minimum pulse width, in us
62 #define MAX_PW 2400 //minimum pulse width, in us
63 #define SLOT_WIDTH 2800 //slot width, in us
65 volatile uint16_t au16_servoPWidths[NUM_SERVOS];
66 volatile uint8_t u8_currentServo =0;
67 volatile uint8_t u8_servoEdge = 1;
68 volatile uint16_t u16_slotWidthTicks = 0;
70 void initServos(
void) {
75 CONFIG_RB2_AS_DIG_OUTPUT();
76 CONFIG_RB3_AS_DIG_OUTPUT();
77 CONFIG_RB13_AS_DIG_OUTPUT();
78 CONFIG_RB14_AS_DIG_OUTPUT();
82 for (u8_i=0; u8_i<NUM_SERVOS; u8_i++) au16_servoPWidths[u8_i]=u16_initPW;
110 #if (defined(__dsPIC33E__) || defined(__PIC24E__))
113 void _ISRFAST _T2Interrupt (
void) {
116 setServoOutput(u8_currentServo, 1);
117 OC1R = au16_servoPWidths[u8_currentServo];
118 OC1RS = au16_servoPWidths[u8_currentServo];
123 OC1CON1 = OC_TIMER2_SRC |
128 void _ISR _OC1Interrupt(
void) {
131 setServoOutput(u8_currentServo, 0);
133 if (u8_currentServo != NUM_SERVOS) {
134 setServoOutput(u8_currentServo, 1);
135 OC1R = au16_servoPWidths[u8_currentServo];
136 OC1RS = au16_servoPWidths[u8_currentServo];
145 void configOutputCapture1(
void) {}
150 void _ISR _OC1Interrupt(
void) {
153 setServoOutput(u8_currentServo, u8_servoEdge);
155 if (u8_servoEdge == 1) {
157 OC1R = OC1R + au16_servoPWidths[u8_currentServo];
161 if (u8_currentServo != NUM_SERVOS -1)
162 OC1R = u16_slotWidthTicks*(u8_currentServo+1);
167 if (u8_currentServo == NUM_SERVOS) u8_currentServo = 0;
172 void configOutputCapture1(
void) {
176 OC1CON = OC_TIMER2_SRC |
187 void getServoValue(
void) {
190 printf(
"Choose servo (1,2,3,4): ");
192 sscanf(sz_buf,
"%d",(
int *) &u16_servo);
193 if ((u16_servo > 4) || (u16_servo < 1)) {
194 printf(
"Invalid servo..\n");
197 printf(
"Enter pulse width (min 600, max 2400): ");
199 sscanf(sz_buf,
"%d",(
int *) &u16_pw);
200 if ((u16_pw > 2400) || (u16_pw < 600)) {
201 printf(
"Invalid pulse width..\n");
204 #if (defined(__dsPIC33E__) || defined(__PIC24E__))
223 configOutputCapture1();
224 #if (defined(__dsPIC33E__) || defined(__PIC24E__))