File content as of revision 0:9e0de96e4dfd:

//Nicholas Restivo
#include "mbed.h"
#include "PwmIn.h"
#include "Servo.h"
#include "LSM9DS1.h"

#define PI 3.14159

Servo Rout(p22);
Servo Lout(p21);

Serial pc(USBTX, USBRX);

PwmIn elev(p16);
PwmIn right(p17);
PwmIn left(p18);
float Rin = 0;
float Lin = 0;
float Ein = 0;
bool RCon = true;

float pitch;

LSM9DS1 IMU(p28, p27, 0xD6, 0x3C);


float getAttitude(float ax, float ay, float az)
{
    float roll = atan2(ay, az);
    float pitch_res = atan2(-ax, sqrt(ay * ay + az * az));

    // Convert everything from radians to degrees:
    pitch_res *= 180.0 / PI;
    roll  *= 180.0 / PI;
    if (pitch_res < -3.0 || pitch_res > 3.0) pc.printf("Pitch: %f,    Roll: %f degress\n\r",pitch_res,roll);
    return pitch_res;
}

int main() {
    
    Rout.calibrate(0.0009, 90.0);
    Lout.calibrate(0.0009, 90.0);
    
    //Accelerometer
    IMU.begin();
    if (!IMU.begin()) {
        pc.printf("Failed to communicate with LSM9DS1.\n");
    }
    IMU.calibrate(1);
    while(!IMU.accelAvailable());
    IMU.readAccel();
    pitch = getAttitude(IMU.calcAccel(IMU.ax), IMU.calcAccel(IMU.ay), IMU.calcAccel(IMU.az));
    
    while(1) {
        
        if (RCon) {
        
            printf("Duty Cycle: %f \n", Ein);
            
            // Input Values & Round
            Lin = float(int(left.dutycycle() * 1000 + 0.5)) / 100; //round to 100th & shift - range is .75 - 1
            Rin = float(int(right.dutycycle() * 1000 + 0.5)) / 100; //round to 100th & shift - range is .75 - .96
            Ein = float(int(elev.dutycycle() * 1000 + 0.5)) / 100; //round to 100th & shift - range is .74 -  .53
            
            //Scale to controller
            Lin = 1.0 - (Lin - 0.75) * 4.0; // 1 when stick is centered
            Rin = (Rin - 0.75) * 4.7619; // 0 when stick is centered
            Rin = float(int(Rin * 100 + 0.5)) / 100;
    //        if (Ein > 0.65) {
    //            Ein = 0.0;
    //        }
            if (Ein > 0 && Ein <= 0.65) {
                Ein = (1.0 - (Ein - .53) * 8.333); 
                Ein = float(int(Ein * 100 + 0.5)) / 100; // 0 when centered (and a little below)
                Lin = 1.0 - Ein;
                Rin = Ein;
            }
            
            Lout.write(Lin); //float percent .75 - 1... -> 0 - .25 -> 0 - 1.0
            Rout.write(Rin); //float percent .75 - .96... -> 0 - .21 -> 0 - 1.0
            
        }
        
        else {
            //To-Do!
            //integrate accelerometer readings to allow autonomous control
            //while(!IMU.accelAvailable());
            IMU.readAccel();
            pitch = getAttitude(IMU.calcAccel(IMU.ax), IMU.calcAccel(IMU.ay), IMU.calcAccel(IMU.az));
            if (pitch > 15.0) {
                Rout.write(1.0);
            }
            else if (pitch < -15.0) {
                Lout.write(0.0);
            }
            else {
                Rout.write(0.0);
                Lout.write(1.0);
            }
            
        }
        
        Lin = float(int(left.dutycycle() * 1000 + 0.5)) / 100; //round to 100th & shift - range is .75 - 1
        if (Lin == 0.0) {
            RCon = false;
        }
        else {
            RCon = true;
        }
    }
}