====== Co-Axial Helicopter with IR Elevation Control Code ======

===== Code to send IR pulses =====

<code c IRPulses.c>
#include <TimerOne.h>
 
//comment this out to see the demodulated waveform
//it is useful for debugging purpose.
#define MODULATED 1
 
const int IR_PIN = 3;
const int IR_PIN2 = 4;
const int IR_PIN3 = 5;
const unsigned long DURATION = 180000l;
const int HEADER_DURATION = 2000;
const int HIGH_DURATION = 380;
const int ZERO_LOW_DURATION = 220;
const int ONE_LOW_DURATION = 600;
const byte ROTATION_STATIONARY = 60;
const byte CAL_BYTE = 52;
 
int Throttle, LeftRight, FwdBack;
 
void sendHeader()
{
  #ifndef MODULATED
    digitalWrite(IR_PIN, HIGH);
    digitalWrite(IR_PIN2, HIGH);
    digitalWrite(IR_PIN3, HIGH);
  #else
    TCCR2A |= _BV(COM2B1);
  #endif
   
  delayMicroseconds(HEADER_DURATION);
   
  #ifndef MODULATED
    digitalWrite(IR_PIN, LOW);
    digitalWrite(IR_PIN2, LOW);
    digitalWrite(IR_PIN3, LOW);
  #else
    TCCR2A &= ~_BV(COM2B1);
  #endif
   
  delayMicroseconds(HEADER_DURATION);
   
  #ifndef MODULATED
    digitalWrite(IR_PIN, HIGH);
    digitalWrite(IR_PIN2, HIGH);
    digitalWrite(IR_PIN3, HIGH);
  #else
    TCCR2A |= _BV(COM2B1);
  #endif
   
  delayMicroseconds(HIGH_DURATION);
   
  #ifndef MODULATED
    digitalWrite(IR_PIN, LOW);
    digitalWrite(IR_PIN2, LOW);
    digitalWrite(IR_PIN3, LOW);
  #else
    TCCR2A &= ~_BV(COM2B1);
  #endif
}
 
void sendZero()
{
  delayMicroseconds(ZERO_LOW_DURATION);
 
  #ifndef MODULATED
    digitalWrite(IR_PIN, HIGH);
    digitalWrite(IR_PIN2, HIGH);
    digitalWrite(IR_PIN3, HIGH);
  #else 
    TCCR2A |= _BV(COM2B1);
  #endif
   
  delayMicroseconds(HIGH_DURATION);
   
  #ifndef MODULATED
    digitalWrite(IR_PIN, LOW);
    digitalWrite(IR_PIN2, LOW);
    digitalWrite(IR_PIN3, LOW);
  #else
    TCCR2A &= ~_BV(COM2B1);
  #endif
}
 
void sendOne()
{
  delayMicroseconds(ONE_LOW_DURATION);
   
  #ifndef MODULATED
    digitalWrite(IR_PIN, HIGH);
    digitalWrite(IR_PIN2, HIGH);
    digitalWrite(IR_PIN3, HIGH);
  #else
    TCCR2A |= _BV(COM2B1);
  #endif
   
  delayMicroseconds(HIGH_DURATION);
   
  #ifndef MODULATED
    digitalWrite(IR_PIN, LOW); 
    digitalWrite(IR_PIN2, LOW);
   digitalWrite(IR_PIN3, LOW);  
  #else
    TCCR2A &= ~_BV(COM2B1);
  #endif
}
 
void sendCommand(int throttle, int leftRight, int forwardBackward)
{
  byte b;
 
  sendHeader();
   
  for (int i = 7; i >=0; i--)
  {
    b = ((ROTATION_STATIONARY + leftRight) & (1 << i)) >> i;   
    if (b > 0) sendOne(); else sendZero();
  }
   
  for (int i = 7; i >=0; i--)
  {
    b = ((63 + forwardBackward) & (1 << i)) >> i;   
    if (b > 0) sendOne(); else sendZero();
  }
   
  for (int i = 7; i >=0; i--)
  {
    b = (throttle & (1 << i)) >> i;   
    if (b > 0) sendOne(); else sendZero();
  }
   
  for (int i = 7; i >=0; i--)
  {
    b = (CAL_BYTE & (1 << i)) >> i;   
    if (b > 0) sendOne(); else sendZero();
  }
}
 
