Syma s107g Helicopter controller
Dependencies: AccelSensor mbed
Fork of IR_demo by
main.cpp@2:2ffc4c0b0b90, 2014-12-02 (annotated)
- Committer:
- wsmallwood3
- Date:
- Tue Dec 02 10:35:35 2014 +0000
- Revision:
- 2:2ffc4c0b0b90
- Parent:
- 1:6bdeedba218c
IR Helicopter Demo
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
4180_1 | 0:e0bef6b1aa57 | 1 | #include "mbed.h" |
wsmallwood3 | 2:2ffc4c0b0b90 | 2 | #include "AccelSensor.h" |
4180_1 | 0:e0bef6b1aa57 | 3 | PwmOut IRLED(p21); |
wsmallwood3 | 2:2ffc4c0b0b90 | 4 | AnalogIn UPDOWN(p16); |
wsmallwood3 | 2:2ffc4c0b0b90 | 5 | DigitalOut Flex(LED1); |
wsmallwood3 | 2:2ffc4c0b0b90 | 6 | Serial PC(USBTX, USBRX); |
wsmallwood3 | 2:2ffc4c0b0b90 | 7 | AccelSensor myAccel( p9,p10); //sda and scl |
wsmallwood3 | 1:6bdeedba218c | 8 | |
wsmallwood3 | 1:6bdeedba218c | 9 | int ROTATION_STATIONARY = 60; |
wsmallwood3 | 2:2ffc4c0b0b90 | 10 | int CAL_BYTE = 52; |
wsmallwood3 | 1:6bdeedba218c | 11 | int Throttle, LeftRight, FwdBack; |
wsmallwood3 | 2:2ffc4c0b0b90 | 12 | int acceldata[3]; |
wsmallwood3 | 1:6bdeedba218c | 13 | |
wsmallwood3 | 1:6bdeedba218c | 14 | void PutHeader(void) |
wsmallwood3 | 2:2ffc4c0b0b90 | 15 | { // This is the pulse and null for the header before each command |
wsmallwood3 | 1:6bdeedba218c | 16 | IRLED = 0.5; |
wsmallwood3 | 2:2ffc4c0b0b90 | 17 | wait(.001964); //This is how long the pwm is on |
wsmallwood3 | 1:6bdeedba218c | 18 | IRLED = 0.0; |
wsmallwood3 | 2:2ffc4c0b0b90 | 19 | wait(.001972); //This is how long the pwm is off |
wsmallwood3 | 1:6bdeedba218c | 20 | } |
wsmallwood3 | 1:6bdeedba218c | 21 | |
wsmallwood3 | 1:6bdeedba218c | 22 | void PutFooter(void) |
wsmallwood3 | 2:2ffc4c0b0b90 | 23 | { // This is the pulse and null for the footer at the end of each command |
wsmallwood3 | 1:6bdeedba218c | 24 | IRLED = 0.5; |
wsmallwood3 | 2:2ffc4c0b0b90 | 25 | wait(.000305); //Pulse length |
wsmallwood3 | 1:6bdeedba218c | 26 | IRLED = 0.0; |
wsmallwood3 | 2:2ffc4c0b0b90 | 27 | wait(.088682); //Null length |
wsmallwood3 | 1:6bdeedba218c | 28 | } |
wsmallwood3 | 2:2ffc4c0b0b90 | 29 | |
wsmallwood3 | 1:6bdeedba218c | 30 | void PutZero(void) |
wsmallwood3 | 2:2ffc4c0b0b90 | 31 | { // This is the pulse and null for a digital 0 |
wsmallwood3 | 1:6bdeedba218c | 32 | IRLED = 0.5; |
wsmallwood3 | 1:6bdeedba218c | 33 | wait(.000305); |
wsmallwood3 | 1:6bdeedba218c | 34 | IRLED = 0.0; |
wsmallwood3 | 1:6bdeedba218c | 35 | wait(.000293); |
wsmallwood3 | 1:6bdeedba218c | 36 | } |
wsmallwood3 | 1:6bdeedba218c | 37 | |
wsmallwood3 | 1:6bdeedba218c | 38 | void PutOne(void) |
wsmallwood3 | 2:2ffc4c0b0b90 | 39 | { // This is the pulse and null for a digital 1 |
wsmallwood3 | 1:6bdeedba218c | 40 | IRLED = 0.5; |
wsmallwood3 | 1:6bdeedba218c | 41 | wait(.000306); |
wsmallwood3 | 1:6bdeedba218c | 42 | IRLED = 0.0; |
wsmallwood3 | 1:6bdeedba218c | 43 | wait(0.000687); |
wsmallwood3 | 1:6bdeedba218c | 44 | } |
4180_1 | 0:e0bef6b1aa57 | 45 | |
wsmallwood3 | 2:2ffc4c0b0b90 | 46 | //This function takes in values for throttle, yaw and pitch and sends out the command. |
wsmallwood3 | 1:6bdeedba218c | 47 | void sendCommand(int throttle, int leftRight, int forwardBackward) |
wsmallwood3 | 1:6bdeedba218c | 48 | { |
wsmallwood3 | 1:6bdeedba218c | 49 | int b; |
wsmallwood3 | 1:6bdeedba218c | 50 | PutHeader(); |
wsmallwood3 | 2:2ffc4c0b0b90 | 51 | //LEFT/RIGHT control |
wsmallwood3 | 1:6bdeedba218c | 52 | for (int i = 7; i >=0; i--) |
wsmallwood3 | 1:6bdeedba218c | 53 | { |
wsmallwood3 | 1:6bdeedba218c | 54 | b = ((ROTATION_STATIONARY + leftRight) & (1 << i)) >> i; |
wsmallwood3 | 2:2ffc4c0b0b90 | 55 | if (b > 0) PutOne(); |
wsmallwood3 | 2:2ffc4c0b0b90 | 56 | else PutZero(); |
wsmallwood3 | 1:6bdeedba218c | 57 | } |
wsmallwood3 | 2:2ffc4c0b0b90 | 58 | //FORWARD/BACKWARD control |
wsmallwood3 | 1:6bdeedba218c | 59 | for (int i = 7; i >=0; i--) |
wsmallwood3 | 1:6bdeedba218c | 60 | { |
wsmallwood3 | 1:6bdeedba218c | 61 | b = ((63 + forwardBackward) & (1 << i)) >> i; |
wsmallwood3 | 2:2ffc4c0b0b90 | 62 | if (b > 0) PutOne(); |
wsmallwood3 | 2:2ffc4c0b0b90 | 63 | else PutZero(); |
wsmallwood3 | 2:2ffc4c0b0b90 | 64 | } |
wsmallwood3 | 2:2ffc4c0b0b90 | 65 | //THROTTLE control |
wsmallwood3 | 1:6bdeedba218c | 66 | for (int i = 7; i >=0; i--) |
wsmallwood3 | 1:6bdeedba218c | 67 | { |
wsmallwood3 | 2:2ffc4c0b0b90 | 68 | b = (throttle & (1 << i)) >> i; |
wsmallwood3 | 2:2ffc4c0b0b90 | 69 | if (b > 0) PutOne(); |
wsmallwood3 | 2:2ffc4c0b0b90 | 70 | else PutZero(); |
wsmallwood3 | 1:6bdeedba218c | 71 | } |
wsmallwood3 | 1:6bdeedba218c | 72 | |
wsmallwood3 | 1:6bdeedba218c | 73 | for (int i = 7; i >=0; i--) |
wsmallwood3 | 1:6bdeedba218c | 74 | { |
wsmallwood3 | 1:6bdeedba218c | 75 | b = (CAL_BYTE & (1 << i)) >> i; |
wsmallwood3 | 2:2ffc4c0b0b90 | 76 | if (b > 0) PutOne(); |
wsmallwood3 | 2:2ffc4c0b0b90 | 77 | else PutZero(); |
wsmallwood3 | 1:6bdeedba218c | 78 | } |
wsmallwood3 | 1:6bdeedba218c | 79 | PutFooter(); |
wsmallwood3 | 1:6bdeedba218c | 80 | } |
wsmallwood3 | 1:6bdeedba218c | 81 | |
wsmallwood3 | 2:2ffc4c0b0b90 | 82 | int main() { |
wsmallwood3 | 2:2ffc4c0b0b90 | 83 | Throttle = 45; //0-127 |
wsmallwood3 | 2:2ffc4c0b0b90 | 84 | LeftRight = 0; //convert to -60 (right) to 60(Left) |
wsmallwood3 | 2:2ffc4c0b0b90 | 85 | FwdBack = 0; //convert to -60(forward) to 60(Backward) |
wsmallwood3 | 2:2ffc4c0b0b90 | 86 | myAccel.init(); |
wsmallwood3 | 2:2ffc4c0b0b90 | 87 | myAccel.active(); |
wsmallwood3 | 2:2ffc4c0b0b90 | 88 | |
wsmallwood3 | 2:2ffc4c0b0b90 | 89 | IRLED.period(1.0/38000.0); //Drive IR LED data pin with 38Khz PWM Carrier |
wsmallwood3 | 1:6bdeedba218c | 90 | |
wsmallwood3 | 2:2ffc4c0b0b90 | 91 | while(1){ |
wsmallwood3 | 2:2ffc4c0b0b90 | 92 | myAccel.readData(acceldata); |
wsmallwood3 | 2:2ffc4c0b0b90 | 93 | LeftRight = int(-60*acceldata[1]/700); //X-axis |
wsmallwood3 | 2:2ffc4c0b0b90 | 94 | FwdBack = int(60*acceldata[0]/700); |
wsmallwood3 | 2:2ffc4c0b0b90 | 95 | if(LeftRight>60){LeftRight=60;} |
wsmallwood3 | 2:2ffc4c0b0b90 | 96 | if(LeftRight<-60){LeftRight=-60;} |
wsmallwood3 | 2:2ffc4c0b0b90 | 97 | if(FwdBack>60){FwdBack=60;} |
wsmallwood3 | 2:2ffc4c0b0b90 | 98 | if(FwdBack<-60){FwdBack=-60;} |
wsmallwood3 | 2:2ffc4c0b0b90 | 99 | if(UPDOWN>0.5) |
wsmallwood3 | 2:2ffc4c0b0b90 | 100 | {Throttle = 85; |
wsmallwood3 | 2:2ffc4c0b0b90 | 101 | Flex=1;} |
wsmallwood3 | 2:2ffc4c0b0b90 | 102 | else{Throttle=45; |
wsmallwood3 | 2:2ffc4c0b0b90 | 103 | Flex=0;} |
wsmallwood3 | 2:2ffc4c0b0b90 | 104 | sendCommand(Throttle, LeftRight, FwdBack); |
wsmallwood3 | 2:2ffc4c0b0b90 | 105 | PC.printf("Throttle:%d, LR:%d, FB:%d\n\r",Throttle,LeftRight,FwdBack); |
4180_1 | 0:e0bef6b1aa57 | 106 | } |
4180_1 | 0:e0bef6b1aa57 | 107 | } |