3/18/19
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:87f1199672a1
JAN-16-19
Who changed what in which revision?
User | Revision | Line number | New 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 |