Matthew Marino / Mbed 2 deprecated sailbot_RUDDER2

rud2

Dependencies:   mbed

main.cpp@0:8ed926c84e2f, 2018-11-28 (annotated)

Committer:
LukeMar
Date:
Wed Nov 28 14:53:46 2018 +0000
Revision:
0:8ed926c84e2f
Child:
1:9e727d54e80c
try

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 0:8ed926c84e2f 33 AnalogIn ain(p18);
LukeMar 0:8ed926c84e2f 34 PwmOut motor( p22 );
LukeMar 0:8ed926c84e2f 35 DigitalOut dir( p21 );
LukeMar 0:8ed926c84e2f 36 DigitalOut I(p23);
LukeMar 0:8ed926c84e2f 37 DigitalOut slp(p30); //sleep
LukeMar 0:8ed926c84e2f 38 float d_ang = 180.0;
LukeMar 0:8ed926c84e2f 39 float pos = 180.0;
LukeMar 0:8ed926c84e2f 40 float RC_O;
LukeMar 0:8ed926c84e2f 41
LukeMar 0:8ed926c84e2f 42 //**get position**
LukeMar 0:8ed926c84e2f 43 float posi(float in);
LukeMar 0:8ed926c84e2f 44
LukeMar 0:8ed926c84e2f 45
LukeMar 0:8ed926c84e2f 46 int main(void)
LukeMar 0:8ed926c84e2f 47 {
LukeMar 0:8ed926c84e2f 48
LukeMar 0:8ed926c84e2f 49 I=0;
LukeMar 0:8ed926c84e2f 50 motor.period(.001);
LukeMar 0:8ed926c84e2f 51 motor.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 0:8ed926c84e2f 58 RC_O = 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 0:8ed926c84e2f 62 slp = 0;
LukeMar 0:8ed926c84e2f 63 }
LukeMar 0:8ed926c84e2f 64 //39.19
LukeMar 0:8ed926c84e2f 65 d_ang = (RC_O/6.77)+39.19;
LukeMar 0:8ed926c84e2f 66
LukeMar 0:8ed926c84e2f 67 pos = (ain-.108)/.002466;
LukeMar 0:8ed926c84e2f 68 //pc.printf(" %.3f\n",);
LukeMar 0:8ed926c84e2f 69 if((pos > (d_ang-3.0)) && (pos < (d_ang+3.0))) {
LukeMar 0:8ed926c84e2f 70 motor.pulsewidth(0);
LukeMar 0:8ed926c84e2f 71 slp = 0;
LukeMar 0:8ed926c84e2f 72 }
LukeMar 0:8ed926c84e2f 73 if( (pos > (d_ang+3.0)) && pos < 270.0) {
LukeMar 0:8ed926c84e2f 74 slp = 1;
LukeMar 0:8ed926c84e2f 75 while(pos > d_ang) {
LukeMar 0:8ed926c84e2f 76 dir = 1; //left??
LukeMar 0:8ed926c84e2f 77 motor.pulsewidth(.0005);
LukeMar 0:8ed926c84e2f 78 pc.printf("pos: %.3f\n",pos);
LukeMar 0:8ed926c84e2f 79 pos = posi(ain);
LukeMar 0:8ed926c84e2f 80 }//while pos
LukeMar 0:8ed926c84e2f 81 motor.pulsewidth(0);
LukeMar 0:8ed926c84e2f 82 }//if pos
LukeMar 0:8ed926c84e2f 83 if((pos < (d_ang-3.0)) && pos > 90.0) {
LukeMar 0:8ed926c84e2f 84 slp = 1;
LukeMar 0:8ed926c84e2f 85 while(pos < d_ang) {
LukeMar 0:8ed926c84e2f 86 dir = 0; //right??
LukeMar 0:8ed926c84e2f 87 motor.pulsewidth(.0005);
LukeMar 0:8ed926c84e2f 88 pc.printf("pos: %.3f\n",pos);
LukeMar 0:8ed926c84e2f 89 pos = posi(ain);
LukeMar 0:8ed926c84e2f 90 }//while pos
LukeMar 0:8ed926c84e2f 91 motor.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 0:8ed926c84e2f 100 float posi(float in)
LukeMar 0:8ed926c84e2f 101 {
LukeMar 0:8ed926c84e2f 102 float p1;
LukeMar 0:8ed926c84e2f 103 float p2;
LukeMar 0:8ed926c84e2f 104 float p3;
LukeMar 0:8ed926c84e2f 105 p1 = (ain-.108)/.002466;
LukeMar 0:8ed926c84e2f 106 p2 = (ain-.108)/.002466;
LukeMar 0:8ed926c84e2f 107 p3 = (ain-.108)/.002466;
LukeMar 0:8ed926c84e2f 108 return (p1+p2+p3)/3.0;
LukeMar 0:8ed926c84e2f 109 }