Erin O'Neill
/
CAN_Node
Wheel nodes for CAN messages
main.cpp
- Committer:
- fsae
- Date:
- 2019-06-14
- Revision:
- 96:45cf7ff97595
- Parent:
- 95:5ae63358609b
- Child:
- 97:819c446d97e3
File content as of revision 96:45cf7ff97595:
#include "mbed.h" #include "stats_report.h" #include <array> #define FL 0 #define FR 1 #define RR 2 #define RL 3 #define CAN_NODE RL #define SAMPLING_RATE 0.01 // in sec #define SAMPLING_RATE_WS 0.1 // in sec, for pulse counting #define CAN1_BAUD_RATE 1000000 // 1 Mb/s #define FAULT_RATE 0.25 // in sec Serial pc(USBTX, USBRX); // Serial COM to PC InterruptIn event(p18); Timer t; Timer fault_timer; Ticker fault; double newestEdgeTime = 0; int16_t rpm_fault = 0; Ticker sensors; Ticker wheelSpeedCounter; bool sensorFlag = 0; // Variables for Weel Speed Functions int16_t rpm_integer = 0; double rpm_calculation; double rpm_average = 0; double rpm_history [10] = {0, 0, 0, 0, 0, 0, 0, 0, 0, 0}; double t_o = 0; double t_f; double freq; int window = 10; int index = 1; int ws_state = 0; double rpm, new_rpm; int i = 0, num_pulses = 0, error_count = 0, max_error_count = 25; // Variables for CAN Communication int can_id; int can_multiplier = 1000; CAN can1(p30, p29); // CAN1 pin init char data[8]; //Variables for analog inputs // LP is connected to analog0 which goes to pin 15 which is A0 // BT is connected to analog1 which goes to pin 16 which is A2 AnalogIn input1(A0); // p15, linpot AnalogIn input2(A1); // p16, brake temp //AnalogIn input3(A3); // p17, strain gauge uint32_t analogIn0, analogIn1, analogIn2, analogIn3; int analogToVoltage = 19859; // Function Raises flag every SAMPLING_RATE void readSensors() { sensorFlag = 1; } void checkWSFault() { if(rpm_fault == 0) { double currTime = t; if((currTime - newestEdgeTime) > FAULT_RATE) { rpm_fault = 1; rpm_integer = 0; rpm_history[0] = 0; rpm_history[1] = 0; rpm_history[2] = 0; rpm_history[3] = 0; rpm_history[4] = 0; rpm_history[5] = 0; rpm_history[6] = 0; rpm_history[7] = 0; rpm_history[8] = 0; rpm_history[9] = 0; rpm_average = 0; rpm_integer = 0; } } } // Wheel Speed Decoding: // ***** Moving average method ****** //void fall() //{ // newestEdgeTime = t; // rpm_fault = 0; // if(index == window) index = 0; // // t_f = t; // freq = (1/(t_f - t_o)); // t_o = t_f; // // rpm_calculation = (60/20 * freq)/(window-1); // // rpm_history[index%window] = rpm_calculation; // // rpm_average = rpm_average + rpm_calculation - rpm_history[(++index)%window]; // // rpm_integer = static_cast<uint16_t>(rpm_average); //} // ***** No filtering pulse timing method ****** void fall() { // create interrupt function rpm_fault = 0; t_f = t; freq = (1/(t_f - t_o)); rpm_integer = 60/20 * freq; // 60 seconds / magnet passes 20 holes in spindle per revolution t_o = t_f; } // ***** Pulse counting method ****** //void fall() //{ // num_pulses++; //} // //void pulsecount() //{ // rpm_integer = 60 * num_pulses/(20 * SAMPLING_RATE_WS); // num_pulses * SAMPLING_RATE is frequency * 60 sec / 20 holes per rev is rpm // num_pulses = 0; //} char toCharH(uint32_t value) { uint16_t val = static_cast<uint16_t>(value); return (char) ((uint16_t) (val >> 8)); } char toCharL(uint32_t value) { uint16_t val = static_cast<uint16_t>(value); return (char) val; } uint32_t convToVoltage(uint32_t ADC) { //This function assumes ADC is taking with read_u16 ie. a value from 0 to FFFF representing voltage uint32_t voltage = (ADC * can_multiplier) / analogToVoltage; return voltage; } uint32_t getBrakeTemperature(uint32_t voltage) { //This function is from the INFKL-800 infrared Temperature Sensor Data Sheet //Recall that voltages on chip are passed through voltage divider shifting 5V to 3.3V //Voltages are represented using fixed int @ 1000 times size uint32_t temp = 0; if((voltage*303) > 100000) { temp = voltage*303 - 100000; } return temp/100; //Temperature is represented as fixed int @10 times the size } uint32_t getLinPot(uint32_t voltage) { uint32_t position = 0; switch(CAN_NODE) { case FL: //Transfer Function of 'FL' y = -0.2379x + 837.95 position = (8379500 - (2379*voltage))/1000; return position; case FR: //Transfer Function of 'A' y = -0.2235x + 731.91 position = (7319100 - (2235*voltage))/1000; return position; case RR: //Transfer Function of 'RR' y = -0.234x + 781.61 position = (7816100 - (2340*voltage))/1000; return position; case RL: //Transfer Function of 'RL' y = 0.3291x - 2.8199 position = (7816100 - (2340*voltage))/1000; return position; } return position; //Position is 10 times greater than actual position and unit is mm. ie 700 = 70mm } int main() { // Start WS Decoding t.start(); event.fall(&fall); // Start CAN can1.frequency(CAN1_BAUD_RATE); // Sensor flag callback sensors.attach(&readSensors, SAMPLING_RATE); fault.attach(&checkWSFault, FAULT_RATE); //wheelSpeedCounter.attach(&pulsecount, SAMPLING_RATE_WS); switch(CAN_NODE) { case FL: can_id = 0x80; break; case FR: can_id = 0x81; break; case RR: can_id = 0x82; break; case RL: can_id = 0x83; break; } while(1) { if(sensorFlag) { sensorFlag = 0; analogIn1 = getLinPot(convToVoltage(input1.read_u16())); // Include BrakeTemp if FL or RL if(CAN_NODE == FL || CAN_NODE == RL){ analogIn2 = getBrakeTemperature(convToVoltage(input2.read_u16())); analogIn3 = input2.read_u16(); } else{ analogIn2 = 0; } pc.printf("Analog1: %i Analog2: %i RPM_Fault: %i WheelSpeed: %i \n", analogIn1, analogIn2, rpm_fault, rpm_integer); // Packaging data into CAN Message data[0] = toCharH(rpm_integer); data[1] = toCharL(rpm_integer); data[2] = toCharH(analogIn1); data[3] = toCharL(analogIn1); data[4] = toCharH(analogIn2); data[5] = toCharL(analogIn2); data[6] = toCharH(rpm_fault); data[7] = toCharL(rpm_fault); // Send CAN Message can1.write(CANMessage(can_id, data, 8, CANData, CANStandard)); } } }