Matthew Marino / Mbed 2 deprecated sailbot_RUDDER

Rudder program

Dependencies:   mbed

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?

UserRevisionLine numberNew 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 }