PWM lezen, invoer van RC-ontvanger decoderen en fail-safe toepassen

Code

• PWMread_RCfailsafe
• RC_Read_Example
• RC_FrameRate
• RC_ServoMixer_Example

PWMread_RCfailsafeArduino

/* Kelvin Nelson 24/07/2019 * * Pulsbreedtemodulatie (PWM)-decodering van RC-ontvanger met failsafe * * Deze code bevat eenvoudig te gebruiken functies om blokgolfsignalen te meten op elk arduiuno pro mini, nano of uno pinnen, exclusief A6 en A7. * De code is bedoeld voor gebruik met RC-ontvangers, maar kan ook worden gebruikt in de meeste andere PWM-meettoepassingen als een directe vervanging voor pulseIn(PIN, HIGH). * (tot op heden is het niet getest op een frequentie hoger dan 1khz of op een Arduino mega) * * Een RC-signaalpuls kan worden omgezet van een pulsduur (1000-2000uS) op elke ingangspin in een -+100 % (-+1,0) uitvoer voor gebruik in een schets. * De kalibratie voor deze conversie plus een failsafe-instelling kan voor elk kanaal worden ingesteld. (fail safe toleranties 10-330Hz en 500-2500uS). ** De onbewerkte gegevens voor elke pin kunnen ook worden geëxtraheerd, d.w.z. pulstijd, pulsbreedte, framelengte, taak en frequentie. * * Set-up is snel, de organisatie van dit bestand is als volgt:* * - Overzicht van de code * - Lijst met functies * - Hoe te gebruiken, inclusief voorbeeldschetsen * - Door de gebruiker gedefinieerde variabelen -> specificeer invoerpinnen, zender kalibratie en failsafe. * - Globale variabelen en functies * * OVERZICHT VAN DE CODE:* * De code maakt pinwisselonderbrekingen op de geselecteerde pinnen mogelijk door de juiste registers in te stellen. * * Een spanningsverandering op een van de geselecteerde pinnen zal een van de drie Interrupt Service Routines activeren, afhankelijk van tot welk register de pin behoort. * - ISR(PCINT0_vect), ISR(PCINT1_vect) of ISR(PCINT2_vect) * * Binnen elke ISR bepaalt de code welke pin is gewijzigd, en noteert hij de tijd voordat hij terugkeert naar de hoofdlus(). * * De tijdsintervallen tussen pinwisselingen worden gebruikt om de pulsbreedte en framelengte te berekenen. * * Vlaggen worden ingesteld door de ISR om aan te geven wanneer nieuwe pulsen worden ontvangen. * * De vlaggen worden vervolgens gebruikt om de door elke ISR verzamelde gegevens te extraheren en te verwerken. * * Hoewel het niet precies hetzelfde is, volgt deze code soortgelijke principes als de principes die in deze video worden uitgelegd:https://youtu.be/bENjl1KQbvo * */// LIJST VAN FUNCTIES:// UITVOERTYPE NAAM VAN FUNCTIE OPMERKINGEN// void setup_pwmRead() initialiseer de PWM-meting met behulp van pin change interrupts// RC RECEIVER DECODING// boolean RC_avail() retourneert een HIGH wanneer nieuwe RC-gegevens beschikbaar zijn// float RC_decode (kanaalnummer) decodeert het geselecteerde RC-kanaal in het bereik +-100 %, en past een failsafe toe.// void print_RCpwm() Drukt de onbewerkte gegevens van het RC-kanaal af naar de seriële poort (gebruikt voor kalibratie).// GENERIC PWM MEASUREMENTS// boolean PWM_read (kanaalnummer) geeft een HOOG wanneer een nieuwe puls is ontvangen gedetecteerd op een bepaald kanaal. // De functie slaat de pulsgegevens op naar variabelen buiten de interruptroutines// en moet worden aangeroepen net voordat de rest van de PWM-functies wordt gebruikt.// unsigned long PWM_time() geeft de tijd aan het begin van de puls terug // float PWM() geeft de pulsbreedte terug// float PWM_period() geeft de tijd tussen pulsen terug// float PWM_freq() berekent de frequentie// float PWM_duty() berekent de duty// OPMERKING:PWM_read(CH) en RC_decode(CH) gebruiken dezelfde vlaggen om te detecteren wanneer nieuwe gegevens beschikbaar zijn, wat betekent dat gegevens verloren kunnen gaan als beide tegelijkertijd op hetzelfde kanaal worden gebruikt.// SUGESTIE:als u PWM_read(CH) wilt gebruiken om de framesnelheid van een RC-kanaal te vinden, noem het dan voor RC_decode(CH). De uitvoer van RC_decode(CH) wordt dan standaard de failsafe.// GEBRUIKSAANWIJZING, inclusief voorbeeldschetsen// onder de titel "USER DEFINED VARIABLES" in de onderstaande code://// Stap 1:voer de invoerpinnen in in de array pwmPIN[] ={}. //// - Elk aantal pinnen kan worden ingevoerd in pwmPIN[] (pinnen beschikbaar 0 - 13 en A0 - A5)// - De pinnen hoeven niet in numerieke volgorde te staan, bijvoorbeeld pwmPIN[] ={A0, 5,6,10,8} voor 5 kanalen, of pwmPIN[] ={A0,5} voor 2 kanalen// - Het eerste element in de array is het pinnummer voor "kanaal 1", en het tweede is de pin nummer voor "kanaal 2"... etc.// - Alle pinnen die op de RC-ontvanger zijn aangesloten, moeten aan het begin van de array zijn. dwz de eerste 2 kanalen kunnen RC-ingangen zijn en het 3e kanaal kan worden aangesloten op een ander apparaat zoals de echo-pin van een ultrasone sensor.//// Stap 2:als een RC-ontvanger is aangesloten op alle ingangen, stel dan RC_inputs in op 0, zo niet specificeer het aantal kanalen dat op de ontvanger is aangesloten, dwz RC_inputs =2;//// Stap 3:kalibreer uw zender door een eenvoudige schets te uploaden met dit .ino-bestand in de schetsmap, en druk de onbewerkte PWM-waarden af naar serieel (of kopieer en plak de benodigde functies in de schets).// Gebruik de informatie van de seriële monitor om handmatig de waarden in de arrays RC_min[], RC_mid[], RC_max[] aan te passen aan uw zender (gebruik volledige snelheden om de beste resolutie krijgen). // een voorbeeldschets voor het afdrukken van de PWM-gegevens van het RC-kanaal naar serieel. /* void setup() { setup_pwmRead(); Serieel.begin(9600); } void loop() { if(RC_avail()) print_RCpwm(); } */// Stap 4:Kies een failsafe-positie voor elk kanaal, in het bereik van -1,0 tot +1,0, en voer deze in in de array RC_failsafe[] ={}// Opmerking:als u wilt dat de arduino reageert op het verlies van het zendersignaal moet u mogelijk de failsafe-functie op uw ontvanger uitschakelen (als deze er een heeft).// een voorbeeldschets om de werking van de failsafe te controleren en voor het afdrukken van de gekalibreerde kanalen naar serieel:/* unsigned long now; // timingvariabelen om gegevens regelmatig bij te werken unsigned long rc_update; const int kanalen =6; // specificeer het aantal ontvangerkanalen float RC_in [kanalen]; // een array om de gekalibreerde invoer van receiver void setup () { setup_pwmRead (); Serieel.begin(9600); } void loop() { now =millis(); if(RC_avail() || now - rc_update> 25){ // als RC-gegevens beschikbaar zijn of 25 ms is verstreken sinds de laatste update (aanpassen aan framesnelheid van ontvanger) rc_update =nu; //print_RCpwm(); // uncommment om onbewerkte gegevens van ontvanger naar serieel af te drukken voor (int i =0; i=0 &&pwmPIN[i] <=7) pwmPIN_port[i] =2; // pin behoort tot PCINT2_vect (POORT D) else if (pwmPIN[i]>=8 &&pwmPIN[i] <=13) pwmPIN_port[i] =0; // pin hoort bij PCINT0_vect (POORT B) // verbergt het pinnummer (d.w.z. pin 11 of pin A0) naar de pinpositie in het poortregister. Er is hoogstwaarschijnlijk een betere manier om dit te doen met behulp van een macro... // (Het rechtstreeks lezen van de pinstatus uit de poortregisters versnelt de code in de ISR) if(pwmPIN[i] ==0 || pwmPIN[i ] ==A0 || pwmPIN[i] ==8) pwmPIN_reg[i] =0b00000001; else if(pwmPIN[i] ==1 || pwmPIN[i] ==A1 || pwmPIN[i] ==9) pwmPIN_reg[i] =0b00000010; else if(pwmPIN[i] ==2 || pwmPIN[i] ==A2 || pwmPIN[i] ==10) pwmPIN_reg[i] =0b00000100; else if(pwmPIN[i] ==3 || pwmPIN[i] ==A3 || pwmPIN[i] ==11) pwmPIN_reg[i] =0b00001000; else if(pwmPIN[i] ==4 || pwmPIN[i] ==A4 || pwmPIN[i] ==12) pwmPIN_reg[i] =0b00010000; else if(pwmPIN[i] ==5 || pwmPIN[i] ==A5 || pwmPIN[i] ==13) pwmPIN_reg[i] =0b00100000; anders if(pwmPIN[i] ==6) pwmPIN_reg[i] =0b01000000; anders if(pwmPIN[i] ==7) pwmPIN_reg[i] =0b10000000; }}// SETUP VAN PIN WIJZIGING INTERRUPTSvoid setup_pwmRead(){ for(int i =0; i  num_ch) RC_inputs =num_ch; // definieer het aantal pinnen dat is aangesloten op een RC-ontvanger. } // INTERRUPT SERVICE ROUTINES (ISR) GEBRUIKT OM PWM INPUT TE LEZEN// de PCINT0_vect (B-poortregister) reageert op eventuele wijzigingen op pinnen D8-13.// de PCINT1_vect (C-poortregister) "" "" A0-A5. // de PCINT2_vect (D-poortregister) "" "" D0-7.// poortregisters worden gebruikt om if-statements in ISR-code te versnellen:// https://www.arduino.cc/en/Reference/PortManipulation http ://tronixstuff.com/2011/10/22/tutorial-arduino-port-manipulation/// http://harperjiangnew.blogspot.co.uk/2013/05/arduino-port-manipulation-on-mega-2560 .html// READ INTERRUPTS ON PINS D8-D13:ISR routine detects which pin has changed, and returns PWM pulse width, and pulse repetition period.ISR(PCINT0_vect){ // this function will run if a pin change is detected on portB pciTime =micros(); // Record the time of the PIN change in microseconds for (int i =0; i  RC_inputs) return 0; // if channel number is out of bounds return zero. int i =CH - 1; // determine the pulse width calibration for the RC channel. The default is 1000, 1500 and 2000us. int Min; if(CH <=size_RC_min) Min =RC_min[CH-1]; else Min =1000; int Mid; if(CH <=size_RC_mid) Mid =RC_mid[CH-1]; else Mid =1500; int Max; if(CH <=size_RC_max) Max =RC_max[CH-1]; else Max =2000; float CH_output; if(FAILSAFE(CH) ==HIGH){ // If the RC channel is outside of failsafe tolerances (10-330hz and 500-2500uS) if(CH> size_RC_failsafe) CH_output =0; // and if no failsafe position has been defined, set output to neutral else CH_output =RC_failsafe[i]; // or if defined set the failsafe position } else{ // If the RC signal is valid CH_output =calibrate(PW[i],Min,Mid,Max); // calibrate the pulse width to the range -1 to 1. } return CH_output; // The signal is mapped from a pulsewidth into the range of -1 to +1, using the user defined calibrate() function in this code. // 0 represents neutral or center stick on the transmitter // 1 is full displacement of a control input is one direction (i.e full left rudder) // -1 is full displacement of the control input in the other direction (i.e. full right rudder)}/* * Receiver Calibration */ // NEED TO SPEED UPfloat calibrate(float Rx, int Min, int Mid, int Max){ float calibrated; if (Rx>=Mid) { calibrated =map(Rx, Mid, Max, 0, 1000); // map from 0% to 100% in one direction } else if (Rx ==0) { calibrated =0; // neutral } else { calibrated =map(Rx, Min, Mid, -1000, 0); // map from 0% to -100% in the other direction } return calibrated * 0.001;}// Basic Receiver FAIL SAFE// check for 500-2500us and 10-330Hz (same limits as pololu)boolean FAILSAFE(int CH){ int i =CH-1; boolean failsafe_flag =LOW; if(pwmFlag[i] ==1) // if a new pulse has been measured. { pwmFlag[i] =0; // set flag to zero if(pwmPeriod[i]> 100000) // if time between pulses indicates a pulse rate of less than 10Hz { failsafe_flag =HIGH; } else if(pwmPeriod[i] <3000) // or if time between pulses indicates a pulse rate greater than 330Hz { failsafe_flag =HIGH; } if(PW[i] <500 || PW[i]> 2500) // if pulswidth is outside of the range 500-2500ms { failsafe_flag =HIGH; } } else if (micros() - pwmTimer[i]> 100000) // if there is no new pulswidth measurement within 100ms (10hz) { failsafe_flag =HIGH; } return failsafe_flag; }/* * Quick print function of Rx channel input */void print_RCpwm(){ // display the raw RC Channel PWM Inputs for (int i =0; i =0) Serial.print(" "); if (abs(pc) <100) Serial.print(" "); if (abs(pc) <10) Serial.print(" "); Serial.print(" ");Serial.print(pc);Serial.print("% ");}/* * GENERIC PWM FUNCTIONS */unsigned long pin_time;float pin_pwm;float pin_period;boolean PWM_read(int CH){ if(CH <1 &&CH> num_ch) return false; int i =CH-1; boolean avail =pwmFlag[i]; if (avail ==HIGH){ pwmFlag[i] =LOW; noInterrupts(); pin_time =pwmTimer[i]; pin_pwm =PW[i]; pin_period =pwmPeriod[i]; onderbreekt(); } return avail;}unsigned long PWM_time(){return pin_time;}float PWM_period(){return pin_period;}float PWM(){return pin_pwm;}float PWM_freq(){ float freq; return freq =1000000 / pin_period; // frequency Hz}float PWM_duty(){ float duty; duty =pin_pwm/pin_period; return duty;}

