Diff: main.cpp

Revision:

0:f4edd3407cc5

Child:

1:4b4d5d18fc57

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp	Thu Jan 17 00:28:07 2019 +0000
@@ -0,0 +1,340 @@
+
+/*
+  Sailbot Hull 14
+  L Marino and D Robinson,
+  with D Evangelista, P Frontera, M Kutzer, A Laun
+  2018
+
+  Threaded version using mbed OS5
+  Serial links between nodes
+  Stepper motors with Pololu 2968 drivers (MP6500 based)
+  Piher PST360 angle sensors
+  Adafruit Absolute GPS
+  Telemetry via ???Ubiquity???
+*/
+
+//Thread for logging time and Gps, heading and encoder values is mostly complete
+//control thread is a work in progress
+
+#include "mbed.h"
+#include "MBed_Adafruit_GPS.h"
+#include "BNO055.h"
+#include "Spektrum.h"
+#include "unity.h"
+
+Serial pc(USBTX, USBRX, 115200);
+Spektrum rx(p13, p14);
+int i;
+int j;
+float now;
+//***Mast universal variables***
+AnalogIn   ain(p16);
+PwmOut  motor( p25 );
+DigitalOut   dir( p24 );
+DigitalOut    I(p26);
+DigitalOut slp(p29); //sleep
+float d_ang = 180.0;
+float pos = 180.0;
+float RC_O;
+//***rudder universal variables***
+AnalogIn   r_ain(p15);
+PwmOut  rudder( p22 );
+DigitalOut   r_dir( p21 );
+DigitalOut    r_I(p23);
+DigitalOut r_slp(p30); //sleep
+float r_ang;
+float r_pos;
+float RC_1;
+
+float xx = 4.0; //changes the threshold that the motor goes to sleep on
+float zz = 88; //changes the wait time at end of the the mast rudder threads
+float ww = 10; //changes wait time at end of if statments in mast rudder threads
+//****universal variables for Telemetry****
+DigitalOut heartbeat(LED1);
+Serial * gps_Serial;
+BNO055 bno(p28, p27);
+char c; //when read via Adafruit_GPS::read(), the class returns single character stored here
+float info; //a multiuse variable used for sending things over GPS
+Timer refresh_Timer; //sets up a timer for use in loop; how often do we print GPS info?
+const int refresh_Time = 2000; //refresh time in ms
+
+union Float {       //slightly mysterious data type that we use to send data over serial
+    float    m_float;
+    uint8_t  m_bytes[sizeof(float)];
+};
+float        data;
+uint8_t      bytes[sizeof(float)];
+Float        myFloat;
+
+//**get position**
+float posi();
+float posr();
+
+//*********read reads yaw from IMU **************
+void yaw_read()
+{
+    bno.get_angles();
+    data = bno.euler.yaw;
+    myFloat.m_float = data;
+    data = myFloat.m_float;   // get the float back from the union
+    pc.printf("%.1f", data);
+    Thread::wait(35);
+
+}
+
+//********concatenate time (not currently in use)**********
+float conc(float hr, float min, float sec )
+{
+    return hr+(min/60.0)+(sec/3600.0);
+}
+//********Initialize IMU************
+
+void bno_init(void)
+{
+    if(bno.check()) {
+        //pc.printf("BNO055 connected\r\n");
+        bno.reset();
+        Thread::wait(30);
+        bno.setmode(OPERATION_MODE_CONFIG);
+        bno.SetExternalCrystal(1);
+        //bno.set_orientation(1);
+        bno.setmode(OPERATION_MODE_NDOF);  //Uses magnetometer
+        //bno.setmode(OPERATION_MODE_NDOF_FMC_OFF);   //no magnetometer
+        bno.set_angle_units(RADIANS);
+    } else {
+        //pc.printf("BNO055 NOT connected\r\n Program Trap.");
+        while(1);
+    }
+}
+
+// *****threading*****
+Thread telemetry_thread;
+Thread mast_thread;
+Thread rudder_thread;
+
+void telemetry_callback(void);
+void mast_callback(void);
+void rudder_callback(void);
+
+int main()
+{
+    //start all threads
+    wait(10);
+    //telemetry_thread.start(callback(telemetry_callback));
+    mast_thread.start(callback(mast_callback));
+    rudder_thread.start(callback(rudder_callback));
+    //could have a heart beat but I tried to reduce the number of threads fighting for time
+}//end main
+
+float posi()
+{
+    float p1;
+    float p2;
+    float p3;
+    p1 = (ain-.108)/.002466;
+    Thread::wait(3);
+    p2 = (ain-.108)/.002466;
+    Thread::wait(3);
+    p3 = (ain-.108)/.002466;
+    printf("%f\n",ain);
+    return (p1+p2+p3)/3.0;
+}
+float posr()
+{
+    float r1;
+    float r2;
+    float r3;
+    r1 = (r_ain-.108)/.002466;
+    Thread::wait(3);
+    r2 = (r_ain-.108)/.002466;
+    Thread::wait(3);
+    r3 = (r_ain-.108)/.002466;
+    return (r1+r2+r3)/3.0;
+}
+
+void telemetry_callback(void)
+{
+    //uint64_t now;
+    //debug("telemetry_thread started\r\n");
+
+    pc.baud(115200); //sets virtual COM serial communication to high rate; this is to allow more time to be spent on GPS retrieval
+    gps_Serial = new Serial(p9,p10); //serial object for use w/ GPS
+    Adafruit_GPS myGPS(gps_Serial); //object of Adafruit's GPS class
+
+    myGPS.begin(9600);  //sets baud rate for GPS communication; note this may be changed via Adafruit_GPS::sendCommand(char *)
+    //a list of GPS commands is available at http://www.adafruit.com/datasheets/PMTK_A08.pdf
+
+    myGPS.sendCommand(PMTK_SET_NMEA_OUTPUT_RMCGGA); //these commands are defined in MBed_Adafruit_GPS.h; a link is provided there for command creation
+    myGPS.sendCommand(PMTK_SET_NMEA_UPDATE_1HZ);
+    myGPS.sendCommand(PGCMD_ANTENNA);
+
+    //pc.printf("Connection established at 115200 baud...\n");
+    bno_init();
+
+    Thread::wait(1000);
+    refresh_Timer.start();  //starts the clock on the timer
+
+    while(true) {
+
+        c = myGPS.read();   //queries the GPS
+        bno.get_angles();
+        //check if we recieved a new message from GPS, if so, attempt to parse it,
+        if ( myGPS.newNMEAreceived() ) {
+            if ( !myGPS.parse(myGPS.lastNMEA()) ) {
+                continue;
+            }//if ( !myGPS.parse..
+
+            if (refresh_Timer.read_ms() >= refresh_Time) {
+                refresh_Timer.reset();
+            
+                data = conc(myGPS.hour, myGPS.minute, myGPS.seconds );               // conc(myGPS.hour, myGPS.minute, myGPS.seconds );
+                myFloat.m_float = data;
+                data = myFloat.m_float;   // get the float back from the union
+                pc.printf("%2.0f", data);
+                Thread::wait(35);
+                /*  this part can send hr min second separately, commented to save thread space
+                data = myGPS.hour;               // conc(myGPS.hour, myGPS.minute, myGPS.seconds );
+                myFloat.m_float = data;
+                data = myFloat.m_float;   // get the float back from the union
+                pc.printf("%2.0f", data);
+                Thread::wait(35);
+                //send min
+                data = myGPS.minute;               // conc(myGPS.hour, myGPS.minute, myGPS.seconds );
+                myFloat.m_float = data;
+                data = myFloat.m_float;   // get the float back from the union
+                pc.printf("%2.0f", data);
+                Thread::wait(35);
+                //send sec
+                data = myGPS.seconds;               // conc(myGPS.hour, myGPS.minute, myGPS.seconds );
+                myFloat.m_float = data;
+                data = myFloat.m_float;   // get the float back from the union
+                pc.printf("%2.0f", data);
+                Thread::wait(35);
+                */
+                if (myGPS.fix) {
+                    //rudder and mast position will log here
+                    //send mast angle
+                    data = posi();
+                    myFloat.m_float = data;
+                    data = myFloat.m_float;   // get the float back from the union
+                    pc.printf("%.1f", data);
+                    Thread::wait(35);
+                    //send rudder angle
+                    data = posr();
+                    myFloat.m_float = data;
+                    data = myFloat.m_float;   // get the float back from the union
+                    pc.printf("%.1f", data);
+                    Thread::wait(35);
+                    //send latitude
+                    data = myGPS.latitude;
+                    myFloat.m_float = data;
+                    data = myFloat.m_float;   // get the float back from the union
+                    pc.printf("%.2f", data);
+                    Thread::wait(35);
+                    //send logitude
+                    data = myGPS.longitude;
+                    myFloat.m_float = data;
+                    data = myFloat.m_float;   // get the float back from the union
+                    pc.printf("%.2f", data);
+                    Thread::wait(35);
+                    //send yaw
+                    yaw_read();
+                    //send transmittion
+                    pc.printf("q");
+                    Thread::wait(3000);
+                }//if (myGPS.fix)
+            }//if (refresh_Timer..
+        }//if ( myGPS.newNMEAreceived() )
+    }//while(true)
+} // telemetry_callback()
+
+void mast_callback(void)
+{
+
+    I=0; //sets for 1.5amp output
+    motor.period(.001); //sets the pulse width modulation period
+    motor.pulsewidth(0); 
+    slp = 0;
+    r_slp = 0; //sstarts the current driver in off state
+    Thread::wait(10);
+    // loop
+    while(1) {
+        
+        if (rx.valid) {
+            RC_O = rx.channel[0];
+        } else {
+            //pc.printf(" invalid\r\n");
+            slp = 0;
+        }
+        //39.19
+        d_ang = (RC_O/6.77)+39.19;
+        pos = (ain-.108)/.002466;
+        printf("%f\n",ain);
+        if((pos > (d_ang-xx))&& (pos < (d_ang+xx))) {
+            motor.pulsewidth(0);
+            slp = 0;
+        }
+        if( (pos > (d_ang+xx))) {
+            slp = 1;
+                dir = 1; //left??
+                motor.pulsewidth(.0005);
+                //pc.printf("pos: %.3f\n",pos);
+                Thread::wait(ww);
+                pos = posi();
+        }//if pos
+        if((pos < (d_ang-xx))) {
+            slp = 1;
+                dir = 0; //right??
+                motor.pulsewidth(.0005);
+                //pc.printf("pos: %.3f\n",pos);
+                Thread::wait(ww);
+                pos = posi();
+        }
+        ThisThread::sleep_until(zz);
+    }//while(1)
+}//mast callback
+
+void rudder_callback(void)
+{
+
+    r_I=0; //sets for 1.5amp output
+    rudder.period(.001); //sets the pulse width modulation period
+    rudder.pulsewidth(0); 
+    slp = 0;
+    r_slp = 0; //sstarts the current driver in off state
+    Thread::wait(10);
+    
+    // loop
+    while(1) {
+        
+        if (rx.valid) {
+            RC_1 = rx.channel[1];
+        } else {
+            //pc.printf(" invalid\r\n");
+            r_slp = 0;
+        }
+        //39.19
+        r_ang = (RC_1/6.77)-5.0;
+        r_pos = (r_ain-.108)/.002466;
+
+        if((r_pos > (r_ang-xx)) && (r_pos < (r_ang+xx))) {
+            rudder.pulsewidth(0);
+            r_slp = 0;
+        }
+        if( (r_pos > (r_ang+xx)) ) {  //&& r_pos < 235.0
+            r_slp = 1;
+                r_dir = 1; //left??
+                rudder.pulsewidth(.0005);
+                Thread::wait(ww);
+                r_pos = posr();
+        }//if pos
+        if((r_pos < (r_ang-xx)) ) {   // && r_pos > 55.0
+            r_slp = 1;
+                r_dir = 0; //right??
+                rudder.pulsewidth(.0005);
+                Thread::wait(ww);
+                r_pos = posr();
+        }
+        ThisThread::sleep_until(zz);
+    }//while(1)
+}//rudder callback
\ No newline at end of file