void setup()
{
  pinMode(IR_PIN, OUTPUT);
  digitalWrite(IR_PIN, LOW);
  
  pinMode(IR_PIN2, OUTPUT);
  digitalWrite(IR_PIN2, LOW);
  
  pinMode(IR_PIN3, OUTPUT);
  digitalWrite(IR_PIN3, LOW);
 
  //setup interrupt interval: 180ms 
  Timer1.initialize(DURATION);
  Timer1.attachInterrupt(timerISR);
   
  //setup PWM: f=38Khz PWM=0.5 
  byte v = 8000 / 38;
  TCCR2A = _BV(WGM20);
  TCCR2B = _BV(WGM22) | _BV(CS20);
  OCR2A = v;
  OCR2B = v / 2;
}
 
void loop()
{ 
    
}
 
void timerISR()
{
  //read control values from potentiometers
  Throttle = analogRead(0);
  LeftRight = analogRead(1);
  FwdBack = analogRead(2);
   
  Throttle = 100;//Throttle / 4; //convert to 0 to 255
  LeftRight = 0;//LeftRight / 8 - 64; //convert to -64 to 63
  FwdBack = 0;//FwdBack / 4 - 128; //convert to -128 to 127
   
  sendCommand(Throttle, LeftRight, FwdBack);
}
</code>

===== Code to get Ultrasonic readings =====

<code c Ultrasonic.c>
#include <i2cmaster.h>

byte clockPin = 4;
byte buf[9];//Buffer to store the received valeus
byte addr = 0x02;//address 0x02 in a 8-bit context - 0x01 in a 7-bit context
byte distance;

void setup()
{  
  i2c_init();//I2C frequency = 11494,253Hz
  Serial.begin(115200);
  printUltrasonicCommand(0x00);//Read Version
  printUltrasonicCommand(0x08);//Read Product ID
  printUltrasonicCommand(0x10);//Read Sensor Type
  printUltrasonicCommand(0x14);//Read Measurement Units  
}

void loop()
{    
//  printUltrasonicCommand(0x42);//Read Measurement Byte 0
  distance = readDistance();
  if(distance == 0xFF)
    Serial.println("Error Reading Distance");
  else
    Serial.println(distance, DEC);    
}
byte readDistance()
{  
  delay(100);//There has to be a delay between commands
  byte cmd = 0x42;//Read Measurement Byte 0
   
  pinMode(clockPin, INPUT);//Needed for writing to work
  digitalWrite(clockPin, HIGH);  
 
  if(i2c_start(addr+I2C_WRITE))//Check if there is an error
  {
    Serial.println("ERROR i2c_start");
    i2c_stop();
    return 0xFF;
  }    
  if(i2c_write(cmd))//Check if there is an error
  {
    Serial.println("ERROR i2c_write");
    i2c_stop();
    return 0xFF;
  }  
  i2c_stop();
   
  delayMicroseconds(60);//Needed for receiving to work
  pinMode(clockPin, OUTPUT);
  digitalWrite(clockPin, LOW);
  delayMicroseconds(34);
  pinMode(clockPin, INPUT);
  digitalWrite(clockPin, HIGH);
  delayMicroseconds(60);  
   
  if(i2c_rep_start(addr+I2C_READ))//Check if there is an error
  {
    Serial.println("ERROR i2c_rep_start");
    i2c_stop();    
    return 0xFF;
  }  
  for(int i = 0; i < 8; i++)
    buf[i] = i2c_readAck();
  buf[8] = i2c_readNak();  
  i2c_stop();
 
  return buf[0];  
}

void printUltrasonicCommand(byte cmd)
{
  delay(100);//There has to be a delay between commands
   
  pinMode(clockPin, INPUT);//Needed for writing to work
  digitalWrite(clockPin, HIGH);
 
  if(i2c_start(addr+I2C_WRITE))//Check if there is an error
  {
    Serial.println("ERROR i2c_start");
    i2c_stop();
    return;
  }
  if(i2c_write(cmd))//Check if there is an error
  {
    Serial.println("ERROR i2c_write");
    i2c_stop();
    return;
  }
  i2c_stop();
   
  delayMicroseconds(60);//Needed for receiving to work
  pinMode(clockPin, OUTPUT);
  digitalWrite(clockPin, LOW);
  delayMicroseconds(34);
  pinMode(clockPin, INPUT);
  digitalWrite(clockPin, HIGH);
  delayMicroseconds(60);  

  if(i2c_rep_start(addr+I2C_READ))//Check if there is an error
  {
    Serial.println("ERROR i2c_rep_start");
    i2c_stop();
    return;
  }
  for(int i = 0; i < 8; i++)
    buf[i] = i2c_readAck();
  buf[8] = i2c_readNak();
  i2c_stop();  
 
  if(cmd == 0x00 || cmd == 0x08 || cmd == 0x10 || cmd == 0x14)
  {
      for(int i = 0; i < 9; i++)
      {
        if(buf[i] != 0xFF && buf[i] != 0x00)
          Serial.print(buf[i]);
        else
          break;
      }              
  }
  else
    Serial.print(buf[0], DEC);  

  Serial.println("");      
}
/*
' Wires on NXT jack plug.
' Wire colours may vary. Pin 1 is always end nearest latch.
' 1 White +9V
' 2 Black GND
' 3 Red GND
' 4 Green +5V
' 5 Yellow SCL - also connect clockpin to give a extra low impuls
' 6 Blue SDA
' Do not use i2c pullup resistor - already provided within sensor.
*/
</code>

