diff --git a/controller.ino b/controller.ino index f74c12d..5801ad4 100644 --- a/controller.ino +++ b/controller.ino @@ -7,12 +7,14 @@ //to flash set boot0 (the one further away from reset button) to 1 and press reset, flash, program executes immediately //set boot0 back to 0 to run program on powerup +//#define DEBUG + #define PIN_LED PC13 #define PIN_POTI1 PA7 #define PIN_POTI2 PA6 -#define SENDPERIOD 50 +#define SENDPERIOD 20 uint16_t poti1=0; @@ -24,17 +26,43 @@ long last_send=0; int16_t out_steer=0; int16_t out_speed=0; + +volatile long t_raising1=0; +volatile long t_falling1=0; +volatile long t_raising2=0; +volatile long t_falling2=0; +volatile long t_raising3=0; +volatile long t_falling3=0; +volatile long t_raising4=0; +volatile long t_falling4=0; + +long last_updated_ch1=0; +long last_updated_ch2=0; +long last_updated_ch3=0; +long last_updated_ch4=0; + + +long ppmlagmillis=0; //updated by ppmOK() +boolean flag_ppmupdated=false; //true if at least one ppm value (chx_in) updated + + +#define MAXPPMUPDATETIME 50 //max time it should take to update a ppm channel value (otherwise ppmOK() will return false) uint16_t ch1_in; uint16_t ch2_in; uint16_t ch3_in; uint16_t ch4_in; +//ch1 = steer (ail) + //ch2 = speed (ele) + //ch3 = speed multiplier (thr) -//RC Input Pins #define PIN_CH1 PB9 #define PIN_CH2 PB8 #define PIN_CH3 PB7 #define PIN_CH4 PB6 +#define PPM_TIME_MIN 900 +#define PPM_TIME_MAX 2100 + void setup() { @@ -51,22 +79,20 @@ void setup() { pinMode(PIN_CH1, INPUT); + attachInterrupt(PIN_CH1, ppmchanged1, CHANGE); //see http://docs.leaflabs.com/static.leaflabs.com/pub/leaflabs/maple-docs/0.0.12/lang/api/attachinterrupt.html pinMode(PIN_CH2, INPUT); + attachInterrupt(PIN_CH2, ppmchanged2, CHANGE); pinMode(PIN_CH3, INPUT); + attachInterrupt(PIN_CH3, ppmchanged3, CHANGE); pinMode(PIN_CH4, INPUT); + attachInterrupt(PIN_CH4, ppmchanged4, CHANGE); } void loop() { - //RC Input. chn_in between 0 and 1000 - ch1_in = constrain(map(pulseIn(PIN_CH1, HIGH, 25000),1000,2000, 0,1000), 0,1000); //Read Pulse Width. (pulseIn values typically between 1000 and 2000). Map and constrain between 0 and 1000 - ch2_in = constrain(map(pulseIn(PIN_CH2, HIGH, 25000),1000,2000, 0,1000), 0,1000); - //ch3_in = constrain(map(pulseIn(PIN_CH3, HIGH, 25000),1000,2000, 0,1000), 0,1000); - //ch4_in = constrain(map(pulseIn(PIN_CH4, HIGH, 25000),1000,2000, 0,1000), 0,1000); - //ch1 = steer (ail) - //ch2 = speed (ele) - //ch3 = speed multiplier (thr) + updateChannels(); //calculate chx_in times from ppm signals. 0 <= chx_in <= 1000 + boolean _ppmOK=ppmOK(); //Potis /*poti1=analogRead(PIN_POTI1); @@ -91,20 +117,148 @@ void loop() { out_steer=0; */ - //out_speed=(int16_t)( (ch2_in*2-1000)*(ch3_in/1000.0) ); - out_speed=(int16_t)( (ch2_in*2-1000)); - out_steer=ch1_in*2-1000; + + if (_ppmOK){ //ppm check failed + //out_speed=(int16_t)( (ch2_in*2-1000)*(ch3_in/1000.0) ); + out_speed=(int16_t)( (ch2_in*2-1000)); + out_steer=ch1_in*2-1000; + }else{ + //out_speed=(int16_t)( (ch2_in*2-1000)*(ch3_in/1000.0) ); + out_speed=0; //stop + out_steer=0; //stop + } + + + + if (flag_ppmupdated){ + flag_ppmupdated=false; //clear flag + digitalWrite(PIN_LED,!digitalRead(PIN_LED)); //toggle led + } + if (millis()-last_send>SENDPERIOD){ Serial2.write((uint8_t *) &out_steer, sizeof(out_steer)); Serial2.write((uint8_t *) &out_speed, sizeof(out_speed)); last_send=millis(); - Serial.print("Steer="); - Serial.println(out_steer); - Serial.print("Speed="); - Serial.println(out_speed); + + #ifdef DEBUG + Serial.print("steer="); + Serial.print(out_steer); + Serial.print(" speed="); + Serial.print(out_speed); + /* + Serial.print(ch1_in); + Serial.print(","); + Serial.print(ch2_in); + Serial.print(","); + Serial.print(ch3_in); + Serial.print(","); + Serial.print(ch4_in); + */ + Serial.print(", ppmOK="); + Serial.print(_ppmOK); + Serial.print(", ppmlag="); + Serial.print(ppmlagmillis); + #endif + + Serial.println(); } } + + + +void updateChannels(){ + long funcmillis=millis(); + //Calculate Pulse Width. (pulseIn values typically between 1000 and 2000). Map and constrain between 0 and 1000 + long new_ch1_in=(t_falling1-t_raising1); + if (new_ch1_in>=PPM_TIME_MIN && new_ch1_in<=PPM_TIME_MAX){ //time valid and has changed + ch1_in=constrain(map(new_ch1_in,1000,2000, 0,1000), 0,1000); + last_updated_ch1=funcmillis; + flag_ppmupdated=true; + } + + long new_ch2_in=(t_falling2-t_raising2); + if (new_ch2_in>=PPM_TIME_MIN && new_ch2_in<=PPM_TIME_MAX){ //time valid and has changed + ch2_in=constrain(map(new_ch2_in,1000,2000, 0,1000), 0,1000); + last_updated_ch2=funcmillis; + flag_ppmupdated=true; + } + + long new_ch3_in=(t_falling3-t_raising3); + if (new_ch3_in>=PPM_TIME_MIN && new_ch3_in<=PPM_TIME_MAX){ //time valid and has changed + ch3_in=constrain(map(new_ch3_in,1000,2000, 0,1000), 0,1000); + last_updated_ch3=funcmillis; + flag_ppmupdated=true; + } + + long new_ch4_in=(t_falling4-t_raising4); + if (new_ch4_in>=PPM_TIME_MIN && new_ch4_in<=PPM_TIME_MAX){ //time valid and has changed + ch4_in=constrain(map(new_ch4_in,1000,2000, 0,1000), 0,1000); + last_updated_ch4=funcmillis; + flag_ppmupdated=true; + } + +} + +boolean ppmOK(){ + long m=millis(); + boolean returnvalue=true; + ppmlagmillis=0; + long v; + v=m-last_updated_ch1; + if (v>ppmlagmillis){ ppmlagmillis=v; } + if (v>MAXPPMUPDATETIME){ + returnvalue=false; + } + v=m-last_updated_ch2; + if (v>ppmlagmillis){ ppmlagmillis=v; } + if (v>MAXPPMUPDATETIME){ + returnvalue=false; + } + v=m-last_updated_ch3; + if (v>ppmlagmillis){ ppmlagmillis=v; } + if (v>MAXPPMUPDATETIME){ + returnvalue=false; + } + v=m-last_updated_ch4; + if (v>ppmlagmillis){ ppmlagmillis=v; } + if (v>MAXPPMUPDATETIME){ + returnvalue=false; + } + return returnvalue; +} + +void ppmchanged1(){ + if (digitalRead(PIN_CH1)){ + t_raising1=micros(); + }else{ + t_falling1=micros(); + } +} + +void ppmchanged2(){ + if (digitalRead(PIN_CH2)){ + t_raising2=micros(); + }else{ + t_falling2=micros(); + } +} + +void ppmchanged3(){ + if (digitalRead(PIN_CH3)){ + t_raising3=micros(); + }else{ + t_falling3=micros(); + } +} + +void ppmchanged4(){ + if (digitalRead(PIN_CH4)){ + t_raising4=micros(); + }else{ + t_falling4=micros(); + } +}