Author: Not specified Language: cpp
Description: Not specified Timestamp: 2018-07-03 16:36:22 +0000
View raw paste Reply
  1. #include <SoftwareSerial.h>
  2. #include <TinyGPS.h>
  3. #include <math.h>
  4. #include <SPI.h>
  5. #include <mcp_can.h>
  6.  
  7.  
  8. // LOOP_TIMES
  9. #define LOOP_TIME_GLDC        500
  10. #define LOOP_TIME_CAN         500
  11. #define LOOP_TIME_GPS         100
  12. #define LOOP_TIME_TELEMETRY   800
  13. #define LOOP_TIME_TEMP        10000
  14. #define LOOP_TIME_ERROR       100
  15. #define LOOP_TIME_NANO        500
  16.  
  17. //LIMITS
  18. #define MAX_TEMP          60
  19. #define MAX_CURRENT       40
  20. #define MIN_CELL_VOLTAGE  2.0
  21. #define MIN_BAT_VOLTAGE   100.00
  22.  
  23.  
  24. // ERROR STATES
  25. #define STATE_READY                 0  
  26. #define OVER_TEMP_ERROR             1
  27. #define EMS_ERROR                   2
  28. #define OVER_CURRENT_ERROR          3
  29. #define LOW_BATTERY_VOLTAGE_ERROR   4
  30. #define LOW_CELL_VOLTAGE_ERROR      5
  31. int vehicle_error_holder = STATE_READY;
  32. int temp_error_flag;
  33. int ems_error_flag;
  34. int current_error_flag;
  35. int battery_voltage_error_flag;
  36. int cell_voltage_error_flag;
  37. String error_state = "";
  38.  
  39.  
  40. //Temperature
  41. #define FLASHER               7
  42. #define TEMPERATURE_PIN       A0
  43. int     temp_sum = 0;
  44. int     temp_cnt = 0;
  45. float   temperature = 0;
  46. String  temperature_data = "";
  47.  
  48. //CAN
  49. String can_data;
  50.  
  51. // Motor controller CAN base address and packet offsets
  52. #define MC_CAN_BASE    0x400     // High = Serial Number             Low = "TRIa" string
  53. #define MC_LIMITS      0x01      // High = Active Motor              Low = Error & Limit flags
  54. #define MC_BUS         0x02      // High = Bus Current               Low = Bus Voltage
  55. #define MC_VELOCITY    0x03      // High = Velocity (m/s)            Low = Velocity (rpm)
  56. #define MC_PHASE       0x04      // High = Phase A Current           Low = Phase B Current
  57. #define MC_V_VECTOR    0x05      // High = Vd vector                 Low = Vq vector
  58. #define MC_I_VECTOR    0x06      // High = Id vector                 Low = Iq vector
  59. #define MC_BEMF_VECTOR 0x07      // High = BEMFd vector              Low = BEMFq vector
  60. #define MC_RAIL1       0x08      // High = 15V                       Low = 1.65V
  61. #define MC_RAIL2       0x09      // High = 2.5V                      Low = 1.2V
  62. #define MC_FAN         0x0A      // High = Fan speed (rpm)           Low = Fan drive (%)
  63. #define MC_TEMP1       0x0B      // High = Heatsink Temp             Low = Motor Temp
  64. #define MC_TEMP2       0x0C      // High = Inlet Temp                Low = CPU Temp
  65. #define MC_TEMP3       0x0D      // High = Outlet Temp               Low = Capacitor Temp
  66. #define MC_CUMULATIVE  0x0E      // High = DC Bus AmpHours           Low = Odometer
  67.  
  68. // Driver controls CAN base address and packet offsets
  69. #define DC_CAN_BASE    0x500     // High = Serial Number             Low = "TRIb" string
  70. #define DC_DRIVE       0x01      // High = Motor Current Setpoint    Low = Motor Velocity Setpoint
  71. #define DC_POWER       0x02      // High = Bus Current Setpoint      Low = Unused
  72. #define DC_RESET       0x03      // High = Unused                    Low = Unused
  73. #define DC_SWITCH      0x04      // High = Switch position           Low = Switch state change
  74.  
  75.  
  76. MCP_CAN CAN(53);      // Set CS to pin 53
  77.  
  78.  
  79. unsigned long ID;
  80. byte len = 0;
  81. byte buf[8];
  82.  
  83. int fps_counter = 0;
  84.  
  85. unsigned char bus_current_data[4] = { 0, 0, 0, 0 };
  86. unsigned char bus_voltage_data[4] = { 0, 0, 0, 0 };
  87. unsigned char motor_velocity_data[4] = { 0, 0, 0, 0 };
  88. unsigned char vehicle_velocity_data[4] = { 0, 0, 0, 0 };
  89. unsigned char motor_temperature_data[4] = { 0, 0, 0, 0 };
  90. unsigned char heatsink_temperature_data[4] = { 0, 0, 0, 0 };
  91. unsigned char odometer_data[4] = { 0, 0, 0, 0 };
  92. unsigned char dc_bus_amphours_data[4] = { 0, 0, 0, 0 };
  93. unsigned char set_velocity_data[4] = { 0, 0, 0, 0 };
  94. unsigned char set_current_data[4] = { 0, 0, 0, 0 };
  95.  
  96.  
  97. unsigned char holder[4] = { 0, 0, 0, 0 };
  98. int remainder[4][8] = { 0 };
  99. int counter = 0;
  100. int lag_counter = 0;
  101.  
  102. //BMS Data
  103. int     BMS_receive_index = 0;
  104. String  Consumpted_Energy_str;
  105. String  Battery_Current_str;
  106. String  Battery_Voltage_str;
  107. String Battery_SoC_str;
  108. String  worst_cell_address_str;
  109. String  worst_cell_voltage_str;
  110. String  max_temperature_sensor_str;
  111. String  max_battery_temperature_str;
  112. float   Consumpted_Energy = 0;
  113. float   Battery_Current = 0;
  114. float   Battery_Voltage = 0;
  115. float   Battery_SoC = 0;
  116. float   worst_cell_voltage = 0;
  117. float   max_battery_temperature = 0;
  118. int     worst_cell_address = 0;
  119. int     max_temperature_sensor = 0;
  120.  
  121.  
  122. //CAN Data
  123. float bus_current;
  124. float bus_voltage;
  125. float motor_velocity;
  126. float vehicle_velocity;
  127. float motor_temperature;
  128. float heatsink_temperature;
  129. float odometer;
  130. float dc_bus_amphours;
  131. float set_velocity;
  132. float set_current;
  133. float myPower;
  134. float myConsumption;
  135. float averageVelocity = 45.00;
  136.  
  137.  
  138.  
  139. //GPS
  140. SoftwareSerial gpsSerial(10, 11); // RX, TX
  141. String gpsdump(TinyGPS &gps);
  142. TinyGPS gps;
  143. String gps_data;
  144. bool GpsCame;
  145.  
  146. //GLDC
  147. SoftwareSerial glcdSerial(9, 8); // RX, TX
  148. String glcd_data;
  149.  
  150.  
  151. //Time Holders
  152. unsigned long lastMillisCAN = 0;
  153. unsigned long lastMillisGlcd = 0;
  154. unsigned long lastMillisSend = 0;
  155. unsigned long lastMillisTemp = 0;
  156. unsigned long lastMillisGps = 0;
  157. unsigned long lastMillisError = 0;
  158. unsigned long lastMillisNano = 0;
  159. unsigned long lastMillisFps  = 0;
  160.  
  161. //External Functions
  162. void can_read();
  163. void can_assign();
  164. void glcdWrite();
  165. double ByteToFloatingPoint(unsigned char data[]);
  166. String calculateTemperature();
  167. void checkErrorStates(void);
  168. void checkBatteryParameters(void);
  169.  
  170.  
  171.  
  172. //Nano Steering Wheel Controller
  173. byte true_data_flag = false;
  174. int nano_index = 0;
  175. int steering_angle = 0;
  176. float set_speed = 0;
  177. float brake = 0;
  178. String steering_angle_str;
  179. String set_speed_str;
  180. String brake_str;
  181. String nano_data = "";
  182.  
  183. void loop()
  184. {
  185.         can_read();
  186.         //Main Work
  187.         if (millis() - lastMillisCAN > LOOP_TIME_CAN)
  188.         {
  189.                 can_assign();
  190.                 lastMillisCAN = millis();
  191.         }
  192.  
  193.     if (Serial1.available() > 0) {
  194.       //  "*sterring_angle#set_speed#brake\n"  
  195.       //  "$#1000#28.95#1\n"  
  196.     char incomingChar = (char)Serial1.read();
  197.     if (!true_data_flag && incomingChar == '$') {
  198.       true_data_flag = true;
  199.     }
  200.    
  201.     if (true_data_flag && incomingChar == '#') {
  202.      nano_index++;
  203.      if(nano_index == 1)
  204.       steering_angle_str = "";
  205.      else if (nano_index == 2)
  206.       set_speed_str = "";
  207.      else if (nano_index == 3)
  208.       brake_str = "";
  209.     }
  210.     else if (true_data_flag && incomingChar == '\n')
  211.     {
  212.       steering_angle = steering_angle_str.toInt();
  213.       set_speed = set_speed_str.toFloat();
  214.       brake = brake_str.toInt();
  215.       Serial3.print( "*" + steering_angle_str + "\n");
  216.       Serial2.print( "*" + set_speed_str + "\n");
  217.       true_data_flag = false;
  218.       fps_counter++;
  219.          
  220.    }
  221.     else {
  222.       if(nano_index == 1)
  223.         steering_angle_str += char(incomingChar);
  224.      else if(nano_index == 2)
  225.         set_speed_str += char(incomingChar);
  226.      else if(nano_index == 3)
  227.         brake_str += char(incomingChar);
  228.     }
  229.   }
  230.  
  231.   if ( millis() - lastMillisFps > 1000 )
  232.   {
  233.     Serial.print("#" + String(fps_counter) + "\n");
  234.     fps_counter = 0;
  235.     lastMillisFps = millis();
  236.   }
  237.  
  238.  
  239. /*
  240.  
  241.         if (millis() - lastMillisGps > LOOP_TIME_GPS)
  242.         {
  243.                 while (gpsSerial.available())
  244.                 {
  245.                         char c = gpsSerial.read();
  246.                         //Serial.print(c);
  247.                         if (gps.encode(c))
  248.                         {
  249.                                 GpsCame = true;
  250.                         }
  251.                 }
  252.                 if (GpsCame)
  253.                 {
  254.                         gps_data = gpsdump(gps);
  255.                         GpsCame = false;
  256.                 }
  257.                 else
  258.                 {
  259.                         //gps_data = "-GPS Yok :)-";
  260.                 }
  261.                 lastMillisGps = millis();
  262.         }
  263.  */
  264.  
  265.  
  266.  
  267.   //Temp Measure
  268.   if (millis() - lastMillisTemp > LOOP_TIME_TEMP)
  269.   {
  270.     temp_sum += analogRead(TEMPERATURE_PIN);
  271.     temp_cnt++;
  272.     if (temp_cnt >= 10)
  273.     {
  274.        temperature_data = calculateTemperature();
  275.        temp_cnt = 0;
  276.        temp_sum = 0;
  277.     }
  278.     lastMillisTemp = millis();
  279.   }
  280.  
  281. /*
  282.   ////Read stuff from BMS
  283.     if (Serial1.available() > 0) {
  284.     char incomingChar = (char)Serial1.read();
  285.     if (incomingChar == '#') {
  286.       BMS_receive_index++;
  287.       if (BMS_receive_index == 1) {
  288.         Battery_Voltage_str = "";
  289.       }
  290.       else if (BMS_receive_index == 2) {
  291.         Battery_Current_str = "";
  292.       }
  293.       else if (BMS_receive_index == 3) {
  294.         Consumpted_Energy_str = "";
  295.       }
  296.       else if (BMS_receive_index == 4) {
  297.         Battery_SoC_str = "";
  298.       }
  299.       else if (BMS_receive_index == 5) {
  300.         worst_cell_address_str = "";
  301.       }
  302.       else if (BMS_receive_index == 6) {
  303.         worst_cell_voltage_str = "";
  304.       }
  305.       else if (BMS_receive_index == 7) {
  306.         max_temperature_sensor_str = "";
  307.       }
  308.       else if (BMS_receive_index == 8) {
  309.         max_battery_temperature_str = "";
  310.       }
  311.     }
  312.     else if (incomingChar == '\n')
  313.     {
  314.       Consumpted_Energy = Consumpted_Energy_str.toFloat();
  315.       Battery_Current = Battery_Current_str.toFloat();
  316.       Battery_Voltage = Battery_Voltage_str.toFloat();
  317.       Battery_SoC = Battery_SoC_str.toFloat();
  318.       worst_cell_address = worst_cell_address_str.toInt();
  319.       worst_cell_voltage = worst_cell_voltage_str.toFloat();
  320.       max_temperature_sensor = max_temperature_sensor_str.toInt();
  321.       max_battery_temperature = max_battery_temperature_str.toFloat();
  322.       BMS_receive_index = 0;
  323.     }
  324.     else {
  325.       if (BMS_receive_index == 1) {
  326.         Consumpted_Energy_str += char(incomingChar);
  327.       }
  328.       else if (BMS_receive_index == 2) {
  329.         Battery_Current_str += char(incomingChar);
  330.       }
  331.       else if (BMS_receive_index == 3) {
  332.         Battery_Voltage_str += char(incomingChar);
  333.       }
  334.       else if (BMS_receive_index == 4) {
  335.         Battery_SoC_str += char(incomingChar);
  336.       }
  337.       else if (BMS_receive_index == 5) {
  338.         worst_cell_address_str += char(incomingChar);
  339.       }
  340.       else if (BMS_receive_index == 6) {
  341.         worst_cell_voltage_str += char(incomingChar);
  342.       }
  343.       else if (BMS_receive_index == 7) {
  344.         max_temperature_sensor_str += char(incomingChar);
  345.       }
  346.       else if (BMS_receive_index == 8) {
  347.         max_battery_temperature_str += char(incomingChar);
  348.       }
  349.     }
  350.   }
  351.   */
  352. /*
  353.   // Checking Error States
  354.   if (millis() - lastMillisError > LOOP_TIME_ERROR)
  355.   {
  356.     checkBatteryParameters();
  357.     checkErrorStates();
  358.     lastMillisError = millis();
  359.   }
  360. */
  361.  
  362. /*
  363.     //Send Data to Telemetry
  364.   if (millis() - lastMillisSend > LOOP_TIME_TELEMETRY )
  365.   {
  366.     String allresult = ("*" + gps_data + temperature_data + can_data + "#Z" + String(lag_counter) + "\r\n");
  367.     lag_counter++;
  368.    
  369.     Serial3.print(allresult);
  370.     lastMillisSend = millis();
  371.   }
  372.    */
  373. }
View raw paste Reply