===== Final Code =====

<code c Final.c>
#include <GP2D02.h>
 
//comment this out to see the demodulated waveform
//it is useful for debugging purpose.
#define MODULATED 1
 
//Pins
const int IR_PIN = 3; //Infrared LED pin
const int vIn = 8; //Green Wire of GP2D02
const int vOut = 9; //Yellow Wire of GP2D02
 
//For signal generation. Lengths are in microseconds.
const int HEADER_DURATION = 2000; //Length of header pulse
const int HIGH_DURATION = 380; //Length of high part of 0/1-pulse
const int ZERO_LOW_DURATION = 220; //Length of low part of 0-pulse
const int ONE_LOW_DURATION = 600; //Length of low part of 1-pulse
const byte ROTATION_STATIONARY = 60; // Trim
const byte CAL_BYTE = 52; //Trailing pulse signature
 
//For fitness calculation
int heightCM = 12; //Input the height you want the program to attempt to reach
int heightIRvalue = 447.54*pow(heightCM,-0.407); // Conver CM to the scale that the GP2D02 outputs at
int bestFitness = 1000; // starting best fitness is far from zero
int currentFitness = 1000; // starting fitness is far from zero
int runNum = 0; //Tracks number of runs
int currentIRValue = 0; //Store GP2D02 IRValue
int minIR = 0; //Minimum position IR Value
int maxIR = 0; //Maximum position IR Value
int lastHighThrottle = 255; // Starts last fit high throttle at max range
int lastLowThrottle = 0; // Starts last low high throttle at min range
int lastThrottle = 0; // The value of the throttle on the last run
 
//For timed loop of one run
int starttime = 0; 
int endtime = 0; 
 
//For intial SYMA 107 signal parameters
int Throttle = 0;
int LeftRight = 0;//-64 to 63
int FwdBack = 0;//-128 to 127
 
//Initialize the IR sensor
GP2D02 sensor(vIn,vOut);
 
void sendHeader() //Sends the header pulse that singals the begining of a command
{
  #ifndef MODULATED
    digitalWrite(IR_PIN, HIGH);
  #else
    TCCR2A |= _BV(COM2B1);
  #endif
 
  delayMicroseconds(HEADER_DURATION);
 
  #ifndef MODULATED
    digitalWrite(IR_PIN, LOW);
  #else
    TCCR2A &= ~_BV(COM2B1);
  #endif
 
  delayMicroseconds(HEADER_DURATION);
 
  #ifndef MODULATED
    digitalWrite(IR_PIN, HIGH);
  #else
    TCCR2A |= _BV(COM2B1);
  #endif
 
  delayMicroseconds(HIGH_DURATION);
 
  #ifndef MODULATED
    digitalWrite(IR_PIN, LOW);
  #else
    TCCR2A &= ~_BV(COM2B1);
  #endif
}
 
void sendZero() //Sends one 0 pulse
{
  delayMicroseconds(ZERO_LOW_DURATION);
 
  #ifndef MODULATED
    digitalWrite(IR_PIN, HIGH);
  #else 
    TCCR2A |= _BV(COM2B1);
  #endif
 
  delayMicroseconds(HIGH_DURATION);
 
  #ifndef MODULATED
    digitalWrite(IR_PIN, LOW);
  #else
    TCCR2A &= ~_BV(COM2B1);
  #endif
}
 
void sendOne() //Sends one 1 pulse
{
  delayMicroseconds(ONE_LOW_DURATION);
 
  #ifndef MODULATED
    digitalWrite(IR_PIN, HIGH);
  #else
    TCCR2A |= _BV(COM2B1);
  #endif
 
  delayMicroseconds(HIGH_DURATION);
 
  #ifndef MODULATED
    digitalWrite(IR_PIN, LOW);
  #else
    TCCR2A &= ~_BV(COM2B1);
  #endif
}
 
