File content as of revision 1:4b4d5d18fc57:

/*
  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(p20);
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 = 5.5; //changes the threshold that the motor goes to sleep on
float gg = 9.5; //changes the threshold that the motor goes to sleep on for mast
float zz = 121; //changes wait at end of rc thread
float ww = 5; //changes wait time at end of if statments in mast rudder threads
int ff = 65;//changes wait at end of telemetry
//****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
       //wait time between pieces of telemetry info


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(ff);

}

void roll_read()
{
    bno.get_angles();
    data = bno.euler.roll;
    myFloat.m_float = data;
    data = myFloat.m_float;   // get the float back from the union
    pc.printf("%.1f", data);
    Thread::wait(ff);

}

//********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(1);
    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*449.0-147.53;  //ain*447.73-147.53;
    Thread::wait(3);
    p2 = ain*447.73-147.53;
    Thread::wait(3);
    p3 = ain*447.73-147.53;
    
    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");

    
    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(100);
    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(ff);*/
                //  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(ff);
                    //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(ff);*/
                    //send latitude
                    data = myGPS.latitude;
                    myFloat.m_float = data;
                    data = myFloat.m_float;   // get the float back from the union
                    pc.printf("%.3f", data);
                    Thread::wait(ff);
                    //send logitude
                    data = myGPS.longitude;
                    myFloat.m_float = data;
                    data = myFloat.m_float;   // get the float back from the union
                    pc.printf("%.3f", data);
                    Thread::wait(ff);
                    //send yaw
                    yaw_read();
                    roll_read();
                    //send transmittion
                    pc.printf("q");
                    Thread::wait(1000);
                }//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)-10.0;      //(RC_O/6.77)+39.19;
        pos = posi();
        //printf("%f\n",ain);
        if((pos > (d_ang-gg))&& (pos < (d_ang+gg))) {
            motor.pulsewidth(0);
            slp = 0;
        }
        if( (pos > (d_ang+gg))) {
            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-gg))) {
            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 = posr();

        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