![](/media/cache/img/default_profile.jpg.50x50_q85.jpg)
Rudder program
main.cpp@1:9e727d54e80c, 2018-11-28 (annotated)
- Committer:
- LukeMar
- Date:
- Wed Nov 28 15:17:04 2018 +0000
- Revision:
- 1:9e727d54e80c
- Parent:
- 0:8ed926c84e2f
RUdder version of sailbot RC code
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
LukeMar | 0:8ed926c84e2f | 1 | /* |
LukeMar | 0:8ed926c84e2f | 2 | Test for manual control thread |
LukeMar | 0:8ed926c84e2f | 3 | Spektrum receiver simple test |
LukeMar | 0:8ed926c84e2f | 4 | |
LukeMar | 0:8ed926c84e2f | 5 | range on transmitter is 344-1564 |
LukeMar | 0:8ed926c84e2f | 6 | to correspond to rudder angle of 90-270 |
LukeMar | 0:8ed926c84e2f | 7 | current eqn to convert RC output is |
LukeMar | 0:8ed926c84e2f | 8 | |
LukeMar | 0:8ed926c84e2f | 9 | (IN/6.77)+39.19 |
LukeMar | 0:8ed926c84e2f | 10 | |
LukeMar | 0:8ed926c84e2f | 11 | NB: The specification for Spektrum Remote Receiver Interfacing, |
LukeMar | 0:8ed926c84e2f | 12 | rev A, 12 Apr 2016, has a note about servo position ranges: |
LukeMar | 0:8ed926c84e2f | 13 | "A full range of servo position data ranges from 0-1024 or 0-2045 depending |
LukeMar | 0:8ed926c84e2f | 14 | on the bind type. These limits are equivalent to a +/-150% travel setting |
LukeMar | 0:8ed926c84e2f | 15 | in AirWare. This full range translates to a range of 1194us which when |
LukeMar | 0:8ed926c84e2f | 16 | applied to a pwm servo signal equals 903-2097us. At +/-100% travel, the |
LukeMar | 0:8ed926c84e2f | 17 | data range is equivalent to a servo position data range of approximately |
LukeMar | 0:8ed926c84e2f | 18 | 341 to 1707 which translated to pwm equals 1102 us to 1898 us." |
LukeMar | 0:8ed926c84e2f | 19 | */ |
LukeMar | 0:8ed926c84e2f | 20 | |
LukeMar | 0:8ed926c84e2f | 21 | #include "mbed.h" |
LukeMar | 0:8ed926c84e2f | 22 | //#include "rtos.h" |
LukeMar | 0:8ed926c84e2f | 23 | #include "Spektrum.h" |
LukeMar | 0:8ed926c84e2f | 24 | #include "unity.h" |
LukeMar | 0:8ed926c84e2f | 25 | //****RC control universal variables**** |
LukeMar | 0:8ed926c84e2f | 26 | Serial pc(USBTX, USBRX, 115200); |
LukeMar | 0:8ed926c84e2f | 27 | Spektrum rx(p13, p14); |
LukeMar | 0:8ed926c84e2f | 28 | char c; |
LukeMar | 0:8ed926c84e2f | 29 | int i; |
LukeMar | 0:8ed926c84e2f | 30 | int j; |
LukeMar | 0:8ed926c84e2f | 31 | float now; |
LukeMar | 0:8ed926c84e2f | 32 | //***Stepper Motor universal variables*** |
LukeMar | 1:9e727d54e80c | 33 | AnalogIn r_ain(p18); |
LukeMar | 1:9e727d54e80c | 34 | PwmOut rudder( p22 ); |
LukeMar | 1:9e727d54e80c | 35 | DigitalOut r_dir( p21 ); |
LukeMar | 1:9e727d54e80c | 36 | DigitalOut r_I(p23); |
LukeMar | 1:9e727d54e80c | 37 | DigitalOut r_slp(p30); //sleep |
LukeMar | 1:9e727d54e80c | 38 | float r_ang = 180.0; |
LukeMar | 1:9e727d54e80c | 39 | float r_pos = 180.0; |
LukeMar | 1:9e727d54e80c | 40 | float RC_1; |
LukeMar | 0:8ed926c84e2f | 41 | |
LukeMar | 0:8ed926c84e2f | 42 | //**get position** |
LukeMar | 1:9e727d54e80c | 43 | float posr(); |
LukeMar | 0:8ed926c84e2f | 44 | |
LukeMar | 0:8ed926c84e2f | 45 | |
LukeMar | 0:8ed926c84e2f | 46 | int main(void) |
LukeMar | 0:8ed926c84e2f | 47 | { |
LukeMar | 0:8ed926c84e2f | 48 | |
LukeMar | 1:9e727d54e80c | 49 | r_I=0; |
LukeMar | 1:9e727d54e80c | 50 | rudder.period(.001); |
LukeMar | 1:9e727d54e80c | 51 | rudder.pulsewidth(0); |
LukeMar | 0:8ed926c84e2f | 52 | // loop |
LukeMar | 0:8ed926c84e2f | 53 | while(1) { |
LukeMar | 0:8ed926c84e2f | 54 | //now = rtos::Kernel::get_ms_count(); |
LukeMar | 0:8ed926c84e2f | 55 | //***JUST ADDED stepper code |
LukeMar | 0:8ed926c84e2f | 56 | |
LukeMar | 0:8ed926c84e2f | 57 | if (rx.valid) { |
LukeMar | 1:9e727d54e80c | 58 | RC_1 = rx.channel[0]; |
LukeMar | 0:8ed926c84e2f | 59 | pc.printf(" rc: %f\n",rx.channel[0]); |
LukeMar | 0:8ed926c84e2f | 60 | } else { |
LukeMar | 0:8ed926c84e2f | 61 | pc.printf(" invalid\r\n"); |
LukeMar | 1:9e727d54e80c | 62 | r_slp = 0; |
LukeMar | 0:8ed926c84e2f | 63 | } |
LukeMar | 0:8ed926c84e2f | 64 | //39.19 |
LukeMar | 1:9e727d54e80c | 65 | r_ang = (RC_1/6.77)+39.19; |
LukeMar | 0:8ed926c84e2f | 66 | |
LukeMar | 1:9e727d54e80c | 67 | r_pos = (r_ain-.108)/.002466; |
LukeMar | 0:8ed926c84e2f | 68 | //pc.printf(" %.3f\n",); |
LukeMar | 1:9e727d54e80c | 69 | if((r_pos > (r_ang-3.0)) && (r_pos < (r_ang+3.0))) { |
LukeMar | 1:9e727d54e80c | 70 | rudder.pulsewidth(0); |
LukeMar | 1:9e727d54e80c | 71 | r_slp = 0; |
LukeMar | 0:8ed926c84e2f | 72 | } |
LukeMar | 1:9e727d54e80c | 73 | if( (r_pos > (r_ang+3.0)) && r_pos < 270.0) { |
LukeMar | 1:9e727d54e80c | 74 | r_slp = 1; |
LukeMar | 1:9e727d54e80c | 75 | while(r_pos > r_ang) { |
LukeMar | 1:9e727d54e80c | 76 | r_dir = 1; //left?? |
LukeMar | 1:9e727d54e80c | 77 | rudder.pulsewidth(.0005); |
LukeMar | 1:9e727d54e80c | 78 | pc.printf("pos: %.3f\n",r_pos); |
LukeMar | 1:9e727d54e80c | 79 | r_pos = posr(); |
LukeMar | 0:8ed926c84e2f | 80 | }//while pos |
LukeMar | 1:9e727d54e80c | 81 | rudder.pulsewidth(0); |
LukeMar | 0:8ed926c84e2f | 82 | }//if pos |
LukeMar | 1:9e727d54e80c | 83 | if((r_pos < (r_ang-3.0)) && r_pos > 90.0) { |
LukeMar | 1:9e727d54e80c | 84 | r_slp = 1; |
LukeMar | 1:9e727d54e80c | 85 | while(r_pos < r_ang) { |
LukeMar | 1:9e727d54e80c | 86 | r_dir = 0; //right?? |
LukeMar | 1:9e727d54e80c | 87 | rudder.pulsewidth(.0005); |
LukeMar | 1:9e727d54e80c | 88 | pc.printf("pos: %.3f\n",r_pos); |
LukeMar | 1:9e727d54e80c | 89 | r_pos = posr(); |
LukeMar | 0:8ed926c84e2f | 90 | }//while pos |
LukeMar | 1:9e727d54e80c | 91 | rudder.pulsewidth(0); |
LukeMar | 0:8ed926c84e2f | 92 | } |
LukeMar | 0:8ed926c84e2f | 93 | //***END OF JUST ADDED |
LukeMar | 0:8ed926c84e2f | 94 | ThisThread::sleep_until(121); |
LukeMar | 0:8ed926c84e2f | 95 | }//while(1) |
LukeMar | 0:8ed926c84e2f | 96 | } // main() for TESTS/receiver/simple |
LukeMar | 0:8ed926c84e2f | 97 | |
LukeMar | 0:8ed926c84e2f | 98 | |
LukeMar | 0:8ed926c84e2f | 99 | |
LukeMar | 1:9e727d54e80c | 100 | float posr() |
LukeMar | 0:8ed926c84e2f | 101 | { |
LukeMar | 1:9e727d54e80c | 102 | float r1; |
LukeMar | 1:9e727d54e80c | 103 | float r2; |
LukeMar | 1:9e727d54e80c | 104 | float r3; |
LukeMar | 1:9e727d54e80c | 105 | r1 = (r_ain-.108)/.002466; |
LukeMar | 1:9e727d54e80c | 106 | r2 = (r_ain-.108)/.002466; |
LukeMar | 1:9e727d54e80c | 107 | r3 = (r_ain-.108)/.002466; |
LukeMar | 1:9e727d54e80c | 108 | return (r1+r2+r3)/3.0; |
LukeMar | 0:8ed926c84e2f | 109 | } |