#include <OrangutanMotors.h>

#define CH1 7 // ELE - FWD/REV
#define CH2 4 // AIL - LEFT/RIGHT
#define LED 1
#define rcMin 1000
#define rcMax 2000
#define MIXING 1

OrangutanMotors motors; 
unsigned long input2;
unsigned long input1;

void setup()
{
  pinMode(CH1,INPUT);
  pinMode(CH2,INPUT);
  pinMode(LED, OUTPUT);
  
}

void loop()
{
  input2 = pulseIn(CH2, HIGH) ;
  input1 = pulseIn(CH1, HIGH) ;
 
  if (input2 != 0 && input1 != 0)
  {
	int M1 = (input1 - rcMin) -  (rcMax - rcMin)/2;
	int M2 = (input2 - rcMin) -  (rcMax - rcMin)/2;
   
  if(MIXING) 
	{ 
	   long temp = M1 - M2; 
	   if (temp > 250) 
	   { 
		 temp = 255; 
	   } 
	   else if (temp < -250) 
	   { 
		 temp = -255; 
	   } 
		
	   long temp2 = M1 + M2; 
	   if (temp2 > 250) 
	   { 
		 temp2 = 255; 
	   } 
	   else if (temp2 < -250)
	   { 
		 temp2 = -255; 
	   } 
		
	   M1 = temp; 
	   M2 = temp2; 
	}    
	 

   motors.setSpeeds(M1,M2);
  }
}