main.cpp

00001 #include "mbed.h"
00002 #include "AccelSensor.h"
00003 PwmOut IRLED(p21);
00004 AnalogIn UPDOWN(p16);
00005 DigitalOut Flex(LED1);
00006 Serial PC(USBTX, USBRX);
00007 AccelSensor myAccel( p9,p10); //sda and scl
00008 
00009 int ROTATION_STATIONARY = 60;
00010 int CAL_BYTE = 52;
00011 int Throttle, LeftRight, FwdBack;
00012 int acceldata[3];
00013 
00014  void PutHeader(void)
00015 {   // This is the pulse and null for the header before each command
00016 IRLED = 0.5;
00017 wait(.001964); //This is how long the pwm is on
00018 IRLED = 0.0;
00019 wait(.001972); //This is how long the pwm is off
00020 }
00021 
00022  void PutFooter(void)
00023 {   // This is the pulse and null for the footer at the end of each command
00024 IRLED = 0.5;
00025 wait(.000305); //Pulse length
00026 IRLED = 0.0;
00027 wait(.088682); //Null length
00028 }
00029 
00030  void PutZero(void)
00031 {   // This is the pulse and null for a digital 0
00032 IRLED = 0.5;
00033 wait(.000305);
00034 IRLED = 0.0;
00035 wait(.000293);
00036 }
00037 
00038 void PutOne(void)
00039 {   // This is the pulse and null for a digital 1
00040 IRLED = 0.5;
00041 wait(.000306);
00042 IRLED = 0.0;
00043 wait(0.000687);
00044 }
00045 
00046 //This function takes in values for throttle, yaw and pitch and sends out the command.
00047 void sendCommand(int throttle, int leftRight, int forwardBackward)
00048 {
00049   int b;
00050   PutHeader();
00051   //LEFT/RIGHT  control
00052   for (int i = 7; i >=0; i--)
00053   {
00054     b = ((ROTATION_STATIONARY + leftRight) & (1 << i)) >> i;    
00055     if (b > 0) PutOne(); 
00056     else PutZero();
00057   }
00058   //FORWARD/BACKWARD  control
00059   for (int i = 7; i >=0; i--)
00060   {
00061     b = ((63 + forwardBackward) & (1 << i)) >> i;    
00062     if (b > 0) PutOne(); 
00063     else PutZero();
00064   }
00065   //THROTTLE  control
00066   for (int i = 7; i >=0; i--)
00067   {
00068     b = (throttle & (1 << i)) >> i;
00069     if (b > 0) PutOne(); 
00070     else PutZero();
00071   }
00072    
00073   for (int i = 7; i >=0; i--)
00074   {
00075     b = (CAL_BYTE & (1 << i)) >> i;    
00076     if (b > 0) PutOne(); 
00077     else PutZero();
00078   } 
00079   PutFooter();
00080 }
00081 
00082 int main() {
00083 Throttle = 45; //0-127
00084 LeftRight = 0; //convert to -60 (right) to 60(Left)
00085 FwdBack = 0; //convert to -60(forward) to 60(Backward)
00086 myAccel.init();
00087 myAccel.active();
00088 
00089     IRLED.period(1.0/38000.0); //Drive IR LED data pin with 38Khz PWM Carrier 
00090 
00091     while(1){
00092     myAccel.readData(acceldata);
00093     LeftRight = int(-60*acceldata[1]/700); //X-axis
00094     FwdBack = int(60*acceldata[0]/700);
00095     if(LeftRight>60){LeftRight=60;}
00096     if(LeftRight<-60){LeftRight=-60;}
00097     if(FwdBack>60){FwdBack=60;}
00098     if(FwdBack<-60){FwdBack=-60;}
00099     if(UPDOWN>0.5)
00100         {Throttle = 85;
00101         Flex=1;}
00102     else{Throttle=45;
00103         Flex=0;}
00104     sendCommand(Throttle, LeftRight, FwdBack);
00105     PC.printf("Throttle:%d, LR:%d, FB:%d\n\r",Throttle,LeftRight,FwdBack);
00106     }
00107 }