Matthew Marino / Mbed 2 deprecated FINAL_SAILBOT_CODE

JAN 16 2019

Dependencies:   mbed BNO055 MBed_Adafruit-GPS-Library

main.cpp@0:f4edd3407cc5, 2019-01-17 (annotated)

Committer:
LukeMar
Date:
Thu Jan 17 00:28:07 2019 +0000
Revision:
0:f4edd3407cc5
Child:
1:4b4d5d18fc57
JAN-16-19

Who changed what in which revision?

UserRevisionLine numberNew contents of line
LukeMar 0:f4edd3407cc5 1
LukeMar 0:f4edd3407cc5 2 /*
LukeMar 0:f4edd3407cc5 3 Sailbot Hull 14
LukeMar 0:f4edd3407cc5 4 L Marino and D Robinson,
LukeMar 0:f4edd3407cc5 5 with D Evangelista, P Frontera, M Kutzer, A Laun
LukeMar 0:f4edd3407cc5 6 2018
LukeMar 0:f4edd3407cc5 7
LukeMar 0:f4edd3407cc5 8 Threaded version using mbed OS5
LukeMar 0:f4edd3407cc5 9 Serial links between nodes
LukeMar 0:f4edd3407cc5 10 Stepper motors with Pololu 2968 drivers (MP6500 based)
LukeMar 0:f4edd3407cc5 11 Piher PST360 angle sensors
LukeMar 0:f4edd3407cc5 12 Adafruit Absolute GPS
LukeMar 0:f4edd3407cc5 13 Telemetry via ???Ubiquity???
LukeMar 0:f4edd3407cc5 14 */
LukeMar 0:f4edd3407cc5 15
LukeMar 0:f4edd3407cc5 16 //Thread for logging time and Gps, heading and encoder values is mostly complete
LukeMar 0:f4edd3407cc5 17 //control thread is a work in progress
LukeMar 0:f4edd3407cc5 18
LukeMar 0:f4edd3407cc5 19 #include "mbed.h"
LukeMar 0:f4edd3407cc5 20 #include "MBed_Adafruit_GPS.h"
LukeMar 0:f4edd3407cc5 21 #include "BNO055.h"
LukeMar 0:f4edd3407cc5 22 #include "Spektrum.h"
LukeMar 0:f4edd3407cc5 23 #include "unity.h"
LukeMar 0:f4edd3407cc5 24
LukeMar 0:f4edd3407cc5 25 Serial pc(USBTX, USBRX, 115200);
LukeMar 0:f4edd3407cc5 26 Spektrum rx(p13, p14);
LukeMar 0:f4edd3407cc5 27 int i;
LukeMar 0:f4edd3407cc5 28 int j;
LukeMar 0:f4edd3407cc5 29 float now;
LukeMar 0:f4edd3407cc5 30 //***Mast universal variables***
LukeMar 0:f4edd3407cc5 31 AnalogIn ain(p16);
LukeMar 0:f4edd3407cc5 32 PwmOut motor( p25 );
LukeMar 0:f4edd3407cc5 33 DigitalOut dir( p24 );
LukeMar 0:f4edd3407cc5 34 DigitalOut I(p26);
LukeMar 0:f4edd3407cc5 35 DigitalOut slp(p29); //sleep
LukeMar 0:f4edd3407cc5 36 float d_ang = 180.0;
LukeMar 0:f4edd3407cc5 37 float pos = 180.0;
LukeMar 0:f4edd3407cc5 38 float RC_O;
LukeMar 0:f4edd3407cc5 39 //***rudder universal variables***
LukeMar 0:f4edd3407cc5 40 AnalogIn r_ain(p15);
LukeMar 0:f4edd3407cc5 41 PwmOut rudder( p22 );
LukeMar 0:f4edd3407cc5 42 DigitalOut r_dir( p21 );
LukeMar 0:f4edd3407cc5 43 DigitalOut r_I(p23);
LukeMar 0:f4edd3407cc5 44 DigitalOut r_slp(p30); //sleep
LukeMar 0:f4edd3407cc5 45 float r_ang;
LukeMar 0:f4edd3407cc5 46 float r_pos;
LukeMar 0:f4edd3407cc5 47 float RC_1;
LukeMar 0:f4edd3407cc5 48
LukeMar 0:f4edd3407cc5 49 float xx = 4.0; //changes the threshold that the motor goes to sleep on
LukeMar 0:f4edd3407cc5 50 float zz = 88; //changes the wait time at end of the the mast rudder threads
LukeMar 0:f4edd3407cc5 51 float ww = 10; //changes wait time at end of if statments in mast rudder threads
LukeMar 0:f4edd3407cc5 52 //****universal variables for Telemetry****
LukeMar 0:f4edd3407cc5 53 DigitalOut heartbeat(LED1);
LukeMar 0:f4edd3407cc5 54 Serial * gps_Serial;
LukeMar 0:f4edd3407cc5 55 BNO055 bno(p28, p27);
LukeMar 0:f4edd3407cc5 56 char c; //when read via Adafruit_GPS::read(), the class returns single character stored here
LukeMar 0:f4edd3407cc5 57 float info; //a multiuse variable used for sending things over GPS
LukeMar 0:f4edd3407cc5 58 Timer refresh_Timer; //sets up a timer for use in loop; how often do we print GPS info?
LukeMar 0:f4edd3407cc5 59 const int refresh_Time = 2000; //refresh time in ms
LukeMar 0:f4edd3407cc5 60
LukeMar 0:f4edd3407cc5 61 union Float { //slightly mysterious data type that we use to send data over serial
LukeMar 0:f4edd3407cc5 62 float m_float;
LukeMar 0:f4edd3407cc5 63 uint8_t m_bytes[sizeof(float)];
LukeMar 0:f4edd3407cc5 64 };
LukeMar 0:f4edd3407cc5 65 float data;
LukeMar 0:f4edd3407cc5 66 uint8_t bytes[sizeof(float)];
LukeMar 0:f4edd3407cc5 67 Float myFloat;
LukeMar 0:f4edd3407cc5 68
LukeMar 0:f4edd3407cc5 69 //**get position**
LukeMar 0:f4edd3407cc5 70 float posi();
LukeMar 0:f4edd3407cc5 71 float posr();
LukeMar 0:f4edd3407cc5 72
LukeMar 0:f4edd3407cc5 73 //*********read reads yaw from IMU **************
LukeMar 0:f4edd3407cc5 74 void yaw_read()
LukeMar 0:f4edd3407cc5 75 {
LukeMar 0:f4edd3407cc5 76 bno.get_angles();
LukeMar 0:f4edd3407cc5 77 data = bno.euler.yaw;
LukeMar 0:f4edd3407cc5 78 myFloat.m_float = data;
LukeMar 0:f4edd3407cc5 79 data = myFloat.m_float; // get the float back from the union
LukeMar 0:f4edd3407cc5 80 pc.printf("%.1f", data);
LukeMar 0:f4edd3407cc5 81 Thread::wait(35);
LukeMar 0:f4edd3407cc5 82
LukeMar 0:f4edd3407cc5 83 }
LukeMar 0:f4edd3407cc5 84
LukeMar 0:f4edd3407cc5 85 //********concatenate time (not currently in use)**********
LukeMar 0:f4edd3407cc5 86 float conc(float hr, float min, float sec )
LukeMar 0:f4edd3407cc5 87 {
LukeMar 0:f4edd3407cc5 88 return hr+(min/60.0)+(sec/3600.0);
LukeMar 0:f4edd3407cc5 89 }
LukeMar 0:f4edd3407cc5 90 //********Initialize IMU************
LukeMar 0:f4edd3407cc5 91
LukeMar 0:f4edd3407cc5 92 void bno_init(void)
LukeMar 0:f4edd3407cc5 93 {
LukeMar 0:f4edd3407cc5 94 if(bno.check()) {
LukeMar 0:f4edd3407cc5 95 //pc.printf("BNO055 connected\r\n");
LukeMar 0:f4edd3407cc5 96 bno.reset();
LukeMar 0:f4edd3407cc5 97 Thread::wait(30);
LukeMar 0:f4edd3407cc5 98 bno.setmode(OPERATION_MODE_CONFIG);
LukeMar 0:f4edd3407cc5 99 bno.SetExternalCrystal(1);
LukeMar 0:f4edd3407cc5 100 //bno.set_orientation(1);
LukeMar 0:f4edd3407cc5 101 bno.setmode(OPERATION_MODE_NDOF); //Uses magnetometer
LukeMar 0:f4edd3407cc5 102 //bno.setmode(OPERATION_MODE_NDOF_FMC_OFF); //no magnetometer
LukeMar 0:f4edd3407cc5 103 bno.set_angle_units(RADIANS);
LukeMar 0:f4edd3407cc5 104 } else {
LukeMar 0:f4edd3407cc5 105 //pc.printf("BNO055 NOT connected\r\n Program Trap.");
LukeMar 0:f4edd3407cc5 106 while(1);
LukeMar 0:f4edd3407cc5 107 }
LukeMar 0:f4edd3407cc5 108 }
LukeMar 0:f4edd3407cc5 109
LukeMar 0:f4edd3407cc5 110 // *****threading*****
LukeMar 0:f4edd3407cc5 111 Thread telemetry_thread;
LukeMar 0:f4edd3407cc5 112 Thread mast_thread;
LukeMar 0:f4edd3407cc5 113 Thread rudder_thread;
LukeMar 0:f4edd3407cc5 114
LukeMar 0:f4edd3407cc5 115 void telemetry_callback(void);
LukeMar 0:f4edd3407cc5 116 void mast_callback(void);
LukeMar 0:f4edd3407cc5 117 void rudder_callback(void);
LukeMar 0:f4edd3407cc5 118
LukeMar 0:f4edd3407cc5 119 int main()
LukeMar 0:f4edd3407cc5 120 {
LukeMar 0:f4edd3407cc5 121 //start all threads
LukeMar 0:f4edd3407cc5 122 wait(10);
LukeMar 0:f4edd3407cc5 123 //telemetry_thread.start(callback(telemetry_callback));
LukeMar 0:f4edd3407cc5 124 mast_thread.start(callback(mast_callback));
LukeMar 0:f4edd3407cc5 125 rudder_thread.start(callback(rudder_callback));
LukeMar 0:f4edd3407cc5 126 //could have a heart beat but I tried to reduce the number of threads fighting for time
LukeMar 0:f4edd3407cc5 127 }//end main
LukeMar 0:f4edd3407cc5 128
LukeMar 0:f4edd3407cc5 129 float posi()
LukeMar 0:f4edd3407cc5 130 {
LukeMar 0:f4edd3407cc5 131 float p1;
LukeMar 0:f4edd3407cc5 132 float p2;
LukeMar 0:f4edd3407cc5 133 float p3;
LukeMar 0:f4edd3407cc5 134 p1 = (ain-.108)/.002466;
LukeMar 0:f4edd3407cc5 135 Thread::wait(3);
LukeMar 0:f4edd3407cc5 136 p2 = (ain-.108)/.002466;
LukeMar 0:f4edd3407cc5 137 Thread::wait(3);
LukeMar 0:f4edd3407cc5 138 p3 = (ain-.108)/.002466;
LukeMar 0:f4edd3407cc5 139 printf("%f\n",ain);
LukeMar 0:f4edd3407cc5 140 return (p1+p2+p3)/3.0;
LukeMar 0:f4edd3407cc5 141 }
LukeMar 0:f4edd3407cc5 142 float posr()
LukeMar 0:f4edd3407cc5 143 {
LukeMar 0:f4edd3407cc5 144 float r1;
LukeMar 0:f4edd3407cc5 145 float r2;
LukeMar 0:f4edd3407cc5 146 float r3;
LukeMar 0:f4edd3407cc5 147 r1 = (r_ain-.108)/.002466;
LukeMar 0:f4edd3407cc5 148 Thread::wait(3);
LukeMar 0:f4edd3407cc5 149 r2 = (r_ain-.108)/.002466;
LukeMar 0:f4edd3407cc5 150 Thread::wait(3);
LukeMar 0:f4edd3407cc5 151 r3 = (r_ain-.108)/.002466;
LukeMar 0:f4edd3407cc5 152 return (r1+r2+r3)/3.0;
LukeMar 0:f4edd3407cc5 153 }
LukeMar 0:f4edd3407cc5 154
LukeMar 0:f4edd3407cc5 155 void telemetry_callback(void)
LukeMar 0:f4edd3407cc5 156 {
LukeMar 0:f4edd3407cc5 157 //uint64_t now;
LukeMar 0:f4edd3407cc5 158 //debug("telemetry_thread started\r\n");
LukeMar 0:f4edd3407cc5 159
LukeMar 0:f4edd3407cc5 160 pc.baud(115200); //sets virtual COM serial communication to high rate; this is to allow more time to be spent on GPS retrieval
LukeMar 0:f4edd3407cc5 161 gps_Serial = new Serial(p9,p10); //serial object for use w/ GPS
LukeMar 0:f4edd3407cc5 162 Adafruit_GPS myGPS(gps_Serial); //object of Adafruit's GPS class
LukeMar 0:f4edd3407cc5 163
LukeMar 0:f4edd3407cc5 164 myGPS.begin(9600); //sets baud rate for GPS communication; note this may be changed via Adafruit_GPS::sendCommand(char *)
LukeMar 0:f4edd3407cc5 165 //a list of GPS commands is available at http://www.adafruit.com/datasheets/PMTK_A08.pdf
LukeMar 0:f4edd3407cc5 166
LukeMar 0:f4edd3407cc5 167 myGPS.sendCommand(PMTK_SET_NMEA_OUTPUT_RMCGGA); //these commands are defined in MBed_Adafruit_GPS.h; a link is provided there for command creation
LukeMar 0:f4edd3407cc5 168 myGPS.sendCommand(PMTK_SET_NMEA_UPDATE_1HZ);
LukeMar 0:f4edd3407cc5 169 myGPS.sendCommand(PGCMD_ANTENNA);
LukeMar 0:f4edd3407cc5 170
LukeMar 0:f4edd3407cc5 171 //pc.printf("Connection established at 115200 baud...\n");
LukeMar 0:f4edd3407cc5 172 bno_init();
LukeMar 0:f4edd3407cc5 173
LukeMar 0:f4edd3407cc5 174 Thread::wait(1000);
LukeMar 0:f4edd3407cc5 175 refresh_Timer.start(); //starts the clock on the timer
LukeMar 0:f4edd3407cc5 176
LukeMar 0:f4edd3407cc5 177 while(true) {
LukeMar 0:f4edd3407cc5 178
LukeMar 0:f4edd3407cc5 179 c = myGPS.read(); //queries the GPS
LukeMar 0:f4edd3407cc5 180 bno.get_angles();
LukeMar 0:f4edd3407cc5 181 //check if we recieved a new message from GPS, if so, attempt to parse it,
LukeMar 0:f4edd3407cc5 182 if ( myGPS.newNMEAreceived() ) {
LukeMar 0:f4edd3407cc5 183 if ( !myGPS.parse(myGPS.lastNMEA()) ) {
LukeMar 0:f4edd3407cc5 184 continue;
LukeMar 0:f4edd3407cc5 185 }//if ( !myGPS.parse..
LukeMar 0:f4edd3407cc5 186
LukeMar 0:f4edd3407cc5 187 if (refresh_Timer.read_ms() >= refresh_Time) {
LukeMar 0:f4edd3407cc5 188 refresh_Timer.reset();
LukeMar 0:f4edd3407cc5 189
LukeMar 0:f4edd3407cc5 190 data = conc(myGPS.hour, myGPS.minute, myGPS.seconds ); // conc(myGPS.hour, myGPS.minute, myGPS.seconds );
LukeMar 0:f4edd3407cc5 191 myFloat.m_float = data;
LukeMar 0:f4edd3407cc5 192 data = myFloat.m_float; // get the float back from the union
LukeMar 0:f4edd3407cc5 193 pc.printf("%2.0f", data);
LukeMar 0:f4edd3407cc5 194 Thread::wait(35);
LukeMar 0:f4edd3407cc5 195 /* this part can send hr min second separately, commented to save thread space
LukeMar 0:f4edd3407cc5 196 data = myGPS.hour; // conc(myGPS.hour, myGPS.minute, myGPS.seconds );
LukeMar 0:f4edd3407cc5 197 myFloat.m_float = data;
LukeMar 0:f4edd3407cc5 198 data = myFloat.m_float; // get the float back from the union
LukeMar 0:f4edd3407cc5 199 pc.printf("%2.0f", data);
LukeMar 0:f4edd3407cc5 200 Thread::wait(35);
LukeMar 0:f4edd3407cc5 201 //send min
LukeMar 0:f4edd3407cc5 202 data = myGPS.minute; // conc(myGPS.hour, myGPS.minute, myGPS.seconds );
LukeMar 0:f4edd3407cc5 203 myFloat.m_float = data;
LukeMar 0:f4edd3407cc5 204 data = myFloat.m_float; // get the float back from the union
LukeMar 0:f4edd3407cc5 205 pc.printf("%2.0f", data);
LukeMar 0:f4edd3407cc5 206 Thread::wait(35);
LukeMar 0:f4edd3407cc5 207 //send sec
LukeMar 0:f4edd3407cc5 208 data = myGPS.seconds; // conc(myGPS.hour, myGPS.minute, myGPS.seconds );
LukeMar 0:f4edd3407cc5 209 myFloat.m_float = data;
LukeMar 0:f4edd3407cc5 210 data = myFloat.m_float; // get the float back from the union
LukeMar 0:f4edd3407cc5 211 pc.printf("%2.0f", data);
LukeMar 0:f4edd3407cc5 212 Thread::wait(35);
LukeMar 0:f4edd3407cc5 213 */
LukeMar 0:f4edd3407cc5 214 if (myGPS.fix) {
LukeMar 0:f4edd3407cc5 215 //rudder and mast position will log here
LukeMar 0:f4edd3407cc5 216 //send mast angle
LukeMar 0:f4edd3407cc5 217 data = posi();
LukeMar 0:f4edd3407cc5 218 myFloat.m_float = data;
LukeMar 0:f4edd3407cc5 219 data = myFloat.m_float; // get the float back from the union
LukeMar 0:f4edd3407cc5 220 pc.printf("%.1f", data);
LukeMar 0:f4edd3407cc5 221 Thread::wait(35);
LukeMar 0:f4edd3407cc5 222 //send rudder angle
LukeMar 0:f4edd3407cc5 223 data = posr();
LukeMar 0:f4edd3407cc5 224 myFloat.m_float = data;
LukeMar 0:f4edd3407cc5 225 data = myFloat.m_float; // get the float back from the union
LukeMar 0:f4edd3407cc5 226 pc.printf("%.1f", data);
LukeMar 0:f4edd3407cc5 227 Thread::wait(35);
LukeMar 0:f4edd3407cc5 228 //send latitude
LukeMar 0:f4edd3407cc5 229 data = myGPS.latitude;
LukeMar 0:f4edd3407cc5 230 myFloat.m_float = data;
LukeMar 0:f4edd3407cc5 231 data = myFloat.m_float; // get the float back from the union
LukeMar 0:f4edd3407cc5 232 pc.printf("%.2f", data);
LukeMar 0:f4edd3407cc5 233 Thread::wait(35);
LukeMar 0:f4edd3407cc5 234 //send logitude
LukeMar 0:f4edd3407cc5 235 data = myGPS.longitude;
LukeMar 0:f4edd3407cc5 236 myFloat.m_float = data;
LukeMar 0:f4edd3407cc5 237 data = myFloat.m_float; // get the float back from the union
LukeMar 0:f4edd3407cc5 238 pc.printf("%.2f", data);
LukeMar 0:f4edd3407cc5 239 Thread::wait(35);
LukeMar 0:f4edd3407cc5 240 //send yaw
LukeMar 0:f4edd3407cc5 241 yaw_read();
LukeMar 0:f4edd3407cc5 242 //send transmittion
LukeMar 0:f4edd3407cc5 243 pc.printf("q");
LukeMar 0:f4edd3407cc5 244 Thread::wait(3000);
LukeMar 0:f4edd3407cc5 245 }//if (myGPS.fix)
LukeMar 0:f4edd3407cc5 246 }//if (refresh_Timer..
LukeMar 0:f4edd3407cc5 247 }//if ( myGPS.newNMEAreceived() )
LukeMar 0:f4edd3407cc5 248 }//while(true)
LukeMar 0:f4edd3407cc5 249 } // telemetry_callback()
LukeMar 0:f4edd3407cc5 250
LukeMar 0:f4edd3407cc5 251 void mast_callback(void)
LukeMar 0:f4edd3407cc5 252 {
LukeMar 0:f4edd3407cc5 253
LukeMar 0:f4edd3407cc5 254 I=0; //sets for 1.5amp output
LukeMar 0:f4edd3407cc5 255 motor.period(.001); //sets the pulse width modulation period
LukeMar 0:f4edd3407cc5 256 motor.pulsewidth(0);
LukeMar 0:f4edd3407cc5 257 slp = 0;
LukeMar 0:f4edd3407cc5 258 r_slp = 0; //sstarts the current driver in off state
LukeMar 0:f4edd3407cc5 259 Thread::wait(10);
LukeMar 0:f4edd3407cc5 260 // loop
LukeMar 0:f4edd3407cc5 261 while(1) {
LukeMar 0:f4edd3407cc5 262
LukeMar 0:f4edd3407cc5 263 if (rx.valid) {
LukeMar 0:f4edd3407cc5 264 RC_O = rx.channel[0];
LukeMar 0:f4edd3407cc5 265 } else {
LukeMar 0:f4edd3407cc5 266 //pc.printf(" invalid\r\n");
LukeMar 0:f4edd3407cc5 267 slp = 0;
LukeMar 0:f4edd3407cc5 268 }
LukeMar 0:f4edd3407cc5 269 //39.19
LukeMar 0:f4edd3407cc5 270 d_ang = (RC_O/6.77)+39.19;
LukeMar 0:f4edd3407cc5 271 pos = (ain-.108)/.002466;
LukeMar 0:f4edd3407cc5 272 printf("%f\n",ain);
LukeMar 0:f4edd3407cc5 273 if((pos > (d_ang-xx))&& (pos < (d_ang+xx))) {
LukeMar 0:f4edd3407cc5 274 motor.pulsewidth(0);
LukeMar 0:f4edd3407cc5 275 slp = 0;
LukeMar 0:f4edd3407cc5 276 }
LukeMar 0:f4edd3407cc5 277 if( (pos > (d_ang+xx))) {
LukeMar 0:f4edd3407cc5 278 slp = 1;
LukeMar 0:f4edd3407cc5 279 dir = 1; //left??
LukeMar 0:f4edd3407cc5 280 motor.pulsewidth(.0005);
LukeMar 0:f4edd3407cc5 281 //pc.printf("pos: %.3f\n",pos);
LukeMar 0:f4edd3407cc5 282 Thread::wait(ww);
LukeMar 0:f4edd3407cc5 283 pos = posi();
LukeMar 0:f4edd3407cc5 284 }//if pos
LukeMar 0:f4edd3407cc5 285 if((pos < (d_ang-xx))) {
LukeMar 0:f4edd3407cc5 286 slp = 1;
LukeMar 0:f4edd3407cc5 287 dir = 0; //right??
LukeMar 0:f4edd3407cc5 288 motor.pulsewidth(.0005);
LukeMar 0:f4edd3407cc5 289 //pc.printf("pos: %.3f\n",pos);
LukeMar 0:f4edd3407cc5 290 Thread::wait(ww);
LukeMar 0:f4edd3407cc5 291 pos = posi();
LukeMar 0:f4edd3407cc5 292 }
LukeMar 0:f4edd3407cc5 293 ThisThread::sleep_until(zz);
LukeMar 0:f4edd3407cc5 294 }//while(1)
LukeMar 0:f4edd3407cc5 295 }//mast callback
LukeMar 0:f4edd3407cc5 296
LukeMar 0:f4edd3407cc5 297 void rudder_callback(void)
LukeMar 0:f4edd3407cc5 298 {
LukeMar 0:f4edd3407cc5 299
LukeMar 0:f4edd3407cc5 300 r_I=0; //sets for 1.5amp output
LukeMar 0:f4edd3407cc5 301 rudder.period(.001); //sets the pulse width modulation period
LukeMar 0:f4edd3407cc5 302 rudder.pulsewidth(0);
LukeMar 0:f4edd3407cc5 303 slp = 0;
LukeMar 0:f4edd3407cc5 304 r_slp = 0; //sstarts the current driver in off state
LukeMar 0:f4edd3407cc5 305 Thread::wait(10);
LukeMar 0:f4edd3407cc5 306
LukeMar 0:f4edd3407cc5 307 // loop
LukeMar 0:f4edd3407cc5 308 while(1) {
LukeMar 0:f4edd3407cc5 309
LukeMar 0:f4edd3407cc5 310 if (rx.valid) {
LukeMar 0:f4edd3407cc5 311 RC_1 = rx.channel[1];
LukeMar 0:f4edd3407cc5 312 } else {
LukeMar 0:f4edd3407cc5 313 //pc.printf(" invalid\r\n");
LukeMar 0:f4edd3407cc5 314 r_slp = 0;
LukeMar 0:f4edd3407cc5 315 }
LukeMar 0:f4edd3407cc5 316 //39.19
LukeMar 0:f4edd3407cc5 317 r_ang = (RC_1/6.77)-5.0;
LukeMar 0:f4edd3407cc5 318 r_pos = (r_ain-.108)/.002466;
LukeMar 0:f4edd3407cc5 319
LukeMar 0:f4edd3407cc5 320 if((r_pos > (r_ang-xx)) && (r_pos < (r_ang+xx))) {
LukeMar 0:f4edd3407cc5 321 rudder.pulsewidth(0);
LukeMar 0:f4edd3407cc5 322 r_slp = 0;
LukeMar 0:f4edd3407cc5 323 }
LukeMar 0:f4edd3407cc5 324 if( (r_pos > (r_ang+xx)) ) { //&& r_pos < 235.0
LukeMar 0:f4edd3407cc5 325 r_slp = 1;
LukeMar 0:f4edd3407cc5 326 r_dir = 1; //left??
LukeMar 0:f4edd3407cc5 327 rudder.pulsewidth(.0005);
LukeMar 0:f4edd3407cc5 328 Thread::wait(ww);
LukeMar 0:f4edd3407cc5 329 r_pos = posr();
LukeMar 0:f4edd3407cc5 330 }//if pos
LukeMar 0:f4edd3407cc5 331 if((r_pos < (r_ang-xx)) ) { // && r_pos > 55.0
LukeMar 0:f4edd3407cc5 332 r_slp = 1;
LukeMar 0:f4edd3407cc5 333 r_dir = 0; //right??
LukeMar 0:f4edd3407cc5 334 rudder.pulsewidth(.0005);
LukeMar 0:f4edd3407cc5 335 Thread::wait(ww);
LukeMar 0:f4edd3407cc5 336 r_pos = posr();
LukeMar 0:f4edd3407cc5 337 }
LukeMar 0:f4edd3407cc5 338 ThisThread::sleep_until(zz);
LukeMar 0:f4edd3407cc5 339 }//while(1)
LukeMar 0:f4edd3407cc5 340 }//rudder callback