![](/media/cache/img/default_profile.jpg.50x50_q85.jpg)
NR18 Active Aero Code
Dependencies: BNO055 NeoStrip Servo mbed
Diff: main.cpp
- Revision:
- 0:10a064cee806
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