source: orbit/arduino/puzzlebox_orbit_Uno_V3/puzzlebox_orbit_Uno_V3.ino @ 076a101

Servo
Last change on this file since 076a101 was a9059a5, checked in by Steve Castellotti <sc@…>, 10 years ago

Arduino:

  • updated to permit custom setting of Throttle, Yaw, and Pitch

Android:

  • Minor tweak to Configuration location

iOS:

  • Property mode set to 100644
File size: 6.3 KB
RevLine 
[f0fb814]1/*
2
[a9059a5]3Mimics the fade example but with an extra parameter for frequency. It should dim but with a flicker
4because the frequency has been set low enough for the human eye to detect. This flicker is easiest to see when
5the LED is moving with respect to the eye and when it is between about 20% - 60% brighness. The library
6allows for a frequency range from 1Hz - 2MHz on 16 bit timers and 31Hz - 2 MHz on 8 bit timers. When
7SetPinFrequency()/SetPinFrequencySafe() is called, a bool is returned which can be tested to verify the
8frequency was actually changed.
9
10This example runs on mega and uno.
11*/
[f0fb814]12
13#include <PWM.h>
14
[a9059a5]15// use pin 11 on the Mega instead, otherwise there is a frequency cap at 31 Hz
16int IR = 10;  // the pin that the Infrared LED is attached to
17int ON = 128; // 17.5us/26us = 172.3/256
[f0fb814]18int OFF = 0;
[a9059a5]19int32_t frequency = 38400; // frequency (in Hz)
[f0fb814]20
[a9059a5]21// variables start with "_" means global variable
22char _throttle=0; // 32~127, default 0
23char _pitch=31;   // 0~63, default 31
24char _yaw=78;     // 16~127, default 78
[f0fb814]25char _channel='A';
26char _command;
[a9059a5]27// long _mainCode=0;
28
[f0fb814]29
[a9059a5]30// generate ON/OFF control signals, with starting and stopping PWM generator
[f0fb814]31void innerCycle(int onTime, int offTime)
32{
[a9059a5]33pwmWrite(IR, ON);
34delayMicroseconds(onTime);
35pwmWrite(IR, OFF);
36delayMicroseconds(offTime);
37} // innerCycle
38
[f0fb814]39
[a9059a5]40void emitCode(char BIT) // emitCode generate LOWs between HIGHs as same as the parameter.
[f0fb814]41{
[a9059a5]42        if (BIT) innerCycle(671,732); // 1
43        else innerCycle(337,402); // 0
44} // emitCode
45
[f0fb814]46
47void sendCode(long code)
48{
[a9059a5]49        char n;
50        //starting code, with special time period.
51        innerCycle(730,392); //(773 414)->824 450
52        innerCycle(730,392);
53       
54        for (n=28;n>=0;n--)  emitCode((code >> n) & 1); // getting bits out from code
55       
56} // sendCode
[f0fb814]57
58
59long formCode(char throttle,char yaw,char pitch)
60{
[a9059a5]61        char n;
62        long mainCode=0;
63        int checkSum=0;
64       
65        // throttle
66        for (n=6; n>=0; n--)  bitWrite(mainCode,17+n,bitRead(throttle,n)); // getting the first 7 digits to mainCode
67       
68        bitWrite(mainCode,16,1); // meaning unclear, possibly left button.
69       
70        //channel selection first half
71        if (_channel=='C') bitWrite(mainCode,15,1);
72        else bitWrite(mainCode,15,0); // this digit equals 0 in channel A or B
73       
74        for (n=6; n>=0; n--)  bitWrite(mainCode,8+n,bitRead(yaw,n)); // yaw
75       
76        //channel selection second half
77        if (_channel=='A') bitWrite(mainCode,7,1);
78        else bitWrite(mainCode,7,0); // if channel B or C, this digit equals 0;
79       
80        bitWrite(mainCode,6,0); // meaning unclear, possibly right button.
81       
82        for (n=5; n>=0; n--)  bitWrite(mainCode,n,bitRead(pitch,n)); // pitch 
83       
84        // CheckSum
85        for (n=0; n<=20; n=n+4)  checkSum += ((mainCode >> n) & B1111); // sum up every 4 digits in the code
86       
87        checkSum=checkSum & B1111; // get the last 4 bits of the sum
88        checkSum=(16-checkSum) & B1111; // 16-sum is the formula of this helicopter
89       
90        mainCode= (mainCode << 5) | (checkSum << 1); // get the last 4 digit of CheckSum
91       
92        bitWrite(mainCode,0,1); // finish code
93        return mainCode; // mainCode is a 29 bit binary number
94       
95} // formCode
96
[f0fb814]97
98void setup()
99{
[a9059a5]100        pinMode(13, OUTPUT);
101        digitalWrite(13,LOW);
102        //initialize all timers except for 0, to save time keeping functions
103        InitTimersSafe();
104
105        //sets the frequency for the specified pin
106        bool success = SetPinFrequencySafe(IR, frequency);
107
108        //if the pin frequency was set successfully, turn pin 13 on
109        if(success) {
110                digitalWrite(13, HIGH);   
111        }
112       
113        Serial.begin(9600);
114       
115} // setup
116
117
118void setPitch() {
119
120        char inByte=0;
121        int a=0;
122        int b=0;
123        int c=0;
124        int newPitch=0;
125
126        while (Serial.available() == 0);
127        inByte = Serial.read() - '0';
128        //Serial.println(inByte);
129        a = inByte;
130
131        while (Serial.available() == 0);
132        inByte = Serial.read() - '0';
133        //Serial.println(inByte);
134        b = inByte;
135
136        while (Serial.available() == 0);
137        inByte = Serial.read() - '0';
138        //Serial.println(inByte);
139        c = inByte;
140
141        newPitch = (a * 100) + (b * 10) + c;
142
143        if (newPitch < 0)
144                newPitch=0;
145
146        if (newPitch > 100)
147                newPitch=100;
148               
149        _pitch=newPitch;
150
151        Serial.print("_pitch=");
152        Serial.println(int(_pitch));
153
154} // setPitch
155
[f0fb814]156
[ace9e89]157void setThrottle() {
[a9059a5]158
159        char inByte=0;
160        int a=0;
161        int b=0;
162        int c=0;
163        int newThrottle=0;
164
165        while (Serial.available() == 0);
166        inByte = Serial.read() - '0';
167        //Serial.println(inByte);
168        a = inByte;
169
170        while (Serial.available() == 0);
171        inByte = Serial.read() - '0';
172        //Serial.println(inByte);
173        b = inByte;
174
175        while (Serial.available() == 0);
176        inByte = Serial.read() - '0';
177        //Serial.println(inByte);
178        c = inByte;
179
180        newThrottle = (a * 100) + (b * 10) + c;
181
182        if (newThrottle < 0)
183                newThrottle=0;
184
185        if (newThrottle > 100)
186                newThrottle=100;
187               
188        _throttle=newThrottle;
189
190        Serial.print("_throttle=");
191        Serial.println(int(_throttle));
192
193} // setThrottle
194
195
196void setYaw() {
197
198        char inByte=0;
199        int a=0;
200        int b=0;
201        int c=0;
202        int newYaw=0;
203
204        while (Serial.available() == 0);
205        inByte = Serial.read() - '0';
206        //Serial.println(inByte);
207        a = inByte;
208
209        while (Serial.available() == 0);
210        inByte = Serial.read() - '0';
211        //Serial.println(inByte);
212        b = inByte;
213
214        while (Serial.available() == 0);
215        inByte = Serial.read() - '0';
216        //Serial.println(inByte);
217        c = inByte;
218
219        newYaw = (a * 100) + (b * 10) + c;
220
221        if (newYaw < 0)
222                newYaw=0;
223
224        if (newYaw > 100)
225                newYaw=100;
226               
227        _yaw=newYaw;
228
229        Serial.print("_yaw=");
230        Serial.println(int(_yaw));
231
232} // setYaw
233
[ace9e89]234
[f0fb814]235void loop()
[a9059a5]236{
237        int i;
238        if (Serial.available() > 0)
239        {
240                _command = Serial.read();
241                switch (_command)
242                {
243                        case 'P': _throttle=85; Serial.print("_throttle="); Serial.println(int(_throttle)); break;
244                        case 'O': _throttle=0; Serial.print("_throttle="); Serial.println(int(_throttle)); break;
245                        case 'U':  _throttle+=1;  Serial.print("_throttle="); Serial.println(int(_throttle)); break;
246                        case 'D':  _throttle-=1;  Serial.print("_throttle="); Serial.println(int(_throttle)); break;
247                        case 'L':  _yaw+=15;  Serial.print("_yaw="); Serial.println(int(_yaw)); break;
248                        case 'R':  _yaw-=15;  Serial.print("_yaw="); Serial.println(int(_yaw)); break;
249                        case 'F':  _pitch+=5;  Serial.print("_pitch="); Serial.println(int(_pitch)); break;
250                        case 'B':  _pitch-=5;  Serial.print("_pitch="); Serial.println(int(_pitch)); break;
251                        case '1':  _channel='A';  Serial.println("_channel=A"); break;
252                        case '2':  _channel='B';  Serial.println("_channel=B"); break;
253                        case '3':  _channel='C';  Serial.println("_channel=C"); break;
254                        case 'p':  setPitch(); break;
255                        case 't':  setThrottle(); break;
256                        case 'y':  setYaw(); break;
257                        case 'x':  setThrottle(); break;
258                }
259        }
260       
261        sendCode(formCode(_throttle,_yaw,_pitch));
262        delay(80); //originally 36, 51, 66, 80
263       
264} // loop
[1a53973]265
Note: See TracBrowser for help on using the repository browser.