Syma s107g Helicopter controller

Dependencies:   AccelSensor mbed

Fork of IR_demo by jim hamblen

main.cpp

Committer:
wsmallwood3
Date:
2014-12-02
Revision:
2:2ffc4c0b0b90
Parent:
1:6bdeedba218c

File content as of revision 2:2ffc4c0b0b90:

#include "mbed.h"
#include "AccelSensor.h"
PwmOut IRLED(p21);
AnalogIn UPDOWN(p16);
DigitalOut Flex(LED1);
Serial PC(USBTX, USBRX);
AccelSensor myAccel( p9,p10); //sda and scl

int ROTATION_STATIONARY = 60;
int CAL_BYTE = 52;
int Throttle, LeftRight, FwdBack;
int acceldata[3];

 void PutHeader(void)
{   // This is the pulse and null for the header before each command
IRLED = 0.5;
wait(.001964); //This is how long the pwm is on
IRLED = 0.0;
wait(.001972); //This is how long the pwm is off
}

 void PutFooter(void)
{   // This is the pulse and null for the footer at the end of each command
IRLED = 0.5;
wait(.000305); //Pulse length
IRLED = 0.0;
wait(.088682); //Null length
}

 void PutZero(void)
{   // This is the pulse and null for a digital 0
IRLED = 0.5;
wait(.000305);
IRLED = 0.0;
wait(.000293);
}

void PutOne(void)
{   // This is the pulse and null for a digital 1
IRLED = 0.5;
wait(.000306);
IRLED = 0.0;
wait(0.000687);
}

//This function takes in values for throttle, yaw and pitch and sends out the command.
void sendCommand(int throttle, int leftRight, int forwardBackward)
{
  int b;
  PutHeader();
  //LEFT/RIGHT  control
  for (int i = 7; i >=0; i--)
  {
    b = ((ROTATION_STATIONARY + leftRight) & (1 << i)) >> i;    
    if (b > 0) PutOne(); 
    else PutZero();
  }
  //FORWARD/BACKWARD  control
  for (int i = 7; i >=0; i--)
  {
    b = ((63 + forwardBackward) & (1 << i)) >> i;    
    if (b > 0) PutOne(); 
    else PutZero();
  }
  //THROTTLE  control
  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();
}

int main() {
Throttle = 45; //0-127
LeftRight = 0; //convert to -60 (right) to 60(Left)
FwdBack = 0; //convert to -60(forward) to 60(Backward)
myAccel.init();
myAccel.active();

    IRLED.period(1.0/38000.0); //Drive IR LED data pin with 38Khz PWM Carrier 

    while(1){
    myAccel.readData(acceldata);
    LeftRight = int(-60*acceldata[1]/700); //X-axis
    FwdBack = int(60*acceldata[0]/700);
    if(LeftRight>60){LeftRight=60;}
    if(LeftRight<-60){LeftRight=-60;}
    if(FwdBack>60){FwdBack=60;}
    if(FwdBack<-60){FwdBack=-60;}
    if(UPDOWN>0.5)
        {Throttle = 85;
        Flex=1;}
    else{Throttle=45;
        Flex=0;}
    sendCommand(Throttle, LeftRight, FwdBack);
    PC.printf("Throttle:%d, LR:%d, FB:%d\n\r",Throttle,LeftRight,FwdBack);
    }
}