Zach Ferraguti / Mbed 2 deprecated NR18Code

NR18 Active Aero Code

Dependencies:   BNO055 NeoStrip Servo mbed

main.cpp@0:10a064cee806, 2018-05-14 (annotated)

Committer:
m181878
Date:
Mon May 14 00:22:27 2018 +0000
Revision:
0:10a064cee806
NR18 Active Aero Code

Who changed what in which revision?

UserRevisionLine numberNew contents of line
m181878 0:10a064cee806 1 //#include "madpulse.h"
m181878 0:10a064cee806 2 #include "Servo.h"
m181878 0:10a064cee806 3 #include "mbed.h"
m181878 0:10a064cee806 4 #include "BNO055.h"
m181878 0:10a064cee806 5 //#include "NeoStrip.h"
m181878 0:10a064cee806 6 #define BUFFER_SIZE 1024
m181878 0:10a064cee806 7 #define FIVE_HZ_LOOP 5.0
m181878 0:10a064cee806 8 #define LOG_LOOP_FREQ 25.0 // Four messages are taking turns logging, it should be about 25Hz overall
m181878 0:10a064cee806 9 #define CTRL_LOOP_FREQ 100.0
m181878 0:10a064cee806 10 #define MAIN_LOOP_FREQ 300.0
m181878 0:10a064cee806 11 #define PI (3.14159)
m181878 0:10a064cee806 12 //Mad Pulse IMU code definitions
m181878 0:10a064cee806 13 #define STAT_RATE 1.0
m181878 0:10a064cee806 14 #define IMU_RATE 50.0
m181878 0:10a064cee806 15 #define LOOP_RATE 200.0
m181878 0:10a064cee806 16 #define CMD_TIMEOUT 1.0
m181878 0:10a064cee806 17 Timer t; // Create Timer
m181878 0:10a064cee806 18 CAN can1(p30, p29);
m181878 0:10a064cee806 19 float t_start,t_log,t_ctrl,t_5hz,t_run; // Create timer variables
m181878 0:10a064cee806 20 float dt;
m181878 0:10a064cee806 21 float wrapTo2pi(float ang);
m181878 0:10a064cee806 22 float wrapTopi(float ang);
m181878 0:10a064cee806 23 bool rc_conn,auto_flag,log_flag;
m181878 0:10a064cee806 24 float RPM, TP, FOT, IA; //PE1
m181878 0:10a064cee806 25 float BAR, MAP, Lambda; //PE2
m181878 0:10a064cee806 26 float An1, An2, An3, An4; //PE3
m181878 0:10a064cee806 27 float An5, An6, An7, An8; //PE4
m181878 0:10a064cee806 28 float F1, F2, F3, F4; //PE5
m181878 0:10a064cee806 29 float BV, AT, CT; //PE6
m181878 0:10a064cee806 30 float A5T, A7T; //PE7
m181878 0:10a064cee806 31 float RPMR, TPSR, MAPR, MAFLR; //PE8
m181878 0:10a064cee806 32 float L1M, L2M, TL; //PE9
m181878 0:10a064cee806 33 float DC1, DC2, DC3, DC4, DC5, DC6, DC7, DC8; //PE10
m181878 0:10a064cee806 34 float PS, DWRC, DPS; //PE11
m181878 0:10a064cee806 35 float DAWS, NDAWS, IC, ICP; //PE12
m181878 0:10a064cee806 36 float DWS1, DWS2, NDWS1, NDWS2; //PE13
m181878 0:10a064cee806 37 float FCA, FCS, FCAT, FCCT; //PE14
m181878 0:10a064cee806 38 float FCB, FCM; //PE15
m181878 0:10a064cee806 39 float ICAT, ICCT, ICB, ICM; //PE16
m181878 0:10a064cee806 40 float FR1, FL1, RR1, RL1; //Linear Pots
m181878 0:10a064cee806 41 // Instantiate Serial Ports
m181878 0:10a064cee806 42 //Serial pc(USBTX, USBRX, 115200); // USB UART Terminal
m181878 0:10a064cee806 43 //Serial xbee(p28,p27,115200);
m181878 0:10a064cee806 44 Serial pc(USBTX, USBRX, 9600); // USB UART Terminal
m181878 0:10a064cee806 45 Serial xbee(p28,p27,9600);
m181878 0:10a064cee806 46 // Initiate Class Objects
m181878 0:10a064cee806 47 BNO055 imu(p9,p10); // BNO055 Class Object I2C(SDL,SCL)
m181878 0:10a064cee806 48 Ticker ctrl_ticker;
m181878 0:10a064cee806 49 Servo WingL(p26); //Left Servo
m181878 0:10a064cee806 50 Servo WingR(p25); //Right Servo...if we need one
m181878 0:10a064cee806 51 DigitalIn tps(p17); //Throttle Position Sensor
m181878 0:10a064cee806 52 float tp = 0;
m181878 0:10a064cee806 53 float tpp = 0;
m181878 0:10a064cee806 54 DigitalOut streamline(LED1); //LED on mbed
m181878 0:10a064cee806 55 DigitalOut closed(LED2); //LED on mbed
m181878 0:10a064cee806 56 AnalogIn FR(p15); //initialize linear pots
m181878 0:10a064cee806 57 AnalogIn FL(p16);
m181878 0:10a064cee806 58 AnalogIn RR(p20);
m181878 0:10a064cee806 59 AnalogIn RL(p19);
m181878 0:10a064cee806 60 // ============================================
m181878 0:10a064cee806 61 // Data Log
m181878 0:10a064cee806 62 // ============================================
m181878 0:10a064cee806 63 // Print data to Xbee serial port (115200 Baud)
m181878 0:10a064cee806 64 void dataLog()
m181878 0:10a064cee806 65 {
m181878 0:10a064cee806 66 imu.get_accel();
m181878 0:10a064cee806 67 imu.get_angles();
m181878 0:10a064cee806 68 }
m181878 0:10a064cee806 69 int main()
m181878 0:10a064cee806 70 {
m181878 0:10a064cee806 71 pc.baud(9600);
m181878 0:10a064cee806 72 xbee.baud(9600);
m181878 0:10a064cee806 73 t.start();
m181878 0:10a064cee806 74 t_start = t.read();
m181878 0:10a064cee806 75 t_log = t.read();
m181878 0:10a064cee806 76 t_ctrl = t.read();
m181878 0:10a064cee806 77 t_5hz = t.read();
m181878 0:10a064cee806 78 can1.frequency(250000);
m181878 0:10a064cee806 79 CANMessage msg;
m181878 0:10a064cee806 80 //Test of wing movement,
m181878 0:10a064cee806 81 //this was important to know when the car turned on that active aero
m181878 0:10a064cee806 82 //was functioning, and it looks cool
m181878 0:10a064cee806 83 WingL = 1.0;
m181878 0:10a064cee806 84 wait(1.5);
m181878 0:10a064cee806 85 WingL = 0.75;
m181878 0:10a064cee806 86 wait(1.5);
m181878 0:10a064cee806 87 WingL = 1.0;
m181878 0:10a064cee806 88 wait(1.5);
m181878 0:10a064cee806 89 WingL = 0.75;
m181878 0:10a064cee806 90 while(1) {
m181878 0:10a064cee806 91 tp = tps.read(); //read in raw analog input of throttle position sensor (0 to 1)
m181878 0:10a064cee806 92 FR1=FR.read();
m181878 0:10a064cee806 93 FL1=FL.read();
m181878 0:10a064cee806 94 RR1=RR.read();
m181878 0:10a064cee806 95 RL1=RL.read();
m181878 0:10a064cee806 96 //tpp = (tp - 0.0249)/(0.1116); //Calibrating the reading to a percentage
m181878 0:10a064cee806 97 dataLog();
m181878 0:10a064cee806 98 //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);
m181878 0:10a064cee806 99 xbee.printf("$DAQ, %.1f, %.2f, %.1f, %.1f, %.1f, %.1f, %.1f, %.1f,%.1f \r\n", RPM, TP, An1, An2, FR1, FL1, RR1, RL1, WingL);
m181878 0:10a064cee806 100 wait(0.01);
m181878 0:10a064cee806 101 //Active aero control algorithm (wings open)
m181878 0:10a064cee806 102 //+/- .7 G's lateral is the number I had chosen for active aero arbitrarily. Test with the IMU
m181878 0:10a064cee806 103 //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
m181878 0:10a064cee806 104 /* if(((imu.accel.x/9.81) >= -0.7) && ((imu.accel.x/9.81) <= 0.7) && (tpp >= 0.5) ) {
m181878 0:10a064cee806 105 streamline = 1; //LED1
m181878 0:10a064cee806 106 WingL = 1.0; //subject to change with testing
m181878 0:10a064cee806 107 //Active aero control algorithm (wings closed)
m181878 0:10a064cee806 108 //basically any time the wings are not in a condition to open, keep them closed
m181878 0:10a064cee806 109 } else {
m181878 0:10a064cee806 110 closed = 1; //LED2
m181878 0:10a064cee806 111 WingL = 0.75; //subject to change with testing
m181878 0:10a064cee806 112 }*/
m181878 0:10a064cee806 113 // SAFE ACTIVE AERO
m181878 0:10a064cee806 114 if (TP > 85) { //USES CAN BUS THROTTLE POSITION VALUE
m181878 0:10a064cee806 115 //open
m181878 0:10a064cee806 116 streamline = 1;
m181878 0:10a064cee806 117 WingL=1.0;
m181878 0:10a064cee806 118
m181878 0:10a064cee806 119 } else {
m181878 0:10a064cee806 120 //closed
m181878 0:10a064cee806 121 closed = 1;
m181878 0:10a064cee806 122 WingL=0.75;
m181878 0:10a064cee806 123 }
m181878 0:10a064cee806 124
m181878 0:10a064cee806 125 if(can1.read(msg)) { //This is the loop that it won't enter********
m181878 0:10a064cee806 126 if (msg.id==218099784) { //PE1
m181878 0:10a064cee806 127 //RPM
m181878 0:10a064cee806 128 RPM=(msg.data[0]+256*msg.data[1]);
m181878 0:10a064cee806 129 //TPS
m181878 0:10a064cee806 130 if ((msg.data[2]+256*msg.data[3])>32767) {
m181878 0:10a064cee806 131 TP=0.1*(msg.data[2]+256*msg.data[3])-65536;
m181878 0:10a064cee806 132 } else {
m181878 0:10a064cee806 133 TP=0.1*(msg.data[2]+256*msg.data[3]);
m181878 0:10a064cee806 134 }
m181878 0:10a064cee806 135 //FOT
m181878 0:10a064cee806 136 if ((msg.data[4]+256*msg.data[5])>32767) {
m181878 0:10a064cee806 137 FOT=0.1*(msg.data[4]+256*msg.data[5])-65536;
m181878 0:10a064cee806 138 } else {
m181878 0:10a064cee806 139 FOT=0.1*(msg.data[4]+256*msg.data[5]);
m181878 0:10a064cee806 140 }
m181878 0:10a064cee806 141 if ((msg.data[6]+256*msg.data[7])>32767) {
m181878 0:10a064cee806 142 IA=0.1*(msg.data[6]+256*msg.data[7])-65536;
m181878 0:10a064cee806 143 } else {
m181878 0:10a064cee806 144 IA=0.1*(msg.data[6]+256*msg.data[7]);
m181878 0:10a064cee806 145 }
m181878 0:10a064cee806 146 }
m181878 0:10a064cee806 147 /*if (msg.id==218100040) //PE2
m181878 0:10a064cee806 148 {
m181878 0:10a064cee806 149 // BAR
m181878 0:10a064cee806 150 if ((msg.data[0]+256*msg.data[1])>32767)
m181878 0:10a064cee806 151 {
m181878 0:10a064cee806 152 BAR=0.1*(msg.data[0]+256*msg.data[1])-65536;
m181878 0:10a064cee806 153 }
m181878 0:10a064cee806 154 else
m181878 0:10a064cee806 155 {
m181878 0:10a064cee806 156 BAR=0.1*(msg.data[0]+256*msg.data[1]);
m181878 0:10a064cee806 157 }
m181878 0:10a064cee806 158 //MAP
m181878 0:10a064cee806 159 if ((msg.data[2]+256*msg.data[3])>32767)
m181878 0:10a064cee806 160 {
m181878 0:10a064cee806 161 MAP=0.1*(msg.data[2]+256*msg.data[3])-65536;
m181878 0:10a064cee806 162 }
m181878 0:10a064cee806 163 else
m181878 0:10a064cee806 164 {
m181878 0:10a064cee806 165 MAP=0.1*(msg.data[2]+256*msg.data[3]);
m181878 0:10a064cee806 166 }
m181878 0:10a064cee806 167 // Lambda
m181878 0:10a064cee806 168 if ((msg.data[4]+256*msg.data[5])>32767)
m181878 0:10a064cee806 169 {
m181878 0:10a064cee806 170 Lambda=0.1*(msg.data[4]+256*msg.data[5])-65536;
m181878 0:10a064cee806 171 }
m181878 0:10a064cee806 172 else
m181878 0:10a064cee806 173 {
m181878 0:10a064cee806 174 Lambda=0.1*(msg.data[4]+256*msg.data[5]);
m181878 0:10a064cee806 175 }
m181878 0:10a064cee806 176 }
m181878 0:10a064cee806 177 */
m181878 0:10a064cee806 178 if (msg.id==218100296) { //PE3
m181878 0:10a064cee806 179 // A1
m181878 0:10a064cee806 180 if ((msg.data[0]+256*msg.data[1])>32767) {
m181878 0:10a064cee806 181 An1=0.1*(msg.data[0]+256*msg.data[1])-65536;
m181878 0:10a064cee806 182 } else {
m181878 0:10a064cee806 183 An1=0.1*(msg.data[0]+256*msg.data[1]);
m181878 0:10a064cee806 184 }
m181878 0:10a064cee806 185 //A2
m181878 0:10a064cee806 186 if ((msg.data[2]+256*msg.data[3])>32767) {
m181878 0:10a064cee806 187 An2=0.1*(msg.data[2]+256*msg.data[3])-65536;
m181878 0:10a064cee806 188 } else {
m181878 0:10a064cee806 189 An2=0.1*(msg.data[2]+256*msg.data[3]);
m181878 0:10a064cee806 190 }
m181878 0:10a064cee806 191 // A3
m181878 0:10a064cee806 192 if ((msg.data[4]+256*msg.data[5])>32767) {
m181878 0:10a064cee806 193 An3=0.1*(msg.data[4]+256*msg.data[5])-65536;
m181878 0:10a064cee806 194 } else {
m181878 0:10a064cee806 195 An3=0.1*(msg.data[4]+256*msg.data[5]);
m181878 0:10a064cee806 196 }
m181878 0:10a064cee806 197 // A4
m181878 0:10a064cee806 198 if ((msg.data[6]+256*msg.data[7])>32767) {
m181878 0:10a064cee806 199 An4=0.1*(msg.data[6]+256*msg.data[7])-65536;
m181878 0:10a064cee806 200 } else {
m181878 0:10a064cee806 201 An4=0.1*(msg.data[6]+256*msg.data[7]);
m181878 0:10a064cee806 202 }
m181878 0:10a064cee806 203 }
m181878 0:10a064cee806 204 /*
m181878 0:10a064cee806 205 if (msg.id==218100552) //PE4
m181878 0:10a064cee806 206 {
m181878 0:10a064cee806 207 // A5
m181878 0:10a064cee806 208 if ((msg.data[0]+256*msg.data[1])>32767)
m181878 0:10a064cee806 209 {
m181878 0:10a064cee806 210 An5=0.1*(msg.data[0]+256*msg.data[1])-65536;
m181878 0:10a064cee806 211 }
m181878 0:10a064cee806 212 else
m181878 0:10a064cee806 213 {
m181878 0:10a064cee806 214 An5=0.1*(msg.data[0]+256*msg.data[1]);
m181878 0:10a064cee806 215 }
m181878 0:10a064cee806 216 //A6
m181878 0:10a064cee806 217 if ((msg.data[2]+256*msg.data[3])>32767)
m181878 0:10a064cee806 218 {
m181878 0:10a064cee806 219 An6=0.1*(msg.data[2]+256*msg.data[3])-65536;
m181878 0:10a064cee806 220 }
m181878 0:10a064cee806 221 else
m181878 0:10a064cee806 222 {
m181878 0:10a064cee806 223 An6=0.1*(msg.data[2]+256*msg.data[3]);
m181878 0:10a064cee806 224 }
m181878 0:10a064cee806 225 // A7
m181878 0:10a064cee806 226 if ((msg.data[4]+256*msg.data[5])>32767)
m181878 0:10a064cee806 227 {
m181878 0:10a064cee806 228 An7=0.1*(msg.data[4]+256*msg.data[5])-65536;
m181878 0:10a064cee806 229 }
m181878 0:10a064cee806 230 else
m181878 0:10a064cee806 231 {
m181878 0:10a064cee806 232 An7=0.1*(msg.data[4]+256*msg.data[5]);
m181878 0:10a064cee806 233 }
m181878 0:10a064cee806 234 // A8
m181878 0:10a064cee806 235 if ((msg.data[6]+256*msg.data[7])>32767)
m181878 0:10a064cee806 236 {
m181878 0:10a064cee806 237 An8=0.1*(msg.data[6]+256*msg.data[7])-65536;
m181878 0:10a064cee806 238 }
m181878 0:10a064cee806 239 else
m181878 0:10a064cee806 240 {
m181878 0:10a064cee806 241 An8=0.1*(msg.data[6]+256*msg.data[7]);
m181878 0:10a064cee806 242 }
m181878 0:10a064cee806 243 }
m181878 0:10a064cee806 244 if (msg.id==218100808) //PE5
m181878 0:10a064cee806 245 {
m181878 0:10a064cee806 246 // F1
m181878 0:10a064cee806 247 if ((msg.data[0]+256*msg.data[1])>32767)
m181878 0:10a064cee806 248 {
m181878 0:10a064cee806 249 F1=0.1*(msg.data[0]+256*msg.data[1])-65536;
m181878 0:10a064cee806 250 }
m181878 0:10a064cee806 251 else
m181878 0:10a064cee806 252 {
m181878 0:10a064cee806 253 F1=0.1*(msg.data[0]+256*msg.data[1]);
m181878 0:10a064cee806 254 }
m181878 0:10a064cee806 255 //F2
m181878 0:10a064cee806 256 if ((msg.data[2]+256*msg.data[3])>32767)
m181878 0:10a064cee806 257 {
m181878 0:10a064cee806 258 F2=0.1*(msg.data[2]+256*msg.data[3])-65536;
m181878 0:10a064cee806 259 }
m181878 0:10a064cee806 260 else
m181878 0:10a064cee806 261 {
m181878 0:10a064cee806 262 F2=0.1*(msg.data[2]+256*msg.data[3]);
m181878 0:10a064cee806 263 }
m181878 0:10a064cee806 264 // F3
m181878 0:10a064cee806 265 if ((msg.data[4]+256*msg.data[5])>32767)
m181878 0:10a064cee806 266 {
m181878 0:10a064cee806 267 F3=0.1*(msg.data[4]+256*msg.data[5])-65536;
m181878 0:10a064cee806 268 }
m181878 0:10a064cee806 269 else
m181878 0:10a064cee806 270 {
m181878 0:10a064cee806 271 F3=0.1*(msg.data[4]+256*msg.data[5]);
m181878 0:10a064cee806 272 }
m181878 0:10a064cee806 273 // F4
m181878 0:10a064cee806 274 if ((msg.data[6]+256*msg.data[7])>32767)
m181878 0:10a064cee806 275 {
m181878 0:10a064cee806 276 F4=0.1*(msg.data[6]+256*msg.data[7])-65536;
m181878 0:10a064cee806 277 }
m181878 0:10a064cee806 278 else
m181878 0:10a064cee806 279 {
m181878 0:10a064cee806 280 F4=0.1*(msg.data[6]+256*msg.data[7]);
m181878 0:10a064cee806 281 }
m181878 0:10a064cee806 282 }
m181878 0:10a064cee806 283 if (msg.id==218101064) //PE6
m181878 0:10a064cee806 284 {
m181878 0:10a064cee806 285 // Battery Voltage
m181878 0:10a064cee806 286 if ((msg.data[0]+256*msg.data[1])>32767)
m181878 0:10a064cee806 287 {
m181878 0:10a064cee806 288 BV=0.1*(msg.data[0]+256*msg.data[1])-65536;
m181878 0:10a064cee806 289 }
m181878 0:10a064cee806 290 else
m181878 0:10a064cee806 291 {
m181878 0:10a064cee806 292 BV=0.1*(msg.data[0]+256*msg.data[1]);
m181878 0:10a064cee806 293 }
m181878 0:10a064cee806 294 // Air Temperature
m181878 0:10a064cee806 295 if ((msg.data[2]+256*msg.data[3])>32767)
m181878 0:10a064cee806 296 {
m181878 0:10a064cee806 297 AT=0.1*(msg.data[2]+256*msg.data[3])-65536;
m181878 0:10a064cee806 298 }
m181878 0:10a064cee806 299 else
m181878 0:10a064cee806 300 {
m181878 0:10a064cee806 301 AT=0.1*(msg.data[2]+256*msg.data[3]);
m181878 0:10a064cee806 302 }
m181878 0:10a064cee806 303 // Coolant Temp
m181878 0:10a064cee806 304 if ((msg.data[4]+256*msg.data[5])>32767)
m181878 0:10a064cee806 305 {
m181878 0:10a064cee806 306 CT=0.1*(msg.data[4]+256*msg.data[5])-65536;
m181878 0:10a064cee806 307 }
m181878 0:10a064cee806 308 else
m181878 0:10a064cee806 309 {
m181878 0:10a064cee806 310 CT=0.1*(msg.data[4]+256*msg.data[5]);
m181878 0:10a064cee806 311 }
m181878 0:10a064cee806 312 }
m181878 0:10a064cee806 313 if (msg.id==218101320) //PE7
m181878 0:10a064cee806 314 {
m181878 0:10a064cee806 315 // Analog Input #5 Thermistor
m181878 0:10a064cee806 316 if ((msg.data[0]+256*msg.data[1])>32767)
m181878 0:10a064cee806 317 {
m181878 0:10a064cee806 318 A5T=0.1*(msg.data[0]+256*msg.data[1])-65536;
m181878 0:10a064cee806 319 }
m181878 0:10a064cee806 320 else
m181878 0:10a064cee806 321 {
m181878 0:10a064cee806 322 A5T=0.1*(msg.data[0]+256*msg.data[1]);
m181878 0:10a064cee806 323 }
m181878 0:10a064cee806 324 // Analog Input #7 Thermistor
m181878 0:10a064cee806 325 if ((msg.data[2]+256*msg.data[3])>32767)
m181878 0:10a064cee806 326 {
m181878 0:10a064cee806 327 A7T=0.1*(msg.data[2]+256*msg.data[3])-65536;
m181878 0:10a064cee806 328 }
m181878 0:10a064cee806 329 else
m181878 0:10a064cee806 330 {
m181878 0:10a064cee806 331 A7T=0.1*(msg.data[2]+256*msg.data[3]);
m181878 0:10a064cee806 332 }
m181878 0:10a064cee806 333 }
m181878 0:10a064cee806 334 if (msg.id==218101576) //PE8
m181878 0:10a064cee806 335 {
m181878 0:10a064cee806 336 // RPM Rate
m181878 0:10a064cee806 337 if ((msg.data[0]+256*msg.data[1])>32767)
m181878 0:10a064cee806 338 {
m181878 0:10a064cee806 339 RPMR=0.1*(msg.data[0]+256*msg.data[1])-65536;
m181878 0:10a064cee806 340 }
m181878 0:10a064cee806 341 else
m181878 0:10a064cee806 342 {
m181878 0:10a064cee806 343 RPMR=0.1*(msg.data[0]+256*msg.data[1]);
m181878 0:10a064cee806 344 }
m181878 0:10a064cee806 345 //TPS Rate
m181878 0:10a064cee806 346 if ((msg.data[2]+256*msg.data[3])>32767)
m181878 0:10a064cee806 347 {
m181878 0:10a064cee806 348 TPSR=0.1*(msg.data[2]+256*msg.data[3])-65536;
m181878 0:10a064cee806 349 }
m181878 0:10a064cee806 350 else
m181878 0:10a064cee806 351 {
m181878 0:10a064cee806 352 TPSR=0.1*(msg.data[2]+256*msg.data[3]);
m181878 0:10a064cee806 353 }
m181878 0:10a064cee806 354 // MAP Rate
m181878 0:10a064cee806 355 if ((msg.data[4]+256*msg.data[5])>32767)
m181878 0:10a064cee806 356 {
m181878 0:10a064cee806 357 MAPR=0.1*(msg.data[4]+256*msg.data[5])-65536;
m181878 0:10a064cee806 358 }
m181878 0:10a064cee806 359 else
m181878 0:10a064cee806 360 {
m181878 0:10a064cee806 361 MAPR=0.1*(msg.data[4]+256*msg.data[5]);
m181878 0:10a064cee806 362 }
m181878 0:10a064cee806 363 // MAF Load Rate
m181878 0:10a064cee806 364 if ((msg.data[4]+256*msg.data[5])>32767)
m181878 0:10a064cee806 365 {
m181878 0:10a064cee806 366 MAFLR=0.1*(msg.data[4]+256*msg.data[5])-65536;
m181878 0:10a064cee806 367 }
m181878 0:10a064cee806 368 else
m181878 0:10a064cee806 369 {
m181878 0:10a064cee806 370 MAFLR=0.1*(msg.data[4]+256*msg.data[5]);
m181878 0:10a064cee806 371 }
m181878 0:10a064cee806 372 }
m181878 0:10a064cee806 373 if (msg.id==218101832) //PE9
m181878 0:10a064cee806 374 {
m181878 0:10a064cee806 375 // Lambda #1 Measured
m181878 0:10a064cee806 376 if ((msg.data[0]+256*msg.data[1])>32767)
m181878 0:10a064cee806 377 {
m181878 0:10a064cee806 378 L1M=0.1*(msg.data[0]+256*msg.data[1])-65536;
m181878 0:10a064cee806 379 }
m181878 0:10a064cee806 380 else
m181878 0:10a064cee806 381 {
m181878 0:10a064cee806 382 L1M=0.1*(msg.data[0]+256*msg.data[1]);
m181878 0:10a064cee806 383 }
m181878 0:10a064cee806 384 // Lambda #2 Measured
m181878 0:10a064cee806 385 if ((msg.data[2]+256*msg.data[3])>32767)
m181878 0:10a064cee806 386 {
m181878 0:10a064cee806 387 L2M=0.1*(msg.data[2]+256*msg.data[3])-65536;
m181878 0:10a064cee806 388 }
m181878 0:10a064cee806 389 else
m181878 0:10a064cee806 390 {
m181878 0:10a064cee806 391 L2M=0.1*(msg.data[2]+256*msg.data[3]);
m181878 0:10a064cee806 392 }
m181878 0:10a064cee806 393 // Target Lambda
m181878 0:10a064cee806 394 if ((msg.data[4]+256*msg.data[5])>32767)
m181878 0:10a064cee806 395 {
m181878 0:10a064cee806 396 TL=0.1*(msg.data[4]+256*msg.data[5])-65536;
m181878 0:10a064cee806 397 }
m181878 0:10a064cee806 398 else
m181878 0:10a064cee806 399 {
m181878 0:10a064cee806 400 TL=0.1*(msg.data[4]+256*msg.data[5]);
m181878 0:10a064cee806 401 }
m181878 0:10a064cee806 402 }
m181878 0:10a064cee806 403 if (msg.id==218102344) //PE11
m181878 0:10a064cee806 404 {
m181878 0:10a064cee806 405 // Percent Slip
m181878 0:10a064cee806 406 if ((msg.data[0]+256*msg.data[1])>32767)
m181878 0:10a064cee806 407 {
m181878 0:10a064cee806 408 PS=0.1*(msg.data[0]+256*msg.data[1])-65536;
m181878 0:10a064cee806 409 }
m181878 0:10a064cee806 410 else
m181878 0:10a064cee806 411 {
m181878 0:10a064cee806 412 PS=0.1*(msg.data[0]+256*msg.data[1]);
m181878 0:10a064cee806 413 }
m181878 0:10a064cee806 414 // Driven Wheel Rate of Change
m181878 0:10a064cee806 415 if ((msg.data[2]+256*msg.data[3])>32767)
m181878 0:10a064cee806 416 {
m181878 0:10a064cee806 417 DWRC=0.1*(msg.data[2]+256*msg.data[3])-65536;
m181878 0:10a064cee806 418 }
m181878 0:10a064cee806 419 else
m181878 0:10a064cee806 420 {
m181878 0:10a064cee806 421 DWRC=0.1*(msg.data[2]+256*msg.data[3]);
m181878 0:10a064cee806 422 }
m181878 0:10a064cee806 423 // Desired Percent Slip
m181878 0:10a064cee806 424 if ((msg.data[4]+256*msg.data[5])>32767)
m181878 0:10a064cee806 425 {
m181878 0:10a064cee806 426 DPS=0.1*(msg.data[4]+256*msg.data[5])-65536;
m181878 0:10a064cee806 427 }
m181878 0:10a064cee806 428 else
m181878 0:10a064cee806 429 {
m181878 0:10a064cee806 430 DPS=0.1*(msg.data[4]+256*msg.data[5]);
m181878 0:10a064cee806 431 }
m181878 0:10a064cee806 432 }
m181878 0:10a064cee806 433 if (msg.id==218102600) //PE12
m181878 0:10a064cee806 434 {
m181878 0:10a064cee806 435 // Driven Avg Wheel Speed
m181878 0:10a064cee806 436 DAWS=0.1*(msg.data[0]+256*msg.data[1]);
m181878 0:10a064cee806 437 // Non-Driven Avg Wheel Speed
m181878 0:10a064cee806 438 NDAWS=0.1*(msg.data[2]+256*msg.data[3]);
m181878 0:10a064cee806 439 // Ignition Comp
m181878 0:10a064cee806 440 if ((msg.data[4]+256*msg.data[5])>32767)
m181878 0:10a064cee806 441 {
m181878 0:10a064cee806 442 IC=0.1*(msg.data[4]+256*msg.data[5])-65536;
m181878 0:10a064cee806 443 }
m181878 0:10a064cee806 444 else
m181878 0:10a064cee806 445 {
m181878 0:10a064cee806 446 IC=0.1*(msg.data[4]+256*msg.data[5]);
m181878 0:10a064cee806 447 }
m181878 0:10a064cee806 448 // Ignition Cut Percent
m181878 0:10a064cee806 449 if ((msg.data[6]+256*msg.data[7])>32767)
m181878 0:10a064cee806 450 {
m181878 0:10a064cee806 451 ICP=0.1*(msg.data[6]+256*msg.data[7])-65536;
m181878 0:10a064cee806 452 }
m181878 0:10a064cee806 453 else
m181878 0:10a064cee806 454 {
m181878 0:10a064cee806 455 ICP=0.1*(msg.data[6]+256*msg.data[7]);
m181878 0:10a064cee806 456 }
m181878 0:10a064cee806 457 }
m181878 0:10a064cee806 458 if (msg.id==218102856) //PE13
m181878 0:10a064cee806 459 {
m181878 0:10a064cee806 460 // Driven Wheel Speed #1
m181878 0:10a064cee806 461 DWS1=0.1*(msg.data[0]+256*msg.data[1]);
m181878 0:10a064cee806 462 // Driven Wheel Speed #2
m181878 0:10a064cee806 463 DWS2=0.1*(msg.data[2]+256*msg.data[3]);
m181878 0:10a064cee806 464 // Non-Driven Wheel Speed #1
m181878 0:10a064cee806 465 NDWS1=0.1*(msg.data[4]+256*msg.data[5]);
m181878 0:10a064cee806 466 // Non-Driven Wheel Speed #2
m181878 0:10a064cee806 467 NDWS2=0.1*(msg.data[6]+256*msg.data[7]);
m181878 0:10a064cee806 468 }
m181878 0:10a064cee806 469 if (msg.id==218103112) //PE14
m181878 0:10a064cee806 470 {
m181878 0:10a064cee806 471 // Fuel Comp Accel
m181878 0:10a064cee806 472 if ((msg.data[0]+256*msg.data[1])>32767)
m181878 0:10a064cee806 473 {
m181878 0:10a064cee806 474 FCA=0.1*(msg.data[0]+256*msg.data[1])-65536;
m181878 0:10a064cee806 475 }
m181878 0:10a064cee806 476 else
m181878 0:10a064cee806 477 {
m181878 0:10a064cee806 478 FCA=0.1*(msg.data[0]+256*msg.data[1]);
m181878 0:10a064cee806 479 }
m181878 0:10a064cee806 480 // Fuel Comp Starting
m181878 0:10a064cee806 481 if ((msg.data[2]+256*msg.data[3])>32767)
m181878 0:10a064cee806 482 {
m181878 0:10a064cee806 483 FCS=0.1*(msg.data[2]+256*msg.data[3])-65536;
m181878 0:10a064cee806 484 }
m181878 0:10a064cee806 485 else
m181878 0:10a064cee806 486 {
m181878 0:10a064cee806 487 FCS=0.1*(msg.data[2]+256*msg.data[3]);
m181878 0:10a064cee806 488 }
m181878 0:10a064cee806 489 // Fuel Comp Air Temp
m181878 0:10a064cee806 490 if ((msg.data[4]+256*msg.data[5])>32767)
m181878 0:10a064cee806 491 {
m181878 0:10a064cee806 492 FCAT=0.1*(msg.data[4]+256*msg.data[5])-65536;
m181878 0:10a064cee806 493 }
m181878 0:10a064cee806 494 else
m181878 0:10a064cee806 495 {
m181878 0:10a064cee806 496 FCAT=0.1*(msg.data[4]+256*msg.data[5]);
m181878 0:10a064cee806 497 }
m181878 0:10a064cee806 498 // Fuel Comp Coolant Temp
m181878 0:10a064cee806 499 if ((msg.data[6]+256*msg.data[7])>32767)
m181878 0:10a064cee806 500 {
m181878 0:10a064cee806 501 FCCT=0.1*(msg.data[6]+256*msg.data[7])-65536;
m181878 0:10a064cee806 502 }
m181878 0:10a064cee806 503 else
m181878 0:10a064cee806 504 {
m181878 0:10a064cee806 505 FCCT=0.1*(msg.data[6]+256*msg.data[7]);
m181878 0:10a064cee806 506 }
m181878 0:10a064cee806 507 }
m181878 0:10a064cee806 508 if (msg.id==218103368) //PE15
m181878 0:10a064cee806 509 {
m181878 0:10a064cee806 510 // Fuel Comp Barometer
m181878 0:10a064cee806 511 if ((msg.data[0]+256*msg.data[1])>32767)
m181878 0:10a064cee806 512 {
m181878 0:10a064cee806 513 FCB=0.1*(msg.data[0]+256*msg.data[1])-65536;
m181878 0:10a064cee806 514 }
m181878 0:10a064cee806 515 else
m181878 0:10a064cee806 516 {
m181878 0:10a064cee806 517 FCB=0.1*(msg.data[0]+256*msg.data[1]);
m181878 0:10a064cee806 518 }
m181878 0:10a064cee806 519 // Fuel Comp MAP
m181878 0:10a064cee806 520 if ((msg.data[2]+256*msg.data[3])>32767)
m181878 0:10a064cee806 521 {
m181878 0:10a064cee806 522 FCM=0.1*(msg.data[2]+256*msg.data[3])-65536;
m181878 0:10a064cee806 523 }
m181878 0:10a064cee806 524 else
m181878 0:10a064cee806 525 {
m181878 0:10a064cee806 526 FCM=0.1*(msg.data[2]+256*msg.data[3]);
m181878 0:10a064cee806 527 }
m181878 0:10a064cee806 528 }
m181878 0:10a064cee806 529 if (msg.id==218091592) //PE16
m181878 0:10a064cee806 530 {
m181878 0:10a064cee806 531 // Ignition Comp-Air Temp
m181878 0:10a064cee806 532 if ((msg.data[0]+256*msg.data[1])>32767)
m181878 0:10a064cee806 533 {
m181878 0:10a064cee806 534 ICAT=0.1*(msg.data[0]+256*msg.data[1])-65536;
m181878 0:10a064cee806 535 }
m181878 0:10a064cee806 536 else
m181878 0:10a064cee806 537 {
m181878 0:10a064cee806 538 ICAT=0.1*(msg.data[0]+256*msg.data[1]);
m181878 0:10a064cee806 539 }
m181878 0:10a064cee806 540 // Ignition Comp-Coolant Temp
m181878 0:10a064cee806 541 if ((msg.data[2]+256*msg.data[3])>32767)
m181878 0:10a064cee806 542 {
m181878 0:10a064cee806 543 ICCT=0.1*(msg.data[2]+256*msg.data[3])-65536;
m181878 0:10a064cee806 544 }
m181878 0:10a064cee806 545 else
m181878 0:10a064cee806 546 {
m181878 0:10a064cee806 547 ICCT=0.1*(msg.data[2]+256*msg.data[3]);
m181878 0:10a064cee806 548 }
m181878 0:10a064cee806 549 // Ignition Comp-Barometer
m181878 0:10a064cee806 550 if ((msg.data[4]+256*msg.data[5])>32767)
m181878 0:10a064cee806 551 {
m181878 0:10a064cee806 552 ICB=0.1*(msg.data[4]+256*msg.data[5])-65536;
m181878 0:10a064cee806 553 }
m181878 0:10a064cee806 554 else
m181878 0:10a064cee806 555 {
m181878 0:10a064cee806 556 ICB=0.1*(msg.data[4]+256*msg.data[5]);
m181878 0:10a064cee806 557 }
m181878 0:10a064cee806 558 // Ignition Comp-MAP
m181878 0:10a064cee806 559 if ((msg.data[6]+256*msg.data[7])>32767)
m181878 0:10a064cee806 560 {
m181878 0:10a064cee806 561 ICM=0.1*(msg.data[6]+256*msg.data[7])-65536;
m181878 0:10a064cee806 562 }
m181878 0:10a064cee806 563 else
m181878 0:10a064cee806 564 {
m181878 0:10a064cee806 565 ICM=0.1*(msg.data[6]+256*msg.data[7]);
m181878 0:10a064cee806 566 }
m181878 0:10a064cee806 567 }*/
m181878 0:10a064cee806 568 }
m181878 0:10a064cee806 569 wait(0.01);
m181878 0:10a064cee806 570 }
m181878 0:10a064cee806 571 } // End Main Function