//========================================== // 4in/4out Mixer for Quadrocopter // // Philip Sun // iLuFa // // V 1.12 (2010/08/29) // //========================================== // Port A A0~A3 as Input: A0 (rudder),A1 (throttle), A2 (Eleon/Elevator),A3 (Aileon). // port B B0~B3 as Output: B0 (motor left),B1(motor right), B2 (motor forward),B3 (motor back). //(V1.12) more smooth to control (small action) //B0 = A1 - ŁGA0/2 + ŁGA3/2 //B1 = A1 - ŁGA0/2 - ŁGA3/2 //B2 = A1 + ŁGA0/2 - ŁGA2/2 //B3 = A1 + ŁGA0/2 + ŁGA2/2 //(V1.1) //B0 = A1 - ŁGA0 + ŁGA3 //B1 = A1 - ŁGA0 - ŁGA3 //B2 = A1 + ŁGA0 - ŁGA2 //B3 = A1 + ŁGA0 + ŁGA2 //========================================== //20Mhz, resolution 6.6 uS, 151 stages //port B B4 as output LED 1 = valid input //========================================== #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 INT8 i,j,indata,indatb,TimerS,templateA; INT16 TimerL; INT R[4]; INT16 T[4],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; delay_mS(5); // 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); PWM[1] = T[1] +227 -(T[0]/2) -(T[3]/2); PWM[2] = T[1] +(T[0]/2) -(T[2]/2); PWM[3] = T[1] +(T[0]/2) +(T[2]/2) -227 ; 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