stuff
Dependencies: mbed QEI 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
- Child:
- 2:825571dce613
3/14/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 | 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 |