User Tools

Site Tools


drexel_coax_copter_code

Differences

This shows you the differences between two versions of the page.

Link to this comparison view

drexel_coax_copter_code [2016/11/09 16:32] (current)
dwallace created
Line 1: Line 1:
 +====== 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]]
drexel_coax_copter_code.txt ยท Last modified: 2016/11/09 16:32 by dwallace