#include <16F84.h>
#FUSES NOWDT //No Watch Dog Timer
#FUSES HS //High speed Osc (> 4mhz)
#FUSES NOPUT //No Power Up Timer
#FUSES NOPROTECT //Code not protected from reading
#use delay(clock=20000000)
//#use rs232(baud=9600,xmit=PIN_B6,rcv=PIN_B7) //debug
#byte portb=0x06
INT8 i,j,indata,indatb,TimerS,templateA;
INT16 TimerL;
INT R[4];
INT16 T[4];
INT16 PWM[4];
//------------------------
void main(){
// 0 Init
setup_timer_0(RTCC_INTERNAL|RTCC_DIV_1);
templateA= 0b00001111 ; // input 4 channels
PWM[0]=227;
PWM[1]=151; //init throttle at lowerst position
PWM[2]=227;
PWM[3]=227;
//LDHUng ,Inital Gyro esky 0704,It take about 15 second
set_tris_b(0);
for(TimerL=1;TimerL<=710; TimerL++)
{
portb = 0b00001111;
delay_us(1500); //want servo to move to 90 degrees,center point servo
portb = 0b00000000;
if(TimerL%20<=10)
portb = 0b00010000;
delay_us(18500);
}
output_low(pin_B4);
//LDHUng
delay_mS(100); // wait
// wait an inpit
// To find an interval > 2 mS ,the syn single of PPM
TimerS =0;
do{
indatb = INPUT_A();
indata=(indatb & templateA);}
while(indata==0);
while(TimerS < 40) {
indatb = INPUT_A();
indata=(indatb & templateA);
if(indata ==0) {TimerS++ ; delay_uS(50);} //50uS*40 =2mS
else TimerS =0;
}
// A Determine the input sequence
// Had find the begin of PPM string
// store first channel of PWM in R0, second in R1..
i=0;
do {
do{
indatb = INPUT_A();
indata=(indatb & templateA );}
while (indata ==0);
if (indata ==1){ R[i]=1; templateA=(templateA ^0b00000001);}
if (indata ==2){ R[i]=2; templateA=(templateA ^0b00000010);}
if (indata ==4){ R[i]=4; templateA=(templateA ^0b00000100);}
if (indata ==8){ R[i]=8; templateA=(templateA ^0b00001000);}
i++;
do{
indatb = INPUT_A();
indata=(indatb & templateA );}
while (! indata ==0);
}
while (! templateA==0); // all 4in were detect and sort
//------------------------
// B Read 4 input one by one
// Begin of Endless loop
// Read PWM one by one, R1~R4, take 5.5~9.5 mS
// T0~T3 = 151~302 = 1mS~2mS +- 10%
// delay_cycles(24) =33cycles 6.6uS,
do{
output_low(pin_B4); // Valid input indecator
for (i=0;i<4;++i){
timerL=0;
j=R[i];
if (j==1){ do {} //read first channel
while (! input(pin_A0) );
do { TimerL ++; // first channel rise
delay_cycles(24); // make total 33 cycles
}
while ( input(PIN_A0) );
T[0]=TimerL; //first channel down
}
if (j==2){ do {}
while (! input(pin_A1) );
do { TimerL ++;
delay_cycles(24);
}
while ( input(PIN_A1) );
T[1]=TimerL;
}
if (j==4){ do {}
while (! input(pin_A2) );
do { TimerL ++;
delay_cycles(24);
}
while ( input(PIN_A2) );
T[2]=TimerL;
}
if (j==8){ do {}
while (! input(pin_A3) );
do { TimerL ++;
delay_cycles(24);
}
while ( input(PIN_A3) );
T[3]=TimerL;
}
output_high(pin_B4); // Valid input
}
//---------------------------
// C Calculating
// Calculating must less than 2.5mS
/*PWM[0] = T[1] +(T[3]/2) -(T[0]/2)-1;
PWM[1] = T[1] +227 -(T[0]/2) -(T[3]/2)+2;
PWM[2] = T[1] +(T[0]/2) -(T[2]/2)-3;
PWM[3] = T[1] +(T[0]/2) +(T[2]/2) -227 -3 ;
*/
PWM[0] = T[1] +T[3] -T[0]-1;
PWM[1] = T[1] +454 -T[0] -T[3]+5;
PWM[2] = T[1] +T[0] -T[2]-6;
PWM[3] = T[1] +T[0] +T[2] -454 -9 ;
for (i=0;i<4;++i){
if (PWM[i] < 130) PWM[i] = 130; // safe range control, 90%~110%
if (PWM[i] > 325) PWM[i] = 325;
}
//---------------------------
// D Out put 4 PWM
// output 4 PWM, take 2.5mS
// 1 unit = 33 cycles, 6.6 uS
output_high(pin_B0);
delay_cycles(10) ; //delay to match output_low
output_high(pin_B1);
delay_cycles(10) ;
output_high(pin_B2);
delay_cycles(10) ;
output_high(pin_B3);
timerL=0;
while (timerL < 330){
timerL++;
if (timerL == PWM[0]) output_low(pin_B0);
if (timerL == PWM[1]) output_low(pin_B1);
if (timerL == PWM[2]) output_low(pin_B2);
if (timerL == PWM[3]) output_low(pin_B3);
delay_cycles(1); //total 33 cycles
}
}
while (true); //End of endless loop
} // End of Main
