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 float r_ang = 180.0;
00039 float r_pos = 180.0;
00040 float RC_1;
00041 
00042 //**get position**
00043 float posr();
00044 
00045 
00046 int main(void)
00047 {
00048 
00049     r_I=0;
00050     rudder.period(.001);
00051     rudder.pulsewidth(0);
00052     // loop
00053     while(1) {
00054         //now = rtos::Kernel::get_ms_count();
00055         //***JUST ADDED stepper code
00056 
00057         if (rx.valid) {
00058             RC_1 = rx.channel[0];
00059             pc.printf(" rc: %f\n",rx.channel[0]);
00060         } else {
00061             pc.printf(" invalid\r\n");
00062             r_slp = 0;
00063         }
00064         //39.19
00065         r_ang = (RC_1/6.77)+39.19;
00066 
00067         r_pos = (r_ain-.108)/.002466;
00068         //pc.printf(" %.3f\n",);
00069         if((r_pos > (r_ang-3.0)) && (r_pos < (r_ang+3.0))) {
00070             rudder.pulsewidth(0);
00071             r_slp = 0;
00072         }
00073         if( (r_pos > (r_ang+3.0)) && r_pos < 270.0) {
00074             r_slp = 1;
00075             while(r_pos > r_ang) {
00076                 r_dir = 1; //left??
00077                 rudder.pulsewidth(.0005);
00078                 pc.printf("pos: %.3f\n",r_pos);
00079                 r_pos = posr();
00080             }//while pos
00081             rudder.pulsewidth(0);
00082         }//if pos
00083         if((r_pos < (r_ang-3.0)) && r_pos > 90.0) {
00084             r_slp = 1;
00085             while(r_pos < r_ang) {
00086                 r_dir = 0; //right??
00087                 rudder.pulsewidth(.0005);
00088                 pc.printf("pos: %.3f\n",r_pos);
00089                 r_pos = posr();
00090             }//while pos
00091             rudder.pulsewidth(0);
00092         }
00093         //***END OF JUST ADDED
00094         ThisThread::sleep_until(121);
00095     }//while(1)
00096 } // main() for TESTS/receiver/simple
00097 
00098 
00099 
00100 float posr()
00101 {
00102     float r1;
00103     float r2;
00104     float r3;
00105     r1 = (r_ain-.108)/.002466;
00106     r2 = (r_ain-.108)/.002466;
00107     r3 = (r_ain-.108)/.002466;
00108     return (r1+r2+r3)/3.0;
00109 }