Syma s107g Helicopter controller

Dependencies:   AccelSensor mbed

Fork of IR_demo by jim hamblen

main.cpp

Committer:
wsmallwood3
Date:
2014-11-26
Revision:
1:6bdeedba218c
Parent:
0:e0bef6b1aa57
Child:
2:2ffc4c0b0b90

File content as of revision 1:6bdeedba218c:

//http://www.kerrywong.com/2012/08/27/reverse-engineering-the-syma-s107g-ir-protocol/
#include "mbed.h"
 
PwmOut IRLED(p21);
//IR send and receive demo

int ROTATION_STATIONARY = 60;
int CAL_BYTE = 52; 
 
int Throttle, LeftRight, FwdBack;



 void PutHeader(void)
{   // CODE HERE WILL RUN WHEN INTERUPT IS GENERATED 
IRLED = 0.5;
wait(.001964);
IRLED = 0.0;
wait(.001972);
}

 void PutFooter(void)
{   // CODE HERE WILL RUN WHEN INTERUPT IS GENERATED 
IRLED = 0.5;
wait(.000305);
IRLED = 0.0;
wait(.088682);
}
 
 void PutZero(void)
{   // CODE HERE WILL RUN WHEN INTERUPT IS GENERATED 
IRLED = 0.5;
wait(.000305);
IRLED = 0.0;
wait(.000293);
}

void PutOne(void)
{   // CODE HERE WILL RUN WHEN INTERUPT IS GENERATED 
IRLED = 0.5;
wait(.000306);
IRLED = 0.0;
wait(0.000687);
}

void sendCommand(int throttle, int leftRight, int forwardBackward)
{
  int b;
 
  PutHeader();
   
  for (int i = 7; i >=0; i--)
  {
    b = ((ROTATION_STATIONARY + leftRight) & (1 << i)) >> i;    
    if (b > 0) PutOne(); else PutZero();
  }
   
  for (int i = 7; i >=0; i--)
  {
    b = ((63 + forwardBackward) & (1 << i)) >> i;    
    if (b > 0) PutOne(); else PutZero();
  } 
   
  for (int i = 7; i >=0; i--)
  {
    b = (throttle & (1 << i)) >> i;    
    if (b > 0) PutOne(); else PutZero();
  }
   
  for (int i = 7; i >=0; i--)
  {
    b = (CAL_BYTE & (1 << i)) >> i;    
    if (b > 0) PutOne(); else PutZero();
  } 
  
  PutFooter();
  
}

void timerISR()
{
  //read control values from potentiometers
  //Throttle = analogRead(0);
  //LeftRight = analogRead(1);
  //FwdBack = analogRead(2);
   
  //Throttle = Throttle / 4; //convert to 0 to 255
  Throttle = 65;
  //LeftRight = LeftRight / 8 - 64; //convert to -64 to 63
  LeftRight = 0; //convert to -64 to 63
  //FwdBack = FwdBack / 4 - 128; //convert to -128 to 127
  FwdBack = 0; //convert to -128 to 127
   
  sendCommand(Throttle, LeftRight, FwdBack);
}

 
void Thrust(void) //Test function to test flying.
{
    //Header
    PutHeader();
    //First Byte 
    PutZero(); //1
    PutOne(); //2
    PutZero(); //3
    PutZero(); //4
    PutOne(); //5
    PutOne(); //6
    PutZero(); //7
    PutZero(); //8
    //Second Byte 
    PutZero(); //1
    PutZero(); //2
    PutOne(); //3
    PutOne(); //4
    PutOne(); //5
    PutOne(); //6
    PutOne(); //7
    PutOne(); //8
    //Third Byte 
    PutZero(); //1
    PutOne(); //2
    PutZero(); //3
    PutZero(); //4
    PutZero(); //5
    PutOne(); //6
    PutOne(); //7
    PutOne(); //8
    //Fourth Byte 
    PutZero(); //1
    PutOne(); //2
    PutOne(); //3
    PutZero(); //4
    PutOne(); //5
    PutOne(); //6
    PutOne(); //7
    PutZero(); //8
    //Footer
    PutFooter();
} 



 
int main() {
    //IR Transmit code
    IRLED.period(1.0/38000.0);
    //Drive IR LED data pin with 38Khz PWM Carrier 
    while(1) {
    //Thrust();
      timerISR();
    
    }
}