Matthew Marino / Mbed 2 deprecated FINAL_SAILBOT_CODE

JAN 16 2019

Dependencies:   mbed BNO055 MBed_Adafruit-GPS-Library

main.cpp@1:4b4d5d18fc57, 2019-03-14 (annotated)

Committer:
LukeMar
Date:
Thu Mar 14 22:09:48 2019 +0000
Revision:
1:4b4d5d18fc57
Parent:
0:f4edd3407cc5 
3/14/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 1:4b4d5d18fc57 31 AnalogIn ain(p20);
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 1:4b4d5d18fc57 49 float xx = 5.5; //changes the threshold that the motor goes to sleep on
LukeMar 1:4b4d5d18fc57 50 float gg = 9.5; //changes the threshold that the motor goes to sleep on for mast
LukeMar 1:4b4d5d18fc57 51 float zz = 121; //changes wait at end of rc thread
LukeMar 1:4b4d5d18fc57 52 float ww = 5; //changes wait time at end of if statments in mast rudder threads
LukeMar 1:4b4d5d18fc57 53 int ff = 65;//changes wait at end of telemetry
LukeMar 0:f4edd3407cc5 54 //****universal variables for Telemetry****
LukeMar 0:f4edd3407cc5 55 DigitalOut heartbeat(LED1);
LukeMar 0:f4edd3407cc5 56 Serial * gps_Serial;
LukeMar 0:f4edd3407cc5 57 BNO055 bno(p28, p27);
LukeMar 0:f4edd3407cc5 58 char c; //when read via Adafruit_GPS::read(), the class returns single character stored here
LukeMar 0:f4edd3407cc5 59 float info; //a multiuse variable used for sending things over GPS
LukeMar 0:f4edd3407cc5 60 Timer refresh_Timer; //sets up a timer for use in loop; how often do we print GPS info?
LukeMar 0:f4edd3407cc5 61 const int refresh_Time = 2000; //refresh time in ms
LukeMar 1:4b4d5d18fc57 62 //wait time between pieces of telemetry info
LukeMar 1:4b4d5d18fc57 63
LukeMar 0:f4edd3407cc5 64
LukeMar 0:f4edd3407cc5 65 union Float { //slightly mysterious data type that we use to send data over serial
LukeMar 0:f4edd3407cc5 66 float m_float;
LukeMar 0:f4edd3407cc5 67 uint8_t m_bytes[sizeof(float)];
LukeMar 0:f4edd3407cc5 68 };
LukeMar 0:f4edd3407cc5 69 float data;
LukeMar 0:f4edd3407cc5 70 uint8_t bytes[sizeof(float)];
LukeMar 0:f4edd3407cc5 71 Float myFloat;
LukeMar 0:f4edd3407cc5 72
LukeMar 0:f4edd3407cc5 73 //**get position**
LukeMar 0:f4edd3407cc5 74 float posi();
LukeMar 0:f4edd3407cc5 75 float posr();
LukeMar 0:f4edd3407cc5 76
LukeMar 0:f4edd3407cc5 77 //*********read reads yaw from IMU **************
LukeMar 0:f4edd3407cc5 78 void yaw_read()
LukeMar 0:f4edd3407cc5 79 {
LukeMar 0:f4edd3407cc5 80 bno.get_angles();
LukeMar 0:f4edd3407cc5 81 data = bno.euler.yaw;
LukeMar 0:f4edd3407cc5 82 myFloat.m_float = data;
LukeMar 0:f4edd3407cc5 83 data = myFloat.m_float; // get the float back from the union
LukeMar 0:f4edd3407cc5 84 pc.printf("%.1f", data);
LukeMar 1:4b4d5d18fc57 85 Thread::wait(ff);
LukeMar 1:4b4d5d18fc57 86
LukeMar 1:4b4d5d18fc57 87 }
LukeMar 1:4b4d5d18fc57 88
LukeMar 1:4b4d5d18fc57 89 void roll_read()
LukeMar 1:4b4d5d18fc57 90 {
LukeMar 1:4b4d5d18fc57 91 bno.get_angles();
LukeMar 1:4b4d5d18fc57 92 data = bno.euler.roll;
LukeMar 1:4b4d5d18fc57 93 myFloat.m_float = data;
LukeMar 1:4b4d5d18fc57 94 data = myFloat.m_float; // get the float back from the union
LukeMar 1:4b4d5d18fc57 95 pc.printf("%.1f", data);
LukeMar 1:4b4d5d18fc57 96 Thread::wait(ff);
LukeMar 0:f4edd3407cc5 97
LukeMar 0:f4edd3407cc5 98 }
LukeMar 0:f4edd3407cc5 99
LukeMar 0:f4edd3407cc5 100 //********concatenate time (not currently in use)**********
LukeMar 0:f4edd3407cc5 101 float conc(float hr, float min, float sec )
LukeMar 0:f4edd3407cc5 102 {
LukeMar 0:f4edd3407cc5 103 return hr+(min/60.0)+(sec/3600.0);
LukeMar 0:f4edd3407cc5 104 }
LukeMar 0:f4edd3407cc5 105 //********Initialize IMU************
LukeMar 0:f4edd3407cc5 106
LukeMar 0:f4edd3407cc5 107 void bno_init(void)
LukeMar 0:f4edd3407cc5 108 {
LukeMar 0:f4edd3407cc5 109 if(bno.check()) {
LukeMar 0:f4edd3407cc5 110 //pc.printf("BNO055 connected\r\n");
LukeMar 0:f4edd3407cc5 111 bno.reset();
LukeMar 0:f4edd3407cc5 112 Thread::wait(30);
LukeMar 0:f4edd3407cc5 113 bno.setmode(OPERATION_MODE_CONFIG);
LukeMar 0:f4edd3407cc5 114 bno.SetExternalCrystal(1);
LukeMar 0:f4edd3407cc5 115 //bno.set_orientation(1);
LukeMar 0:f4edd3407cc5 116 bno.setmode(OPERATION_MODE_NDOF); //Uses magnetometer
LukeMar 0:f4edd3407cc5 117 //bno.setmode(OPERATION_MODE_NDOF_FMC_OFF); //no magnetometer
LukeMar 0:f4edd3407cc5 118 bno.set_angle_units(RADIANS);
LukeMar 0:f4edd3407cc5 119 } else {
LukeMar 0:f4edd3407cc5 120 //pc.printf("BNO055 NOT connected\r\n Program Trap.");
LukeMar 0:f4edd3407cc5 121 while(1);
LukeMar 0:f4edd3407cc5 122 }
LukeMar 0:f4edd3407cc5 123 }
LukeMar 0:f4edd3407cc5 124
LukeMar 0:f4edd3407cc5 125 // *****threading*****
LukeMar 0:f4edd3407cc5 126 Thread telemetry_thread;
LukeMar 0:f4edd3407cc5 127 Thread mast_thread;
LukeMar 0:f4edd3407cc5 128 Thread rudder_thread;
LukeMar 0:f4edd3407cc5 129
LukeMar 0:f4edd3407cc5 130 void telemetry_callback(void);
LukeMar 0:f4edd3407cc5 131 void mast_callback(void);
LukeMar 0:f4edd3407cc5 132 void rudder_callback(void);
LukeMar 0:f4edd3407cc5 133
LukeMar 0:f4edd3407cc5 134 int main()
LukeMar 0:f4edd3407cc5 135 {
LukeMar 0:f4edd3407cc5 136 //start all threads
LukeMar 1:4b4d5d18fc57 137 wait(1);
LukeMar 1:4b4d5d18fc57 138 telemetry_thread.start(callback(telemetry_callback));
LukeMar 0:f4edd3407cc5 139 mast_thread.start(callback(mast_callback));
LukeMar 0:f4edd3407cc5 140 rudder_thread.start(callback(rudder_callback));
LukeMar 0:f4edd3407cc5 141 //could have a heart beat but I tried to reduce the number of threads fighting for time
LukeMar 0:f4edd3407cc5 142 }//end main
LukeMar 0:f4edd3407cc5 143
LukeMar 0:f4edd3407cc5 144 float posi()
LukeMar 0:f4edd3407cc5 145 {
LukeMar 0:f4edd3407cc5 146 float p1;
LukeMar 0:f4edd3407cc5 147 float p2;
LukeMar 0:f4edd3407cc5 148 float p3;
LukeMar 1:4b4d5d18fc57 149 p1 = ain*449.0-147.53; //ain*447.73-147.53;
LukeMar 0:f4edd3407cc5 150 Thread::wait(3);
LukeMar 1:4b4d5d18fc57 151 p2 = ain*447.73-147.53;
LukeMar 0:f4edd3407cc5 152 Thread::wait(3);
LukeMar 1:4b4d5d18fc57 153 p3 = ain*447.73-147.53;
LukeMar 1:4b4d5d18fc57 154
LukeMar 0:f4edd3407cc5 155 return (p1+p2+p3)/3.0;
LukeMar 0:f4edd3407cc5 156 }
LukeMar 0:f4edd3407cc5 157 float posr()
LukeMar 0:f4edd3407cc5 158 {
LukeMar 0:f4edd3407cc5 159 float r1;
LukeMar 0:f4edd3407cc5 160 float r2;
LukeMar 0:f4edd3407cc5 161 float r3;
LukeMar 0:f4edd3407cc5 162 r1 = (r_ain-.108)/.002466;
LukeMar 0:f4edd3407cc5 163 Thread::wait(3);
LukeMar 0:f4edd3407cc5 164 r2 = (r_ain-.108)/.002466;
LukeMar 0:f4edd3407cc5 165 Thread::wait(3);
LukeMar 0:f4edd3407cc5 166 r3 = (r_ain-.108)/.002466;
LukeMar 0:f4edd3407cc5 167 return (r1+r2+r3)/3.0;
LukeMar 0:f4edd3407cc5 168 }
LukeMar 0:f4edd3407cc5 169
LukeMar 0:f4edd3407cc5 170 void telemetry_callback(void)
LukeMar 0:f4edd3407cc5 171 {
LukeMar 0:f4edd3407cc5 172 //uint64_t now;
LukeMar 0:f4edd3407cc5 173 //debug("telemetry_thread started\r\n");
LukeMar 0:f4edd3407cc5 174
LukeMar 1:4b4d5d18fc57 175
LukeMar 0:f4edd3407cc5 176 gps_Serial = new Serial(p9,p10); //serial object for use w/ GPS
LukeMar 0:f4edd3407cc5 177 Adafruit_GPS myGPS(gps_Serial); //object of Adafruit's GPS class
LukeMar 0:f4edd3407cc5 178
LukeMar 0:f4edd3407cc5 179 myGPS.begin(9600); //sets baud rate for GPS communication; note this may be changed via Adafruit_GPS::sendCommand(char *)
LukeMar 0:f4edd3407cc5 180 //a list of GPS commands is available at http://www.adafruit.com/datasheets/PMTK_A08.pdf
LukeMar 0:f4edd3407cc5 181
LukeMar 0:f4edd3407cc5 182 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 183 myGPS.sendCommand(PMTK_SET_NMEA_UPDATE_1HZ);
LukeMar 0:f4edd3407cc5 184 myGPS.sendCommand(PGCMD_ANTENNA);
LukeMar 0:f4edd3407cc5 185
LukeMar 0:f4edd3407cc5 186 //pc.printf("Connection established at 115200 baud...\n");
LukeMar 0:f4edd3407cc5 187 bno_init();
LukeMar 0:f4edd3407cc5 188
LukeMar 1:4b4d5d18fc57 189 Thread::wait(100);
LukeMar 0:f4edd3407cc5 190 refresh_Timer.start(); //starts the clock on the timer
LukeMar 0:f4edd3407cc5 191
LukeMar 0:f4edd3407cc5 192 while(true) {
LukeMar 0:f4edd3407cc5 193
LukeMar 0:f4edd3407cc5 194 c = myGPS.read(); //queries the GPS
LukeMar 0:f4edd3407cc5 195 bno.get_angles();
LukeMar 0:f4edd3407cc5 196 //check if we recieved a new message from GPS, if so, attempt to parse it,
LukeMar 0:f4edd3407cc5 197 if ( myGPS.newNMEAreceived() ) {
LukeMar 0:f4edd3407cc5 198 if ( !myGPS.parse(myGPS.lastNMEA()) ) {
LukeMar 0:f4edd3407cc5 199 continue;
LukeMar 0:f4edd3407cc5 200 }//if ( !myGPS.parse..
LukeMar 0:f4edd3407cc5 201
LukeMar 0:f4edd3407cc5 202 if (refresh_Timer.read_ms() >= refresh_Time) {
LukeMar 0:f4edd3407cc5 203 refresh_Timer.reset();
LukeMar 1:4b4d5d18fc57 204
LukeMar 1:4b4d5d18fc57 205 /* data = conc(myGPS.hour, myGPS.minute, myGPS.seconds ); // conc(myGPS.hour, myGPS.minute, myGPS.seconds );
LukeMar 0:f4edd3407cc5 206 myFloat.m_float = data;
LukeMar 0:f4edd3407cc5 207 data = myFloat.m_float; // get the float back from the union
LukeMar 0:f4edd3407cc5 208 pc.printf("%2.0f", data);
LukeMar 1:4b4d5d18fc57 209 Thread::wait(ff);*/
LukeMar 1:4b4d5d18fc57 210 // this part can send hr min second separately, commented to save thread space
LukeMar 0:f4edd3407cc5 211 data = myGPS.hour; // conc(myGPS.hour, myGPS.minute, myGPS.seconds );
LukeMar 0:f4edd3407cc5 212 myFloat.m_float = data;
LukeMar 0:f4edd3407cc5 213 data = myFloat.m_float; // get the float back from the union
LukeMar 0:f4edd3407cc5 214 pc.printf("%2.0f", data);
LukeMar 0:f4edd3407cc5 215 Thread::wait(35);
LukeMar 0:f4edd3407cc5 216 //send min
LukeMar 0:f4edd3407cc5 217 data = myGPS.minute; // conc(myGPS.hour, myGPS.minute, myGPS.seconds );
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("%2.0f", data);
LukeMar 0:f4edd3407cc5 221 Thread::wait(35);
LukeMar 0:f4edd3407cc5 222 //send sec
LukeMar 0:f4edd3407cc5 223 data = myGPS.seconds; // conc(myGPS.hour, myGPS.minute, myGPS.seconds );
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("%2.0f", data);
LukeMar 0:f4edd3407cc5 227 Thread::wait(35);
LukeMar 1:4b4d5d18fc57 228
LukeMar 0:f4edd3407cc5 229 if (myGPS.fix) {
LukeMar 0:f4edd3407cc5 230 //rudder and mast position will log here
LukeMar 0:f4edd3407cc5 231 //send mast angle
LukeMar 1:4b4d5d18fc57 232 /* data = posi();
LukeMar 0:f4edd3407cc5 233 myFloat.m_float = data;
LukeMar 0:f4edd3407cc5 234 data = myFloat.m_float; // get the float back from the union
LukeMar 0:f4edd3407cc5 235 pc.printf("%.1f", data);
LukeMar 1:4b4d5d18fc57 236 Thread::wait(ff);
LukeMar 0:f4edd3407cc5 237 //send rudder angle
LukeMar 0:f4edd3407cc5 238 data = posr();
LukeMar 0:f4edd3407cc5 239 myFloat.m_float = data;
LukeMar 0:f4edd3407cc5 240 data = myFloat.m_float; // get the float back from the union
LukeMar 0:f4edd3407cc5 241 pc.printf("%.1f", data);
LukeMar 1:4b4d5d18fc57 242 Thread::wait(ff);*/
LukeMar 0:f4edd3407cc5 243 //send latitude
LukeMar 0:f4edd3407cc5 244 data = myGPS.latitude;
LukeMar 0:f4edd3407cc5 245 myFloat.m_float = data;
LukeMar 0:f4edd3407cc5 246 data = myFloat.m_float; // get the float back from the union
LukeMar 1:4b4d5d18fc57 247 pc.printf("%.3f", data);
LukeMar 1:4b4d5d18fc57 248 Thread::wait(ff);
LukeMar 0:f4edd3407cc5 249 //send logitude
LukeMar 0:f4edd3407cc5 250 data = myGPS.longitude;
LukeMar 0:f4edd3407cc5 251 myFloat.m_float = data;
LukeMar 0:f4edd3407cc5 252 data = myFloat.m_float; // get the float back from the union
LukeMar 1:4b4d5d18fc57 253 pc.printf("%.3f", data);
LukeMar 1:4b4d5d18fc57 254 Thread::wait(ff);
LukeMar 0:f4edd3407cc5 255 //send yaw
LukeMar 0:f4edd3407cc5 256 yaw_read();
LukeMar 1:4b4d5d18fc57 257 roll_read();
LukeMar 0:f4edd3407cc5 258 //send transmittion
LukeMar 0:f4edd3407cc5 259 pc.printf("q");
LukeMar 1:4b4d5d18fc57 260 Thread::wait(1000);
LukeMar 0:f4edd3407cc5 261 }//if (myGPS.fix)
LukeMar 0:f4edd3407cc5 262 }//if (refresh_Timer..
LukeMar 0:f4edd3407cc5 263 }//if ( myGPS.newNMEAreceived() )
LukeMar 0:f4edd3407cc5 264 }//while(true)
LukeMar 0:f4edd3407cc5 265 } // telemetry_callback()
LukeMar 0:f4edd3407cc5 266
LukeMar 0:f4edd3407cc5 267 void mast_callback(void)
LukeMar 0:f4edd3407cc5 268 {
LukeMar 0:f4edd3407cc5 269
LukeMar 0:f4edd3407cc5 270 I=0; //sets for 1.5amp output
LukeMar 0:f4edd3407cc5 271 motor.period(.001); //sets the pulse width modulation period
LukeMar 1:4b4d5d18fc57 272 motor.pulsewidth(0);
LukeMar 0:f4edd3407cc5 273 slp = 0;
LukeMar 0:f4edd3407cc5 274 r_slp = 0; //sstarts the current driver in off state
LukeMar 0:f4edd3407cc5 275 Thread::wait(10);
LukeMar 0:f4edd3407cc5 276 // loop
LukeMar 0:f4edd3407cc5 277 while(1) {
LukeMar 1:4b4d5d18fc57 278
LukeMar 0:f4edd3407cc5 279 if (rx.valid) {
LukeMar 0:f4edd3407cc5 280 RC_O = rx.channel[0];
LukeMar 0:f4edd3407cc5 281 } else {
LukeMar 0:f4edd3407cc5 282 //pc.printf(" invalid\r\n");
LukeMar 0:f4edd3407cc5 283 slp = 0;
LukeMar 0:f4edd3407cc5 284 }
LukeMar 0:f4edd3407cc5 285 //39.19
LukeMar 1:4b4d5d18fc57 286 d_ang = (RC_O/6.77)-10.0; //(RC_O/6.77)+39.19;
LukeMar 1:4b4d5d18fc57 287 pos = posi();
LukeMar 1:4b4d5d18fc57 288 //printf("%f\n",ain);
LukeMar 1:4b4d5d18fc57 289 if((pos > (d_ang-gg))&& (pos < (d_ang+gg))) {
LukeMar 0:f4edd3407cc5 290 motor.pulsewidth(0);
LukeMar 0:f4edd3407cc5 291 slp = 0;
LukeMar 0:f4edd3407cc5 292 }
LukeMar 1:4b4d5d18fc57 293 if( (pos > (d_ang+gg))) {
LukeMar 0:f4edd3407cc5 294 slp = 1;
LukeMar 1:4b4d5d18fc57 295 dir = 1; //left??
LukeMar 1:4b4d5d18fc57 296 motor.pulsewidth(.0005);
LukeMar 1:4b4d5d18fc57 297 //pc.printf("pos: %.3f\n",pos);
LukeMar 1:4b4d5d18fc57 298 Thread::wait(ww);
LukeMar 1:4b4d5d18fc57 299 pos = posi();
LukeMar 0:f4edd3407cc5 300 }//if pos
LukeMar 1:4b4d5d18fc57 301 if((pos < (d_ang-gg))) {
LukeMar 0:f4edd3407cc5 302 slp = 1;
LukeMar 1:4b4d5d18fc57 303 dir = 0; //right??
LukeMar 1:4b4d5d18fc57 304 motor.pulsewidth(.0005);
LukeMar 1:4b4d5d18fc57 305 //pc.printf("pos: %.3f\n",pos);
LukeMar 1:4b4d5d18fc57 306 Thread::wait(ww);
LukeMar 1:4b4d5d18fc57 307 pos = posi();
LukeMar 0:f4edd3407cc5 308 }
LukeMar 0:f4edd3407cc5 309 ThisThread::sleep_until(zz);
LukeMar 0:f4edd3407cc5 310 }//while(1)
LukeMar 0:f4edd3407cc5 311 }//mast callback
LukeMar 0:f4edd3407cc5 312
LukeMar 0:f4edd3407cc5 313 void rudder_callback(void)
LukeMar 0:f4edd3407cc5 314 {
LukeMar 0:f4edd3407cc5 315
LukeMar 0:f4edd3407cc5 316 r_I=0; //sets for 1.5amp output
LukeMar 0:f4edd3407cc5 317 rudder.period(.001); //sets the pulse width modulation period
LukeMar 1:4b4d5d18fc57 318 rudder.pulsewidth(0);
LukeMar 0:f4edd3407cc5 319 slp = 0;
LukeMar 0:f4edd3407cc5 320 r_slp = 0; //sstarts the current driver in off state
LukeMar 0:f4edd3407cc5 321 Thread::wait(10);
LukeMar 1:4b4d5d18fc57 322
LukeMar 0:f4edd3407cc5 323 // loop
LukeMar 0:f4edd3407cc5 324 while(1) {
LukeMar 1:4b4d5d18fc57 325
LukeMar 0:f4edd3407cc5 326 if (rx.valid) {
LukeMar 0:f4edd3407cc5 327 RC_1 = rx.channel[1];
LukeMar 0:f4edd3407cc5 328 } else {
LukeMar 0:f4edd3407cc5 329 //pc.printf(" invalid\r\n");
LukeMar 0:f4edd3407cc5 330 r_slp = 0;
LukeMar 0:f4edd3407cc5 331 }
LukeMar 0:f4edd3407cc5 332 //39.19
LukeMar 0:f4edd3407cc5 333 r_ang = (RC_1/6.77)-5.0;
LukeMar 1:4b4d5d18fc57 334 r_pos = posr();
LukeMar 0:f4edd3407cc5 335
LukeMar 0:f4edd3407cc5 336 if((r_pos > (r_ang-xx)) && (r_pos < (r_ang+xx))) {
LukeMar 0:f4edd3407cc5 337 rudder.pulsewidth(0);
LukeMar 0:f4edd3407cc5 338 r_slp = 0;
LukeMar 0:f4edd3407cc5 339 }
LukeMar 0:f4edd3407cc5 340 if( (r_pos > (r_ang+xx)) ) { //&& r_pos < 235.0
LukeMar 0:f4edd3407cc5 341 r_slp = 1;
LukeMar 1:4b4d5d18fc57 342 r_dir = 1; //left??
LukeMar 1:4b4d5d18fc57 343 rudder.pulsewidth(.0005);
LukeMar 1:4b4d5d18fc57 344 Thread::wait(ww);
LukeMar 1:4b4d5d18fc57 345 r_pos = posr();
LukeMar 0:f4edd3407cc5 346 }//if pos
LukeMar 0:f4edd3407cc5 347 if((r_pos < (r_ang-xx)) ) { // && r_pos > 55.0
LukeMar 0:f4edd3407cc5 348 r_slp = 1;
LukeMar 1:4b4d5d18fc57 349 r_dir = 0; //right??
LukeMar 1:4b4d5d18fc57 350 rudder.pulsewidth(.0005);
LukeMar 1:4b4d5d18fc57 351 Thread::wait(ww);
LukeMar 1:4b4d5d18fc57 352 r_pos = posr();
LukeMar 0:f4edd3407cc5 353 }
LukeMar 0:f4edd3407cc5 354 ThisThread::sleep_until(zz);
LukeMar 0:f4edd3407cc5 355 }//while(1)
LukeMar 0:f4edd3407cc5 356 }//rudder callback