Zach Ferraguti
NR18 Active Aero Code
Dependencies: BNO055 NeoStrip Servo mbed
Diff: main.cpp
- Revision:
- 0:10a064cee806
--- /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