Relay switch

Author: michalpelka

servo/servo.ino

@@ -2,20 +2,25 @@
     #define in1 6
     #define in2 7
     #define button 4
+    #define RELAY 12
     #define CH1 3
+    #define CH2 2    
     #define RANGE 1000
 
     void setup() {
-      pinMode(enA, OUTPUT);
-      pinMode(in1, OUTPUT);
-      pinMode(in2, OUTPUT);
+      pinMode(enA,    OUTPUT);
+      pinMode(in1,    OUTPUT);
+      pinMode(in2,    OUTPUT);
+      pinMode(RELAY,  OUTPUT);
       pinMode(button, INPUT);
-      pinMode(CH1, INPUT);
+      pinMode(CH1,    INPUT);
+      pinMode(CH2,    INPUT);
       TCCR1B = TCCR1B & B11111000 | B00000001;
       // Set initial rotation direction
       digitalWrite(in1, LOW);
       digitalWrite(in2, HIGH);
       attachInterrupt(digitalPinToInterrupt(CH1), ch1_interrupt, CHANGE);
+      attachInterrupt(digitalPinToInterrupt(CH2), ch2_interrupt, CHANGE);
 
       Serial.begin(9600);
     }
@@ -25,26 +30,26 @@
     int oldCh1;
 
 
-    volatile unsigned int oldCh;
-    volatile unsigned int microsStart=0;
-    volatile unsigned int readout =0; 
+    volatile unsigned int oldCh[2];
+    volatile unsigned int microsStart[2]={0,0};
+    volatile unsigned int readout[2]={0,0}; 
     volatile bool isPPMOk = false;
     volatile unsigned int valid_frames = 0;
 
     void ch1_interrupt() {
       volatile int ch = digitalRead(CH1);
-      if (ch==1 && oldCh==0)
+      if (ch==1 && oldCh[0]==0)
       {
-        microsStart = micros(); 
+        microsStart[0] = micros(); 
       }
-      if (ch==0 && oldCh==1)
+      if (ch==0 && oldCh[0]==1)
       {
         //Faling
-        readout = micros()- microsStart;
+        readout[0] = micros()- microsStart[0];
         valid_frames ++;
 
       }
-      if (readout > 2500)
+      if (readout[0] > 2500)
       {
         valid_frames = 0;
         isPPMOk = false;
@@ -56,16 +61,31 @@
           isPPMOk = true;
         }
       }
-     
-      oldCh = ch;
+      oldCh[0] = ch;
+    }
+    void ch2_interrupt()
+    {
+      volatile int ch = digitalRead(CH2);
+      if (ch==1 && oldCh[1]==0)
+      {
+        microsStart[1] = micros(); 
+      }
+      if (ch==0 && oldCh[1]==1)
+      {
+        //Faling
+        readout[1] = micros()- microsStart[1];
+      }
+
+      oldCh[1] = ch;
+      
     }
 
     int lastSetpoint = 0;
 
     char buffer [50];
     void loop() {
       // input PPM smoothing
-      int setPoint = map(readout, 1000, 2000, -RANGE, RANGE);
+      int setPoint = map(readout[0], 1000, 2000, -RANGE, RANGE);
       if (setPoint > RANGE) setPoint = RANGE;
       if (setPoint < -RANGE) setPoint = -RANGE;
 
@@ -101,9 +121,7 @@
         //Serial.println(pwmOutput);
         analogWrite(enA, pwmOutput); // Send PWM signal to L298N Enable pin
         // Read button - Debounce
-  
-      
-  
+    
         //int i=sprintf (buffer, "%d %d \n", setPoint,input );
         //for(int l= 0; l<=i; l++)
         //  Serial.print(buffer[l]);
@@ -124,10 +142,19 @@
         digitalWrite(in2, LOW);
         digitalWrite(in1, LOW);
       }
+
+      // Relay
+      if(readout[1]>1850 and readout[1] < 3000)
+      {
+         digitalWrite(RELAY, LOW);
+      }else
+      {
+         digitalWrite(RELAY, HIGH);
+      }
 
       // logging
 
-      int i=sprintf (buffer, "%d %d %d %d %d, %d \n",readout, setPoint,setPointSmooth, output,  input, isPPMOk);
+      int i=sprintf (buffer, "%d %d %d %d %d %d, %d \n",readout[0],readout[1], setPoint,setPointSmooth, output,  input, isPPMOk);
       for(int l= 0; l<=i; l++)
         Serial.print(buffer[l]);