File content as of revision 0:10a064cee806:

//#include "madpulse.h"
#include "Servo.h"
#include "mbed.h"
#include "BNO055.h"
//#include "NeoStrip.h"
#define BUFFER_SIZE 1024
#define FIVE_HZ_LOOP 5.0
#define LOG_LOOP_FREQ 25.0 // Four messages are taking turns logging, it should be about 25Hz overall
#define CTRL_LOOP_FREQ 100.0
#define MAIN_LOOP_FREQ 300.0
#define PI (3.14159)
//Mad Pulse IMU code definitions
#define STAT_RATE 1.0
#define IMU_RATE 50.0
#define LOOP_RATE 200.0
#define CMD_TIMEOUT 1.0
Timer t;    // Create Timer
CAN can1(p30, p29);
float t_start,t_log,t_ctrl,t_5hz,t_run; // Create timer variables
float dt;
float wrapTo2pi(float ang);
float wrapTopi(float ang);
bool rc_conn,auto_flag,log_flag;
float RPM, TP, FOT, IA;         //PE1
float BAR, MAP, Lambda;         //PE2
float An1, An2, An3, An4;       //PE3
float An5, An6, An7, An8;       //PE4
float F1, F2, F3, F4;           //PE5
float BV, AT, CT;               //PE6
float A5T, A7T;                 //PE7
float RPMR, TPSR, MAPR, MAFLR;  //PE8
float L1M, L2M, TL;             //PE9
float DC1, DC2, DC3, DC4, DC5, DC6, DC7, DC8;   //PE10
float PS, DWRC, DPS;            //PE11
float DAWS, NDAWS, IC, ICP;     //PE12
float DWS1, DWS2, NDWS1, NDWS2; //PE13
float FCA, FCS, FCAT, FCCT;     //PE14
float FCB, FCM;                 //PE15
float ICAT, ICCT, ICB, ICM;     //PE16
float FR1, FL1, RR1, RL1;           //Linear Pots
// Instantiate Serial Ports
//Serial pc(USBTX, USBRX, 115200);           // USB UART Terminal
//Serial xbee(p28,p27,115200);
Serial pc(USBTX, USBRX, 9600);           // USB UART Terminal
Serial xbee(p28,p27,9600);
// Initiate Class Objects
BNO055 imu(p9,p10);             // BNO055 Class Object I2C(SDL,SCL)
Ticker ctrl_ticker;
Servo WingL(p26); //Left Servo
Servo WingR(p25); //Right Servo...if we need one
DigitalIn tps(p17); //Throttle Position Sensor
float tp = 0;
float tpp = 0;
DigitalOut streamline(LED1); //LED on mbed
DigitalOut closed(LED2); //LED on mbed
AnalogIn FR(p15);      //initialize linear pots
AnalogIn FL(p16);
AnalogIn RR(p20);
AnalogIn RL(p19);
// ============================================
// Data Log
// ============================================
// Print data to Xbee serial port (115200 Baud)
void dataLog()
{
    imu.get_accel();
    imu.get_angles();
}
int main()
{
    pc.baud(9600);
    xbee.baud(9600);
    t.start();
    t_start = t.read();
    t_log = t.read();
    t_ctrl = t.read();
    t_5hz = t.read();
    can1.frequency(250000);
    CANMessage msg;
    //Test of wing movement,
    //this was important to know when the car turned on that active aero
    //was functioning, and it looks cool
    WingL = 1.0;
    wait(1.5);
    WingL = 0.75;
    wait(1.5);
    WingL = 1.0;
    wait(1.5);
    WingL = 0.75;
    while(1) {
        tp = tps.read(); //read in raw analog input of throttle position sensor (0 to 1)
        FR1=FR.read();
        FL1=FL.read();
        RR1=RR.read();
        RL1=RL.read();
        //tpp = (tp - 0.0249)/(0.1116); //Calibrating the reading to a percentage
        dataLog();
        //xbee.printf("$DAQ, %.3f, %.3f, %.3f, %.1f, %.2f, %.1f, %.1f\r\n", imu.accel.x/9.81, imu.accel.y/9.81, imu.accel.z/9.81, RPM, tpp, FOT, IA);
        xbee.printf("$DAQ, %.1f, %.2f, %.1f, %.1f, %.1f, %.1f, %.1f, %.1f,%.1f \r\n", RPM, TP, An1, An2, FR1, FL1, RR1, RL1, WingL);
        wait(0.01);
        //Active aero control algorithm (wings open)
        //+/- .7 G's lateral is the number I had chosen for active aero arbitrarily. Test with the IMU
        //This code reads the IMU value and the Throttle position directly from the TPS. This is the code you want to use. Worst case, use safe active aero
        /* if(((imu.accel.x/9.81) >= -0.7) && ((imu.accel.x/9.81) <= 0.7) && (tpp >= 0.5) ) {
             streamline = 1; //LED1
             WingL = 1.0; //subject to change with testing
             //Active aero control algorithm (wings closed)
             //basically any time the wings are not in a condition to open, keep them closed
         } else {
             closed = 1; //LED2
             WingL = 0.75; //subject to change with testing
         }*/
        // SAFE ACTIVE AERO
        if (TP > 85) { //USES CAN BUS THROTTLE POSITION VALUE
            //open
            streamline = 1;
            WingL=1.0;

        } else {
            //closed
            closed = 1;
            WingL=0.75;
        }

        if(can1.read(msg)) {    //This is the loop that it won't enter********
            if (msg.id==218099784) { //PE1
                //RPM
                RPM=(msg.data[0]+256*msg.data[1]);
                //TPS
                if ((msg.data[2]+256*msg.data[3])>32767) {
                    TP=0.1*(msg.data[2]+256*msg.data[3])-65536;
                } else {
                    TP=0.1*(msg.data[2]+256*msg.data[3]);
                }
                //FOT
                if ((msg.data[4]+256*msg.data[5])>32767) {
                    FOT=0.1*(msg.data[4]+256*msg.data[5])-65536;
                } else {
                    FOT=0.1*(msg.data[4]+256*msg.data[5]);
                }
                if ((msg.data[6]+256*msg.data[7])>32767) {
                    IA=0.1*(msg.data[6]+256*msg.data[7])-65536;
                } else {
                    IA=0.1*(msg.data[6]+256*msg.data[7]);
                }
            }
            /*if (msg.id==218100040)  //PE2
            {
                // BAR
                if ((msg.data[0]+256*msg.data[1])>32767)
                {
                    BAR=0.1*(msg.data[0]+256*msg.data[1])-65536;
                }
                else
                {
                    BAR=0.1*(msg.data[0]+256*msg.data[1]);
                }
                //MAP
                if ((msg.data[2]+256*msg.data[3])>32767)
                {
                    MAP=0.1*(msg.data[2]+256*msg.data[3])-65536;
                }
                else
                {
                    MAP=0.1*(msg.data[2]+256*msg.data[3]);
                }
                // Lambda
                if ((msg.data[4]+256*msg.data[5])>32767)
                {
                    Lambda=0.1*(msg.data[4]+256*msg.data[5])-65536;
                }
                else
                {
                    Lambda=0.1*(msg.data[4]+256*msg.data[5]);
                }
            }
            */
            if (msg.id==218100296) { //PE3
                // A1
                if ((msg.data[0]+256*msg.data[1])>32767) {
                    An1=0.1*(msg.data[0]+256*msg.data[1])-65536;
                } else {
                    An1=0.1*(msg.data[0]+256*msg.data[1]);
                }
                //A2
                if ((msg.data[2]+256*msg.data[3])>32767) {
                    An2=0.1*(msg.data[2]+256*msg.data[3])-65536;
                } else {
                    An2=0.1*(msg.data[2]+256*msg.data[3]);
                }
                // A3
                if ((msg.data[4]+256*msg.data[5])>32767) {
                    An3=0.1*(msg.data[4]+256*msg.data[5])-65536;
                } else {
                    An3=0.1*(msg.data[4]+256*msg.data[5]);
                }
                // A4
                if ((msg.data[6]+256*msg.data[7])>32767) {
                    An4=0.1*(msg.data[6]+256*msg.data[7])-65536;
                } else {
                    An4=0.1*(msg.data[6]+256*msg.data[7]);
                }
            }
            /*
            if (msg.id==218100552)  //PE4
            {
                // A5
                if ((msg.data[0]+256*msg.data[1])>32767)
                {
                    An5=0.1*(msg.data[0]+256*msg.data[1])-65536;
                }
                else
                {
                    An5=0.1*(msg.data[0]+256*msg.data[1]);
                }
                //A6
                if ((msg.data[2]+256*msg.data[3])>32767)
                {
                    An6=0.1*(msg.data[2]+256*msg.data[3])-65536;
                }
                else
                {
                    An6=0.1*(msg.data[2]+256*msg.data[3]);
                }
                // A7
                if ((msg.data[4]+256*msg.data[5])>32767)
                {
                    An7=0.1*(msg.data[4]+256*msg.data[5])-65536;
                }
                else
                {
                    An7=0.1*(msg.data[4]+256*msg.data[5]);
                }
                // A8
                if ((msg.data[6]+256*msg.data[7])>32767)
                {
                    An8=0.1*(msg.data[6]+256*msg.data[7])-65536;
                }
                else
                {
                    An8=0.1*(msg.data[6]+256*msg.data[7]);
                }
            }
            if (msg.id==218100808)  //PE5
            {
                // F1
                if ((msg.data[0]+256*msg.data[1])>32767)
                {
                    F1=0.1*(msg.data[0]+256*msg.data[1])-65536;
                }
                else
                {
                    F1=0.1*(msg.data[0]+256*msg.data[1]);
                }
                //F2
                if ((msg.data[2]+256*msg.data[3])>32767)
                {
                    F2=0.1*(msg.data[2]+256*msg.data[3])-65536;
                }
                else
                {
                    F2=0.1*(msg.data[2]+256*msg.data[3]);
                }
                // F3
                if ((msg.data[4]+256*msg.data[5])>32767)
                {
                    F3=0.1*(msg.data[4]+256*msg.data[5])-65536;
                }
                else
                {
                    F3=0.1*(msg.data[4]+256*msg.data[5]);
                }
                // F4
                if ((msg.data[6]+256*msg.data[7])>32767)
                {
                    F4=0.1*(msg.data[6]+256*msg.data[7])-65536;
                }
                else
                {
                    F4=0.1*(msg.data[6]+256*msg.data[7]);
                }
            }
            if (msg.id==218101064)  //PE6
            {
               // Battery Voltage
                if ((msg.data[0]+256*msg.data[1])>32767)
                {
                    BV=0.1*(msg.data[0]+256*msg.data[1])-65536;
                }
                else
                {
                    BV=0.1*(msg.data[0]+256*msg.data[1]);
                }
                // Air Temperature
                if ((msg.data[2]+256*msg.data[3])>32767)
                {
                    AT=0.1*(msg.data[2]+256*msg.data[3])-65536;
                }
                else
                {
                    AT=0.1*(msg.data[2]+256*msg.data[3]);
                }
                // Coolant Temp
                if ((msg.data[4]+256*msg.data[5])>32767)
                {
                    CT=0.1*(msg.data[4]+256*msg.data[5])-65536;
                }
                else
                {
                    CT=0.1*(msg.data[4]+256*msg.data[5]);
                }
            }
            if (msg.id==218101320)  //PE7
            {
               // Analog Input #5 Thermistor
                if ((msg.data[0]+256*msg.data[1])>32767)
                {
                    A5T=0.1*(msg.data[0]+256*msg.data[1])-65536;
                }
                else
                {
                    A5T=0.1*(msg.data[0]+256*msg.data[1]);
                }
                // Analog Input #7 Thermistor
                if ((msg.data[2]+256*msg.data[3])>32767)
                {
                    A7T=0.1*(msg.data[2]+256*msg.data[3])-65536;
                }
                else
                {
                    A7T=0.1*(msg.data[2]+256*msg.data[3]);
                }
            }
            if (msg.id==218101576)  //PE8
            {
                // RPM Rate
                if ((msg.data[0]+256*msg.data[1])>32767)
                {
                    RPMR=0.1*(msg.data[0]+256*msg.data[1])-65536;
                }
                else
                {
                    RPMR=0.1*(msg.data[0]+256*msg.data[1]);
                }
                //TPS Rate
                if ((msg.data[2]+256*msg.data[3])>32767)
                {
                    TPSR=0.1*(msg.data[2]+256*msg.data[3])-65536;
                }
                else
                {
                    TPSR=0.1*(msg.data[2]+256*msg.data[3]);
                }
                // MAP Rate
                if ((msg.data[4]+256*msg.data[5])>32767)
                {
                    MAPR=0.1*(msg.data[4]+256*msg.data[5])-65536;
                }
                else
                {
                    MAPR=0.1*(msg.data[4]+256*msg.data[5]);
                }
                // MAF Load Rate
                if ((msg.data[4]+256*msg.data[5])>32767)
                {
                    MAFLR=0.1*(msg.data[4]+256*msg.data[5])-65536;
                }
                else
                {
                    MAFLR=0.1*(msg.data[4]+256*msg.data[5]);
                }
            }
            if (msg.id==218101832)  //PE9
            {
                // Lambda #1 Measured
                if ((msg.data[0]+256*msg.data[1])>32767)
                {
                    L1M=0.1*(msg.data[0]+256*msg.data[1])-65536;
                }
                else
                {
                    L1M=0.1*(msg.data[0]+256*msg.data[1]);
                }
                // Lambda #2 Measured
                if ((msg.data[2]+256*msg.data[3])>32767)
                {
                    L2M=0.1*(msg.data[2]+256*msg.data[3])-65536;
                }
                else
                {
                    L2M=0.1*(msg.data[2]+256*msg.data[3]);
                }
                // Target Lambda
                if ((msg.data[4]+256*msg.data[5])>32767)
                {
                    TL=0.1*(msg.data[4]+256*msg.data[5])-65536;
                }
                else
                {
                    TL=0.1*(msg.data[4]+256*msg.data[5]);
                }
            }
             if (msg.id==218102344)  //PE11
            {
                // Percent Slip
                if ((msg.data[0]+256*msg.data[1])>32767)
                {
                    PS=0.1*(msg.data[0]+256*msg.data[1])-65536;
                }
                else
                {
                    PS=0.1*(msg.data[0]+256*msg.data[1]);
                }
                // Driven Wheel Rate of Change
                if ((msg.data[2]+256*msg.data[3])>32767)
                {
                    DWRC=0.1*(msg.data[2]+256*msg.data[3])-65536;
                }
                else
                {
                    DWRC=0.1*(msg.data[2]+256*msg.data[3]);
                }
                // Desired Percent Slip
                if ((msg.data[4]+256*msg.data[5])>32767)
                {
                    DPS=0.1*(msg.data[4]+256*msg.data[5])-65536;
                }
                else
                {
                    DPS=0.1*(msg.data[4]+256*msg.data[5]);
                }
            }
            if (msg.id==218102600)  //PE12
            {
                // Driven Avg Wheel Speed
                DAWS=0.1*(msg.data[0]+256*msg.data[1]);
                // Non-Driven Avg Wheel Speed
                NDAWS=0.1*(msg.data[2]+256*msg.data[3]);
                // Ignition Comp
                if ((msg.data[4]+256*msg.data[5])>32767)
                {
                    IC=0.1*(msg.data[4]+256*msg.data[5])-65536;
                }
                else
                {
                    IC=0.1*(msg.data[4]+256*msg.data[5]);
                }
                // Ignition Cut Percent
                if ((msg.data[6]+256*msg.data[7])>32767)
                {
                    ICP=0.1*(msg.data[6]+256*msg.data[7])-65536;
                }
                else
                {
                    ICP=0.1*(msg.data[6]+256*msg.data[7]);
                }
            }
            if (msg.id==218102856)  //PE13
            {
                // Driven Wheel Speed #1
                DWS1=0.1*(msg.data[0]+256*msg.data[1]);
                // Driven Wheel Speed #2
                DWS2=0.1*(msg.data[2]+256*msg.data[3]);
                // Non-Driven Wheel Speed #1
                NDWS1=0.1*(msg.data[4]+256*msg.data[5]);
                // Non-Driven Wheel Speed #2
                NDWS2=0.1*(msg.data[6]+256*msg.data[7]);
            }
            if (msg.id==218103112)  //PE14
            {
                // Fuel Comp Accel
                if ((msg.data[0]+256*msg.data[1])>32767)
                {
                    FCA=0.1*(msg.data[0]+256*msg.data[1])-65536;
                }
                else
                {
                    FCA=0.1*(msg.data[0]+256*msg.data[1]);
                }
                // Fuel Comp Starting
                if ((msg.data[2]+256*msg.data[3])>32767)
                {
                    FCS=0.1*(msg.data[2]+256*msg.data[3])-65536;
                }
                else
                {
                    FCS=0.1*(msg.data[2]+256*msg.data[3]);
                }
                // Fuel Comp Air Temp
                if ((msg.data[4]+256*msg.data[5])>32767)
                {
                    FCAT=0.1*(msg.data[4]+256*msg.data[5])-65536;
                }
                else
                {
                    FCAT=0.1*(msg.data[4]+256*msg.data[5]);
                }
                // Fuel Comp Coolant Temp
                if ((msg.data[6]+256*msg.data[7])>32767)
                {
                    FCCT=0.1*(msg.data[6]+256*msg.data[7])-65536;
                }
                else
                {
                    FCCT=0.1*(msg.data[6]+256*msg.data[7]);
                }
            }
            if (msg.id==218103368)  //PE15
            {
                // Fuel Comp Barometer
                if ((msg.data[0]+256*msg.data[1])>32767)
                {
                    FCB=0.1*(msg.data[0]+256*msg.data[1])-65536;
                }
                else
                {
                    FCB=0.1*(msg.data[0]+256*msg.data[1]);
                }
                // Fuel Comp MAP
                if ((msg.data[2]+256*msg.data[3])>32767)
                {
                    FCM=0.1*(msg.data[2]+256*msg.data[3])-65536;
                }
                else
                {
                    FCM=0.1*(msg.data[2]+256*msg.data[3]);
                }
            }
            if (msg.id==218091592)  //PE16
            {
                // Ignition Comp-Air Temp
                if ((msg.data[0]+256*msg.data[1])>32767)
                {
                    ICAT=0.1*(msg.data[0]+256*msg.data[1])-65536;
                }
                else
                {
                    ICAT=0.1*(msg.data[0]+256*msg.data[1]);
                }
                // Ignition Comp-Coolant Temp
                if ((msg.data[2]+256*msg.data[3])>32767)
                {
                    ICCT=0.1*(msg.data[2]+256*msg.data[3])-65536;
                }
                else
                {
                    ICCT=0.1*(msg.data[2]+256*msg.data[3]);
                }
                // Ignition Comp-Barometer
                if ((msg.data[4]+256*msg.data[5])>32767)
                {
                    ICB=0.1*(msg.data[4]+256*msg.data[5])-65536;
                }
                else
                {
                    ICB=0.1*(msg.data[4]+256*msg.data[5]);
                }
                // Ignition Comp-MAP
                if ((msg.data[6]+256*msg.data[7])>32767)
                {
                    ICM=0.1*(msg.data[6]+256*msg.data[7])-65536;
                }
                else
                {
                    ICM=0.1*(msg.data[6]+256*msg.data[7]);
                }
            }*/
        }
        wait(0.01);
    }
} // End Main Function