RC_Read_ExampleArduino

unsigned long now; // timing variables to update data at a regular interval unsigned long rc_update;const int channels =6; // specify the number of receiver channelsfloat RC_in[channels]; // an array to store the calibrated input from receiver void setup() { setup_pwmRead(); Serial.begin(9600);}void loop() { now =millis(); if(RC_avail() || now - rc_update> 25){ // if RC data is available or 25ms has passed since last update (adjust to be equal or greater than the frame rate of receiver) rc_update =now; print_RCpwm(); // uncommment to print raw data from receiver to serial for (int i =0; i 
RC_FrameRateArduino 
 Example sketch that prints the frame rate and frequency of an RC Receiver. The PWMread_RCfailsafe.ino file should be copied into the same folder in order for the functions to be available.
void setup() { setup_pwmRead(); Serial.begin(9600);}void loop() { // Print RC receiver frame length and frame rate if (PWM_read(1)){ // if a new pulse is detected on channel 1 Serial.print(PWM_period(),0);Serial.print("uS "); Serial.print(PWM_freq());Serial.println("Hz"); }}
 
RC_ServoMixer_ExampleArduino 
 An servo mixing example. Two channels from a 6 channel are receiver are mixed and sent to two servos controlled using the servo library. The PWMread_RCfailsafe.ino file should be copied into the same folder in order for the functions to be available.
// servo variables#include  // include the servo library to control the servosServo servo1; // name each servo output for use with the servo library Servo servo2; // Each servo must be attached to a pin that has a PWM output// on the arduino uno, nano and pro mini these pins are 3, 5, 6, 9, 10 and 11const int servo1_pin =9; // identify the pins that each servo signal wire is connected toconst int servo2_pin =10;// Select Servo Direction, Rates and Sub-trim (the size of each array must match the number of servos)boolean servo_dir[] ={0,1}; // Direction:0 is normal, 1 is reversefloat servo_rates[] ={1,0.5}; // Rates:range 0 to 2 (1 =+-500us (NORMAL), 2 =+-1000us (MAX)):The amount of servo deflection in both directionsfloat servo_subtrim[] ={0.0,0.0}; // Subtrimrange -1 to +1 (-1 =1000us, 0 =1500us, 1 =2000us):The neutral position of the servoboolean servo_mix_on =true;unsigned long now; // timing variables to update data at a regular interval unsigned long rc_update;// Receiver variablesconst int channels =6; // specify the number of receiver channelsfloat RC_in[channels]; // an array to store the calibrated input from receiver void setup() { servo1.attach(servo1_pin, 500, 2500); // attach the servo library to each servo pin, and define min and max uS values servo2.attach(servo2_pin, 500, 2500); setup_pwmRead(); Serial.begin(9600);}void loop() { now =millis(); if(RC_avail() || now - rc_update> 25){ // if RC data is available or 25ms has passed since last update (adjust to> frame rate of receiver) rc_update =now; print_RCpwm(); // uncommment to print raw data from receiver to serial for (int i =0; i 1) mix1 =1; // limit mixer output to +-1 else if(mix1 <-1) mix1 =-1; if(mix2> 1) mix2 =1; // limit mixer output to +-1 else if(mix2 <-1) mix2 =-1; // Calculate the pulse widths for the servos servo1_uS =calc_uS(mix1, 1); // apply the servo rates, direction and sub_trim for servo 1, and convert to a RC pulsewidth (microseconds, uS) servo2_uS =calc_uS(mix2, 2); // apply the servo rates, direction and sub_trim for servo 2, and convert to a RC pulsewidth (microseconds, uS) } else{ // MIXING OFF servo1_uS =calc_uS(RC_in[1],1); // apply the servo rates, direction and sub_trim for servo 1, and convert to a RC pulsewidth (microseconds, uS) servo2_uS =calc_uS(RC_in[2],2); // apply the servo rates, direction and sub_trim for servo 1, and convert to a RC pulsewidth (microseconds, uS) } servo1.writeMicroseconds(servo1_uS); // write the pulsewidth to the servo. servo2.writeMicroseconds(servo2_uS); // write the pulsewidth to the servo. }}int calc_uS(float cmd, int servo){ // cmd =commanded position +-100% // servo =servo num (to apply correct direction, rates and trim) int i =servo-1; float dir; if(servo_dir[i] ==0) dir =-1; else dir =1; // set the direction of servo travel cmd =1500 + (cmd*servo_rates[i]*dir + servo_subtrim[i])*500; // apply servo rates and sub trim, then convert to a uS value if(cmd> 2500) cmd =2500; // limit pulsewidth to the range 500 to 2500us else if(cmd <500) cmd =500; return cmd;}
  
 
 Schema's  
 This RC Receiver is powered by 5v and ground from the ICSP pins with the 6 signal outputs connected to pins 2-7 
 Micro servo 1 is powered by 5v pin and ground, with signal wire connected to pin 9 
 Micro servo 2 powered by 3.3v pin and ground, with signal wired connected to pin 10