First of all, I apologize for my bad English. I'm trying to read 6 channel values of my RC transmitter (Flysky FS-TH9X) using the trainer port and then I'm trying to move 6 servos according to the channel values. All these will be done by using a single arduino mega. After searching I got the below sketch from the forum
http://forum.arduino.cc/index.php?topic=7167.0
But this sketch is not related with servo control. According to this sketch, trainer cable is connected to Pin 4 of my mega. It works fine with the mega to read 6 ch value of my RC transmitter. The sketch is little big, so to save time, mainly look at the print portion:
#define channumber 6 //Cuantos canales tiene tu radio???????/How many channels have your radio???
#define filter 10 // Filtro anti salto/ Glitch Filter
int channel[channumber]; //Valores de canales leidos/ readed Channel values
int lastReadChannel[channumber]; //Ultima lectura obtenida/ Last values readed
int conta=0; //Contador/couter
void setup()
{
Serial.begin(9600); //Iniciamos com serial/ Serial Begin
pinMode(4, INPUT); //Patita 4 como entrada / Pin 4 as input
pinMode(13, OUTPUT); // Led pin 13
}
void loop()
{
if(pulseIn(4, HIGH) > 3000) //Si el pulso del pin 4 es > 3000 usegundos continua /If pulse > 3000 useconds, continues
{
for(int i = 0; i <= channumber-1; i++) //lee los pulsos de los canales / Read the pulses of the channels
{
channel[i]=pulseIn(4, HIGH);
}
for(int i = 0; i <= channumber-1; i++) //Promedia los pulsos/Average the pulses
{
if((channel[i] > 2000) || (channel[i] <100))//Si se pasa del rango envia ultimo pulso/ If channel > max range, chage the value to the last pulse
{
channel[i]= lastReadChannel[i];
}
else
{
channel[i]=(lastReadChannel[i]+channel[i])/2; //Promedio el pulso pasado con el nuevo pulso/Average the last pulse eith the current pulse
conta++; //Incrementa el contador/ increment counter
}
}
}
if(conta > filter)//Si el contador es mayor al filtro imprime valores/ If counter is > than filter, then prints values
{
for(int i = 0; i <= channumber-1; i++) //Ciclo para imprimir valores/Cycle to print values
{
Serial.print("CH"); //Canal/Channel
Serial.print(i+1); // Numero del canal / Channel number
Serial.print(": "); // que te importa
Serial.println(channel[i]);
lastReadChannel[i]=channel[i];
}
if(channel[4] > 1000) //si el canal 5 tiene un rango mayor a 500 enciende el LED/ If channel 5 is > than 500 turn on the led
{
digitalWrite(13, HIGH);
}
else
{
digitalWrite(13, LOW);//Si no lo apaga/If not turn it off
}
delay(400); //Delay
conta=0;//Reinicia el contador/ Restart couter.
}
}
Then I'm trying to modify the above sketch to control 6 servos using the same arduino mega according to the 6 channel values. Problem is that, my modified sketch successfully compiles and uploaded to the mega, but then it doesn't read the channels and the servos do not move. My sketch is given below, which is modified from the above sketch. In my sketch I attached the trainer cable at Pin 2. My below sketch has all the lines of the above sketch, plus few lines I added for servo control. Pl look for the big gaps where I added required lines for controlling the 6 servos:
#include <Servo.h>
#define channumber 6 //Cuantos canales tiene tu radio???????/How many channels have your radio???
#define filter 10 // Filtro anti salto/ Glitch Filter
int channel[channumber]; //Valores de canales leidos/ readed Channel values
int lastReadChannel[channumber]; //Ultima lectura obtenida/ Last values readed
int conta=0; //Contador/couter
int value0 = 1082;
int value1 = 1082;
int value2 = 660;
int value3 = 1082;
int value4 = 1082;
int value5 = 1082;
// Create a Servo object for each servo
Servo servo1;
Servo servo2;
Servo servo3;
Servo servo4;
Servo servo5;
Servo servo6;
// TO ADD SERVOS:
// Servo servo7;
// etc...
// Common servo setup values
int minPulse = 660; // minimum servo position, us (microseconds)
int maxPulse = 1505; // maximum servo position, us
void setup()
{
Serial.begin(9600); //Iniciamos com serial/ Serial Begin
pinMode(2, INPUT); //Patita 2 como entrada / Pin 2 as input
pinMode(13, OUTPUT); // Led pin 13
{
servo1.attach(3); // attaches the servo on pin 3 to the servo object
servo2.attach(4); // attaches the servo on pin 4 to the servo object
servo3.attach(5); // attaches the servo on pin 5 to the servo object
servo4.attach(6); // attaches the servo on pin 6 to the servo object
servo5.attach(7); // attaches the servo on pin 7 to the servo object
servo6.attach(8); // attaches the servo on pin 8 to the servo object
}
}
void loop()
{
if(pulseIn(2, HIGH) > 3000) //Si el pulso del pin 2 es > 3000 usegundos continua /If pulse > 3000 useconds, continues
{
for(int i = 0; i <= channumber-1; i++) //lee los pulsos de los canales / Read the pulses of the channels
{
channel[i]=pulseIn(2, HIGH);
}
for(int i = 0; i <= channumber-1; i++) //Promedia los pulsos/Average the pulses
{
if((channel[i] > 2000) || (channel[i] <100))//Si se pasa del rango envia ultimo pulso/ If channel > max range, chage the value to the last pulse
{
channel[i]= lastReadChannel[i];
}
else
{
channel[i]=(lastReadChannel[i]+channel[i])/2; //Promedio el pulso pasado con el nuevo pulso/Average the last pulse eith the current pulse
conta++; //Incrementa el contador/ increment counter
}
}
}
if(conta > filter)//Si el contador es mayor al filtro imprime valores/ If counter is > than filter, then prints values
{
for(int i = 0; i <= channumber-1; i++) //Ciclo para imprimir valores/Cycle to print values
{
Serial.print("CH"); //Canal/Channel
Serial.print(i+1); // Numero del canal / Channel number
Serial.print(": "); // que te importa
Serial.println(channel[i]);
lastReadChannel[i]=channel[i];
}
if(channel[4] > 1000) //si el canal 5 tiene un rango mayor a 500 enciende el LED/ If channel 5 is > than 500 turn on the led
{
digitalWrite(13, HIGH);
}
else
{
digitalWrite(13, LOW);//Si no lo apaga/If not turn it off
}
delay(400); //Delay
conta=0;//Reinicia el contador/ Restart couter.
}
{
value0 = lastReadChannel[0] ;
value0 = map(lastReadChannel[0], minPulse, maxPulse, 0, 90); // scale it to use it with the servo (angle between 0 and 90)
servo1.write(value0);
delay(15);
}
{
value1 = lastReadChannel[1] ;
int value1 = map(lastReadChannel[1], minPulse, maxPulse, 0, 90); // scale it to use it with the servo (angle between 0 and 90)
servo2.write(value1); // sets the servo position according to the scaled value
delay(15);
}
{
value1 = lastReadChannel[2] ;
int value2 = map(lastReadChannel[2], minPulse, maxPulse, 0, 90); // scale it to use it with the servo (angle between 0 and 90)
servo3.write(value2); // sets the servo position according to the scaled value
delay(15);
}
{
value1 = lastReadChannel[3] ;
int value3 = map(lastReadChannel[3], minPulse, maxPulse, 0, 90); // scale it to use it with the servo (angle between 0 and 90)
servo4.write(value3); // sets the servo position according to the scaled value
delay(15);
}
{
value1 = lastReadChannel[4] ;
int value4 = map(lastReadChannel[4], minPulse, maxPulse, 0, 90); // scale it to use it with the servo (angle between 0 and 90)
servo5.write(value4); // sets the servo position according to the scaled value
delay(15);
value1 = lastReadChannel[5] ;
int value5 = map(lastReadChannel[5], minPulse, maxPulse, 0, 90); // scale it to use it with the servo (angle between 0 and 90)
servo6.write(value5); // sets the servo position according to the scaled value
delay(15); // waits for the servo to get there
}
}
My aim is to read the channel values with trainer cable using arduino mega and then to move 6 servos from the same mega according to the channel values. Any help is much appreciated.