NR18 Active Aero Code

Dependencies:   BNO055 NeoStrip Servo mbed

Files at this revision

API Documentation at this revision

Comitter:
m181878
Date:
Mon May 14 00:22:27 2018 +0000
Commit message:
NR18 Active Aero Code

Changed in this revision

BNO055.lib Show annotated file Show diff for this revision Revisions of this file
NeoStrip.lib Show annotated file Show diff for this revision Revisions of this file
Servo.lib Show annotated file Show diff for this revision Revisions of this file
main.cpp Show annotated file Show diff for this revision Revisions of this file
mbed.bld Show annotated file Show diff for this revision Revisions of this file
diff -r 000000000000 -r 10a064cee806 BNO055.lib
--- /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
diff -r 000000000000 -r 10a064cee806 NeoStrip.lib
--- /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
diff -r 000000000000 -r 10a064cee806 Servo.lib
--- /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
diff -r 000000000000 -r 10a064cee806 main.cpp
--- /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
diff -r 000000000000 -r 10a064cee806 mbed.bld
--- /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