void sendCommand(int throttle, int leftRight, int forwardBackward)
{ //Converts SYMA Parameters to a signal
  //For loops convert decimal numbers to binary numbers
  //Sends a pulse for each 0 or 1.
  byte b;
 
  sendHeader();
 
  for (int i = 7; i >=0; i--) 
  { // Yaw part of signal
    b = ((ROTATION_STATIONARY + leftRight) & (1 << i)) >> i;   
    if (b > 0) sendOne(); else sendZero();
  }
 
  for (int i = 7; i >=0; i--)
  { //Pitch part of signal
    b = ((63 + forwardBackward) & (1 << i)) >> i;   
    if (b > 0) sendOne(); else sendZero();
  }
 
  for (int i = 7; i >=0; i--)
  { //Throttle part of signal
    b = (throttle & (1 << i)) >> i;   
    if (b > 0) sendOne(); else sendZero();
  }
 
  for (int i = 7; i >=0; i--)
  { //Trailing Pulses
    b = (CAL_BYTE & (1 << i)) >> i;   
    if (b > 0) sendOne(); else sendZero();
  }
}
 
void setup()
{ //Initializes the program
  Serial.begin(9600); //Baud rate for command line
  pinMode(IR_PIN, OUTPUT); //Initializes the LED pin
  digitalWrite(IR_PIN, LOW);
 
  //setup PWM: f=38Khz PWM=0.5 
  byte v = 8000 / 38;
  TCCR2A = _BV(WGM20);
  TCCR2B = _BV(WGM22) | _BV(CS20);
  OCR2A = v;
  OCR2B = v / 2;
  minIR = sensor.getRawRange(); //get lowest point IR reading
  maxIR = 3000000*pow(minIR,-2.409)+11; //Convert to centimeters and add range of height of boom in cm
  maxIR = 447.54*pow(maxIR,-0.407); //Convert to scaled IR value
  Serial.println("Program Start");
  Serial.print("Height to reach: ");
  Serial.println(heightIRvalue);
}
 
void loop() //main program loop
{ 
  randomSeed(millis());
  runNum = runNum + 1;
  if(runNum>1){ //Runs after first
  Serial.println("runNum>1");
    if(bestFitness>5){ //if last fitness was high then new random is a smaller number
      Throttle = random(lastThrottle,lastHighThrottle);//0 to 255
      lastLowThrottle = lastThrottle;
      Serial.println("bF>5");
    }
    else if(bestFitness<-5){ //if last fitness was low then new random is larger number
      Throttle = random(lastLowThrottle,lastThrottle);
      lastHighThrottle = lastThrottle;
       Serial.println("bF>5");
    }
    else{ // 0 is the best fitness, this is the final part of the program
        Serial.println("\nFinal Values");
        Serial.print("Throttle: ");
        Serial.println(lastThrottle);
        Serial.print("Number of Runs: ");
        Serial.println(runNum);
      while(1){
        sendCommand(lastThrottle, LeftRight, FwdBack); //generates IR signal
        //end of program
        //hangs indefinately
      }
    }
  }
  else{ //Initial Run
  Serial.println("runNum1");
    Throttle = random(255);
  }
  Serial.print("\nNumber of Runs: ");
  Serial.println(runNum);
  Serial.print("Throttle: ");
  Serial.println(Throttle);
  
  lastThrottle = Throttle;
  starttime = millis();
  endtime = starttime;
  // do this loop for up to 5000mS
  while ((endtime - starttime) <=5000) 
  { //Runs take place over a period of 5 seconds
    sendCommand(Throttle, LeftRight, FwdBack); //generates IR signal
    endtime = millis();
    currentIRValue = sensor.getRawRange();
    currentFitness = currentIRValue-heightIRvalue;
  }
  if(abs(currentFitness)<abs(bestFitness)){
    bestFitness = currentFitness; //If new fitness is closer to zero, replace old fitness
  }
  Serial.print("Current IR Value: ");
  Serial.println(currentIRValue);
  Serial.print("Current Fitness: ");
  Serial.println(bestFitness);
  delay(3000); //wait to lower to lowest position.
}

</code>

===== Libraries =====

  * [[http://code.google.com/p/arduino-timerone/downloads/list|TimerOne.h]]
  * [[https://github.com/jtrollinson/ArduinoGP2D02|GP2D02.h]]