Erin O'Neill / Mbed OS CAN_Node

Wheel nodes for CAN messages

main.cpp@97:819c446d97e3, 2019-09-11 (annotated)

Committer:
fsae
Date:
Wed Sep 11 22:13:27 2019 +0000
Revision:
97:819c446d97e3
Parent:
96:45cf7ff97595 
For testing

Who changed what in which revision?

UserRevisionLine numberNew contents of line
Jonathan Austin 0:2757d7abb7d9 1 #include "mbed.h"
mbed_official 82:abf1b1785bd7 2 #include "stats_report.h"
fsae 96:45cf7ff97595 3 #include <array>
fsae 96:45cf7ff97595 4
fsae 96:45cf7ff97595 5 #define FL 0
fsae 96:45cf7ff97595 6 #define FR 1
fsae 96:45cf7ff97595 7 #define RR 2
fsae 96:45cf7ff97595 8 #define RL 3
fsae 96:45cf7ff97595 9 #define CAN_NODE RL
fsae 96:45cf7ff97595 10
fsae 96:45cf7ff97595 11 #define SAMPLING_RATE 0.01 // in sec
fsae 96:45cf7ff97595 12 #define SAMPLING_RATE_WS 0.1 // in sec, for pulse counting
fsae 96:45cf7ff97595 13 #define CAN1_BAUD_RATE 1000000 // 1 Mb/s
fsae 96:45cf7ff97595 14 #define FAULT_RATE 0.25 // in sec
Jonathan Austin 0:2757d7abb7d9 15
fsae 95:5ae63358609b 16 Serial pc(USBTX, USBRX); // Serial COM to PC
fsae 95:5ae63358609b 17
fsae 96:45cf7ff97595 18 InterruptIn event(p18);
fsae 96:45cf7ff97595 19 Timer t;
fsae 96:45cf7ff97595 20 Timer fault_timer;
Jonathan Austin 0:2757d7abb7d9 21
fsae 96:45cf7ff97595 22 Ticker fault;
fsae 96:45cf7ff97595 23 double newestEdgeTime = 0;
fsae 96:45cf7ff97595 24 int16_t rpm_fault = 0;
fsae 95:5ae63358609b 25
fsae 95:5ae63358609b 26 Ticker sensors;
fsae 96:45cf7ff97595 27 Ticker wheelSpeedCounter;
fsae 95:5ae63358609b 28 bool sensorFlag = 0;
mbed_official 88:bea4f2daa48c 29
fsae 97:819c446d97e3 30 Ticker test;
fsae 97:819c446d97e3 31
fsae 96:45cf7ff97595 32 // Variables for Weel Speed Functions
fsae 96:45cf7ff97595 33 int16_t rpm_integer = 0;
fsae 96:45cf7ff97595 34 double rpm_calculation;
fsae 96:45cf7ff97595 35 double rpm_average = 0;
fsae 96:45cf7ff97595 36 double rpm_history [10] = {0, 0, 0, 0, 0, 0, 0, 0, 0, 0};
fsae 96:45cf7ff97595 37 double t_o = 0;
fsae 96:45cf7ff97595 38 double t_f;
fsae 96:45cf7ff97595 39 double freq;
fsae 96:45cf7ff97595 40 int window = 10;
fsae 96:45cf7ff97595 41 int index = 1;
fsae 96:45cf7ff97595 42 int ws_state = 0;
fsae 96:45cf7ff97595 43 double rpm, new_rpm;
fsae 96:45cf7ff97595 44 int i = 0, num_pulses = 0, error_count = 0, max_error_count = 25;
fsae 96:45cf7ff97595 45
fsae 96:45cf7ff97595 46 // Variables for CAN Communication
fsae 96:45cf7ff97595 47 int can_id;
fsae 96:45cf7ff97595 48 int can_multiplier = 1000;
fsae 95:5ae63358609b 49 CAN can1(p30, p29); // CAN1 pin init
fsae 95:5ae63358609b 50 char data[8];
fsae 95:5ae63358609b 51
fsae 96:45cf7ff97595 52
fsae 96:45cf7ff97595 53 //Variables for analog inputs
fsae 96:45cf7ff97595 54
fsae 96:45cf7ff97595 55 // LP is connected to analog0 which goes to pin 15 which is A0
fsae 96:45cf7ff97595 56 // BT is connected to analog1 which goes to pin 16 which is A2
fsae 96:45cf7ff97595 57
fsae 95:5ae63358609b 58
fsae 96:45cf7ff97595 59 AnalogIn input1(A0); // p15, linpot
fsae 96:45cf7ff97595 60 AnalogIn input2(A1); // p16, brake temp
fsae 96:45cf7ff97595 61 //AnalogIn input3(A3); // p17, strain gauge
fsae 96:45cf7ff97595 62 uint32_t analogIn0, analogIn1, analogIn2, analogIn3;
fsae 96:45cf7ff97595 63 int analogToVoltage = 19859;
fsae 96:45cf7ff97595 64
fsae 96:45cf7ff97595 65 // Function Raises flag every SAMPLING_RATE
fsae 96:45cf7ff97595 66 void readSensors()
fsae 96:45cf7ff97595 67 {
fsae 96:45cf7ff97595 68 sensorFlag = 1;
fsae 95:5ae63358609b 69 }
mbed_official 82:abf1b1785bd7 70
fsae 96:45cf7ff97595 71 void checkWSFault()
fsae 95:5ae63358609b 72 {
fsae 96:45cf7ff97595 73 if(rpm_fault == 0) {
fsae 96:45cf7ff97595 74 double currTime = t;
fsae 96:45cf7ff97595 75 if((currTime - newestEdgeTime) > FAULT_RATE) {
fsae 96:45cf7ff97595 76 rpm_fault = 1;
fsae 96:45cf7ff97595 77 rpm_integer = 0;
fsae 96:45cf7ff97595 78 rpm_history[0] = 0;
fsae 96:45cf7ff97595 79 rpm_history[1] = 0;
fsae 96:45cf7ff97595 80 rpm_history[2] = 0;
fsae 96:45cf7ff97595 81 rpm_history[3] = 0;
fsae 96:45cf7ff97595 82 rpm_history[4] = 0;
fsae 96:45cf7ff97595 83 rpm_history[5] = 0;
fsae 96:45cf7ff97595 84 rpm_history[6] = 0;
fsae 96:45cf7ff97595 85 rpm_history[7] = 0;
fsae 96:45cf7ff97595 86 rpm_history[8] = 0;
fsae 96:45cf7ff97595 87 rpm_history[9] = 0;
fsae 96:45cf7ff97595 88
fsae 96:45cf7ff97595 89 rpm_average = 0;
fsae 96:45cf7ff97595 90 rpm_integer = 0;
mbed_official 88:bea4f2daa48c 91 }
Jonathan Austin 0:2757d7abb7d9 92 }
Jonathan Austin 0:2757d7abb7d9 93 }
fsae 96:45cf7ff97595 94
fsae 96:45cf7ff97595 95 // Wheel Speed Decoding:
fsae 96:45cf7ff97595 96 // ***** Moving average method ******
fsae 96:45cf7ff97595 97 //void fall()
fsae 96:45cf7ff97595 98 //{
fsae 96:45cf7ff97595 99 // newestEdgeTime = t;
fsae 96:45cf7ff97595 100 // rpm_fault = 0;
fsae 96:45cf7ff97595 101 // if(index == window) index = 0;
fsae 96:45cf7ff97595 102 //
fsae 96:45cf7ff97595 103 // t_f = t;
fsae 96:45cf7ff97595 104 // freq = (1/(t_f - t_o));
fsae 96:45cf7ff97595 105 // t_o = t_f;
fsae 96:45cf7ff97595 106 //
fsae 96:45cf7ff97595 107 // rpm_calculation = (60/20 * freq)/(window-1);
fsae 96:45cf7ff97595 108 //
fsae 96:45cf7ff97595 109 // rpm_history[index%window] = rpm_calculation;
fsae 96:45cf7ff97595 110 //
fsae 96:45cf7ff97595 111 // rpm_average = rpm_average + rpm_calculation - rpm_history[(++index)%window];
fsae 96:45cf7ff97595 112 //
fsae 96:45cf7ff97595 113 // rpm_integer = static_cast<uint16_t>(rpm_average);
fsae 96:45cf7ff97595 114 //}
fsae 96:45cf7ff97595 115
fsae 96:45cf7ff97595 116 // ***** No filtering pulse timing method ******
fsae 96:45cf7ff97595 117 void fall()
fsae 96:45cf7ff97595 118 { // create interrupt function
fsae 96:45cf7ff97595 119 rpm_fault = 0;
fsae 96:45cf7ff97595 120 t_f = t;
fsae 96:45cf7ff97595 121 freq = (1/(t_f - t_o));
fsae 96:45cf7ff97595 122 rpm_integer = 60/20 * freq; // 60 seconds / magnet passes 20 holes in spindle per revolution
fsae 96:45cf7ff97595 123
fsae 96:45cf7ff97595 124 t_o = t_f;
fsae 96:45cf7ff97595 125 }
fsae 96:45cf7ff97595 126
fsae 96:45cf7ff97595 127 // ***** Pulse counting method ******
fsae 96:45cf7ff97595 128 //void fall()
fsae 96:45cf7ff97595 129 //{
fsae 96:45cf7ff97595 130 // num_pulses++;
fsae 96:45cf7ff97595 131 //}
fsae 96:45cf7ff97595 132 //
fsae 96:45cf7ff97595 133 //void pulsecount()
fsae 96:45cf7ff97595 134 //{
fsae 96:45cf7ff97595 135 // rpm_integer = 60 * num_pulses/(20 * SAMPLING_RATE_WS); // num_pulses * SAMPLING_RATE is frequency * 60 sec / 20 holes per rev is rpm
fsae 96:45cf7ff97595 136 // num_pulses = 0;
fsae 96:45cf7ff97595 137 //}
fsae 96:45cf7ff97595 138
fsae 96:45cf7ff97595 139 char toCharH(uint32_t value)
fsae 96:45cf7ff97595 140 {
fsae 96:45cf7ff97595 141 uint16_t val = static_cast<uint16_t>(value);
fsae 96:45cf7ff97595 142 return (char) ((uint16_t) (val >> 8));
fsae 96:45cf7ff97595 143 }
fsae 96:45cf7ff97595 144
fsae 96:45cf7ff97595 145 char toCharL(uint32_t value)
fsae 96:45cf7ff97595 146 {
fsae 96:45cf7ff97595 147 uint16_t val = static_cast<uint16_t>(value);
fsae 96:45cf7ff97595 148 return (char) val;
fsae 96:45cf7ff97595 149 }
fsae 96:45cf7ff97595 150
fsae 96:45cf7ff97595 151 uint32_t convToVoltage(uint32_t ADC)
fsae 96:45cf7ff97595 152 {
fsae 96:45cf7ff97595 153 //This function assumes ADC is taking with read_u16 ie. a value from 0 to FFFF representing voltage
fsae 96:45cf7ff97595 154 uint32_t voltage = (ADC * can_multiplier) / analogToVoltage;
fsae 96:45cf7ff97595 155 return voltage;
fsae 96:45cf7ff97595 156 }
fsae 96:45cf7ff97595 157
fsae 96:45cf7ff97595 158 uint32_t getBrakeTemperature(uint32_t voltage)
fsae 96:45cf7ff97595 159 {
fsae 96:45cf7ff97595 160 //This function is from the INFKL-800 infrared Temperature Sensor Data Sheet
fsae 96:45cf7ff97595 161 //Recall that voltages on chip are passed through voltage divider shifting 5V to 3.3V
fsae 96:45cf7ff97595 162 //Voltages are represented using fixed int @ 1000 times size
fsae 96:45cf7ff97595 163 uint32_t temp = 0;
fsae 96:45cf7ff97595 164 if((voltage*303) > 100000) {
fsae 96:45cf7ff97595 165 temp = voltage*303 - 100000;
fsae 96:45cf7ff97595 166 }
fsae 96:45cf7ff97595 167
fsae 96:45cf7ff97595 168 return temp/100; //Temperature is represented as fixed int @10 times the size
fsae 96:45cf7ff97595 169 }
fsae 96:45cf7ff97595 170
fsae 96:45cf7ff97595 171 uint32_t getLinPot(uint32_t voltage)
fsae 96:45cf7ff97595 172 {
fsae 96:45cf7ff97595 173 uint32_t position = 0;
fsae 96:45cf7ff97595 174
fsae 96:45cf7ff97595 175 switch(CAN_NODE) {
fsae 96:45cf7ff97595 176 case FL: //Transfer Function of 'FL' y = -0.2379x + 837.95
fsae 96:45cf7ff97595 177 position = (8379500 - (2379*voltage))/1000;
fsae 96:45cf7ff97595 178 return position;
fsae 96:45cf7ff97595 179 case FR: //Transfer Function of 'A' y = -0.2235x + 731.91
fsae 96:45cf7ff97595 180 position = (7319100 - (2235*voltage))/1000;
fsae 96:45cf7ff97595 181 return position;
fsae 96:45cf7ff97595 182 case RR: //Transfer Function of 'RR' y = -0.234x + 781.61
fsae 96:45cf7ff97595 183 position = (7816100 - (2340*voltage))/1000;
fsae 96:45cf7ff97595 184 return position;
fsae 96:45cf7ff97595 185 case RL: //Transfer Function of 'RL' y = 0.3291x - 2.8199
fsae 96:45cf7ff97595 186 position = (7816100 - (2340*voltage))/1000;
fsae 96:45cf7ff97595 187 return position;
fsae 96:45cf7ff97595 188 }
fsae 96:45cf7ff97595 189
fsae 96:45cf7ff97595 190 return position; //Position is 10 times greater than actual position and unit is mm. ie 700 = 70mm
fsae 96:45cf7ff97595 191 }
fsae 96:45cf7ff97595 192
fsae 97:819c446d97e3 193 void testCAN(){
fsae 97:819c446d97e3 194 char data1[8];
fsae 97:819c446d97e3 195 // Send CAN Message
fsae 97:819c446d97e3 196 can1.write(CANMessage(0x84, data1, 8, CANData, CANStandard));
fsae 97:819c446d97e3 197 }
fsae 97:819c446d97e3 198
fsae 96:45cf7ff97595 199 int main()
fsae 96:45cf7ff97595 200 {
fsae 96:45cf7ff97595 201 // Start WS Decoding
fsae 96:45cf7ff97595 202 t.start();
fsae 96:45cf7ff97595 203 event.fall(&fall);
fsae 96:45cf7ff97595 204
fsae 96:45cf7ff97595 205 // Start CAN
fsae 96:45cf7ff97595 206 can1.frequency(CAN1_BAUD_RATE);
fsae 96:45cf7ff97595 207
fsae 96:45cf7ff97595 208 // Sensor flag callback
fsae 96:45cf7ff97595 209 sensors.attach(&readSensors, SAMPLING_RATE);
fsae 96:45cf7ff97595 210 fault.attach(&checkWSFault, FAULT_RATE);
fsae 96:45cf7ff97595 211 //wheelSpeedCounter.attach(&pulsecount, SAMPLING_RATE_WS);
fsae 97:819c446d97e3 212 test.attach(&testCAN, 1);
fsae 96:45cf7ff97595 213
fsae 96:45cf7ff97595 214 switch(CAN_NODE) {
fsae 96:45cf7ff97595 215 case FL:
fsae 96:45cf7ff97595 216 can_id = 0x80;
fsae 96:45cf7ff97595 217 break;
fsae 96:45cf7ff97595 218 case FR:
fsae 96:45cf7ff97595 219 can_id = 0x81;
fsae 96:45cf7ff97595 220 break;
fsae 96:45cf7ff97595 221 case RR:
fsae 96:45cf7ff97595 222 can_id = 0x82;
fsae 96:45cf7ff97595 223 break;
fsae 96:45cf7ff97595 224 case RL:
fsae 96:45cf7ff97595 225 can_id = 0x83;
fsae 96:45cf7ff97595 226 break;
fsae 96:45cf7ff97595 227 }
fsae 96:45cf7ff97595 228
fsae 96:45cf7ff97595 229 while(1) {
fsae 96:45cf7ff97595 230
fsae 96:45cf7ff97595 231 if(sensorFlag) {
fsae 96:45cf7ff97595 232 sensorFlag = 0;
fsae 96:45cf7ff97595 233
fsae 96:45cf7ff97595 234 analogIn1 = getLinPot(convToVoltage(input1.read_u16()));
fsae 96:45cf7ff97595 235
fsae 96:45cf7ff97595 236 // Include BrakeTemp if FL or RL
fsae 96:45cf7ff97595 237 if(CAN_NODE == FL || CAN_NODE == RL){
fsae 96:45cf7ff97595 238 analogIn2 = getBrakeTemperature(convToVoltage(input2.read_u16()));
fsae 96:45cf7ff97595 239 analogIn3 = input2.read_u16();
fsae 96:45cf7ff97595 240 }
fsae 96:45cf7ff97595 241 else{
fsae 96:45cf7ff97595 242 analogIn2 = 0;
fsae 96:45cf7ff97595 243 }
fsae 96:45cf7ff97595 244 pc.printf("Analog1: %i Analog2: %i RPM_Fault: %i WheelSpeed: %i \n", analogIn1, analogIn2, rpm_fault, rpm_integer);
fsae 96:45cf7ff97595 245
fsae 96:45cf7ff97595 246 // Packaging data into CAN Message
fsae 96:45cf7ff97595 247 data[0] = toCharH(rpm_integer);
fsae 96:45cf7ff97595 248 data[1] = toCharL(rpm_integer);
fsae 96:45cf7ff97595 249 data[2] = toCharH(analogIn1);
fsae 96:45cf7ff97595 250 data[3] = toCharL(analogIn1);
fsae 96:45cf7ff97595 251 data[4] = toCharH(analogIn2);
fsae 96:45cf7ff97595 252 data[5] = toCharL(analogIn2);
fsae 96:45cf7ff97595 253 data[6] = toCharH(rpm_fault);
fsae 96:45cf7ff97595 254 data[7] = toCharL(rpm_fault);
fsae 96:45cf7ff97595 255
fsae 96:45cf7ff97595 256 // Send CAN Message
fsae 96:45cf7ff97595 257 can1.write(CANMessage(can_id, data, 8, CANData, CANStandard));
fsae 96:45cf7ff97595 258 }
fsae 96:45cf7ff97595 259 }
fsae 96:45cf7ff97595 260 }