main.cpp

00001 /*
00002   Test for manual control thread
00003   Spektrum receiver simple test
00004 
00005   range on transmitter is 344-1564
00006   to correspond to rudder angle of 90-270
00007   current eqn to convert RC output is
00008 
00009             (IN/6.77)+39.19
00010 
00011   NB: The specification for Spektrum Remote Receiver Interfacing,
00012   rev A, 12 Apr 2016, has a note about servo position ranges:
00013   "A full range of servo position data ranges from 0-1024 or 0-2045 depending
00014   on the bind type. These limits are equivalent to a +/-150% travel setting
00015   in AirWare. This full range translates to a range of 1194us which when
00016   applied to a pwm servo signal equals 903-2097us. At +/-100% travel, the
00017   data range is equivalent to a servo position data range of approximately
00018   341 to 1707 which translated to pwm equals 1102 us to 1898 us."
00019 */
00020 
00021 #include "mbed.h"
00022 //#include "rtos.h"
00023 #include "Spektrum.h"
00024 #include "unity.h"
00025 //****RC control universal variables****
00026 Serial pc(USBTX, USBRX, 115200);
00027 Spektrum rx(p13, p14);
00028 char c;
00029 int i;
00030 int j;
00031 float now;
00032 //***Stepper Motor universal variables***
00033 AnalogIn   r_ain(p18);
00034 PwmOut  rudder( p22 );
00035 DigitalOut   r_dir( p21 );
00036 DigitalOut    r_I(p23);
00037 DigitalOut r_slp(p30); //sleep
00038 DigitalOut led(LED1);
00039 float r_ang;
00040 float r_pos;
00041 float RC_1;
00042 
00043 //**get position**
00044 float posr();
00045 
00046 
00047 int main(void)
00048 {
00049     
00050     r_I=0;
00051     rudder.period(.001);
00052     rudder.pulsewidth(0);
00053     r_slp = 0;
00054     wait(10);
00055     led = 1; //indicate that the wait period is over
00056     
00057     // loop
00058     while(1) {
00059         //now = rtos::Kernel::get_ms_count();
00060         //***JUST ADDED stepper code
00061 
00062         if (rx.valid) {
00063             RC_1 = rx.channel[1];
00064             pc.printf(" rc: %f\n",rx.channel[0]);
00065         } else {
00066             pc.printf(" invalid\r\n");
00067             r_slp = 0;
00068         }
00069         //39.19
00070         r_ang = (RC_1/6.77)+30.0;
00071 
00072         r_pos = (r_ain-.108)/.002466;
00073         //pc.printf(" %.3f\n",);
00074         if((r_pos > (r_ang-3.0)) && (r_pos < (r_ang+3.0))) {
00075             rudder.pulsewidth(0);
00076             r_slp = 0;
00077         }
00078         if( (r_pos > (r_ang+3.0)) && r_pos < 270.0) {
00079             r_slp = 1;
00080             while(r_pos > r_ang) {
00081                 r_dir = 1; //left??
00082                 rudder.pulsewidth(.0005);
00083                 pc.printf("pos: %.3f\n",r_pos);
00084                 r_pos = posr();
00085             }//while pos
00086             rudder.pulsewidth(0);
00087         }//if pos
00088         if((r_pos < (r_ang-3.0)) && r_pos > 90.0) {
00089             r_slp = 1;
00090             while(r_pos < r_ang) {
00091                 r_dir = 0; //right??
00092                 rudder.pulsewidth(.0005);
00093                 pc.printf("pos: %.3f\n",r_pos);
00094                 r_pos = posr();
00095             }//while pos
00096             rudder.pulsewidth(0);
00097         }
00098         //***END OF JUST ADDED
00099         ThisThread::sleep_until(121);
00100     }//while(1)
00101 } // main() for TESTS/receiver/simple
00102 
00103 
00104 
00105 float posr()
00106 {
00107     float r1;
00108     float r2;
00109     float r3;
00110     r1 = (r_ain-.108)/.002466;
00111     r2 = (r_ain-.108)/.002466;
00112     r3 = (r_ain-.108)/.002466;
00113     return (r1+r2+r3)/3.0;
00114 }