Revision 0:10a064cee806, committed 2018-05-14

Comitter:

m181878

Date:

Mon May 14 00:22:27 2018 +0000

Commit message:

NR18 Active Aero Code

Changed in this revision

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/BNO055.lib	Mon May 14 00:22:27 2018 +0000
@@ -0,0 +1,1 @@
+https://os.mbed.com/users/StressedDave/code/BNO055/#1f722ffec323

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/NeoStrip.lib	Mon May 14 00:22:27 2018 +0000
@@ -0,0 +1,1 @@
+https://os.mbed.com/users/aswild/code/NeoStrip/#f531a2be180d

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/Servo.lib	Mon May 14 00:22:27 2018 +0000
@@ -0,0 +1,1 @@
+https://os.mbed.com/users/simon/code/Servo/#36b69a7ced07

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp	Mon May 14 00:22:27 2018 +0000
@@ -0,0 +1,571 @@
+//#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
\ No newline at end of file

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/mbed.bld	Mon May 14 00:22:27 2018 +0000
@@ -0,0 +1,1 @@
+https://os.mbed.com/users/mbed_official/code/mbed/builds/5aab5a7997ee
\ No newline at end of file