SHARE
TWEET

Arduino sketch for steering angle sensor project

Hired-Goon Apr 10th, 2017 (edited) 92 Never
Not a member of Pastebin yet? Sign Up, it unlocks many cool features!
  1. /*
  2. Arduino Uno R3 using 2x DFRobot CAN BUS shield V2.0
  3. Intercept and scale steering angle sensor signals for Subaru vehicles which have been swapped to a different ratio steering rack
  4. Designed and tested on an AUDM MY12 Forester S-Edition swapped to an AUDM MY15 STI rack p/n 34110VA101
  5. Written 27/11/2016 by Hired-Goon - hiredgoon@iinet.net.au or hired-goon at ozfoz.com
  6.  
  7. Includes code from DFRobot CAN BUS shield V2.0 examples
  8. Includes code from various free online resources such as stackexchange.com
  9.  
  10. Covered under the MIT Licence, see full licence at the end of this file
  11. */
  12.  
  13. //Requires DFRobot CAN BUS Shield library found on DFRobots website: https://github.com/DFRobot/CAN_BUS/raw/master/mcp_can.zip
  14.  
  15. #include <SPI.h>
  16. #include <df_can.h>
  17.  
  18. //_________________________________________ User defined values ________________________________________________________________________
  19. const float scaleLevel = 1.3;           // What percentage should the signal scale? Must be a decimal number (not a percentage) e.g.  30% = 1.3
  20.  
  21.                                         // These CS pins are pin 10 on the DFRobot CAN BUS shield V2.0 - at least one needs to be bent away and jumpered to another pin, specify the pins being used here
  22. const int SPI_CS_PIN_Car = 10;          // Set Chip Select/Slave Select (aka CS or SS) pin for Car board - connected to main wiring loom
  23. const int SPI_CS_PIN_AngleSensor = 8;   // Set Chip Select/Slave Select (aka CS or SS) pin for AngleSensor board - connected to steering angle sensor
  24.  
  25. const int buzzerPin = 3;                // Set the pin the buzzer is connected to
  26. const int canAttempts = 5;              // How many attempts (1 attempt per second) to start CANBUS before beeping - will beep the first 3 times this number is reached (e.g. canAttempts == 5, beep at 5, 10 and 15 fails)
  27. bool speakerEnabled = true;             // If true a warning beeper (if optionally installed) will announce startup complete or error. Set to false to if speaker not installed.
  28. bool startupOKbeep = true;              // If true the beeper will sound on successful start up of the Arduino/CAN interface.
  29. bool disableOnDefogger = true;          // If true turning the defogger on then off.
  30. //______________________________________________________________________________________________________________________________________
  31.  
  32. MCPCAN CAN_Car(SPI_CS_PIN_Car);
  33. MCPCAN CAN_AngleSensor(SPI_CS_PIN_AngleSensor);
  34. unsigned long enabledMillis = 0;
  35. bool scalingEnabled = 1;
  36.  
  37. void setup()
  38. {
  39.     Serial.begin(250000);
  40.     pinMode(buzzerPin, OUTPUT);
  41.     canInit(CAN_Car, "Car");
  42.     canInit(CAN_AngleSensor, "AngleSensor");
  43.     if ((speakerEnabled) && (startupOKbeep)) beep(2); //if we've reached here, we've init'd correctly, beep twice to indicate the system is working
  44. }
  45.  
  46. void loop()
  47. {
  48.     INT32U canId = 0x00;
  49.     unsigned char len = 0;
  50.     unsigned char buf[8];
  51.     int angleData = 0;
  52.     if ((disableOnDefogger) && (CAN_MSGAVAIL == CAN_Car.checkReceive())) {
  53.         CAN_Car.readMsgBuf(&len, buf);
  54.         canId = CAN_Car.getCanId();
  55.         if ((canId == 0x514) && (((buf[7] >> 7) & 1) == 1)) //Check to see if we should toggle the scaling/sensor functionality, handbrake is enabled
  56.         {
  57.             if (((buf[3] & 1) == 0) && (enabledMillis == 0)) //Not already counting, defogger disabled
  58.             {
  59.                 //do nothing
  60.             }
  61.             else if (enabledMillis == 0) //Not already counting - store current time
  62.             {
  63.                 enabledMillis = millis();
  64.             }
  65.             else if ((millis() <= enabledMillis + 5000) && ((buf[3] & 1) == 0)) //Defogger is now disabled, less than 5 seconds have passed, toggle scaling boolean
  66.             {
  67.                 scalingEnabled ^= true; //inverts the current state, if true sets false, if false sets true. Requires key to ACC or OFF position to re-enable VDC
  68.                 beep(3); //half second beep to indicate scaling enabled state change
  69.                 enabledMillis = 0; //reset the time and wait for next enable
  70.             }
  71.             else if ((buf[3] & 1) == 0) //Reaching here means more than 5s passed, defogger disabled, reset the time and wait for next enable
  72.             {
  73.                 enabledMillis = 0;
  74.             }
  75.         }
  76.     }
  77.  
  78.     if ((scalingEnabled) && (CAN_MSGAVAIL == CAN_AngleSensor.checkReceive())) {
  79.         CAN_AngleSensor.readMsgBuf(&len, buf);
  80.         canId = CAN_AngleSensor.getCanId();
  81.        
  82.         if ((canId == 0x02) && (buf[1] != 128)) {
  83.  
  84.             //Byte 0 and byte 1 combine to form steering angle, signed 16 bit int little endian
  85.             //0 is middle, positive for angle left, negative for angle right
  86.             //With AUDM MY15 STI rack installed (p/n 34110VA101) I'm getting ~7000 full lock left and ~-7000 full lock right
  87.             angleData = buf[1] << 8 | buf[0];
  88.  
  89.             //Output to serial port the 3 bytes of interest
  90.             //Serial.print("in: "); Serial.print(buf[0]); Serial.print(" "); Serial.print(buf[1]); Serial.print(" "); Serial.println(buf[2]);
  91.  
  92.             //Scale up the angle data - angle data starts at -2 rather than 0 and works in fixed values of +/-16, thus the +2 then -2 later
  93.             angleData = round((angleData + 2) / 16 * scaleLevel) * 16 - 2;
  94.  
  95.             //Set scaled up angle back to CAN message
  96.             buf[0] = (angleData & 0xFF);
  97.             buf[1] = (angleData >> 8);
  98.  
  99.             //Prepare checksum byte (byte 2)
  100.             buf[2] = ((buf[0] & 0xF) ^ (buf[0] >> 4) ^ (buf[1] & 0xF) ^ (buf[1] >> 4) ^ (buf[2] & 0xF) ^ (buf[3] & 0xF) ^ (buf[3] >> 4) ^ (buf[4] & 0xF) ^ (buf[4] >> 4) ^ (buf[5] & 0xF) ^ (buf[5] >> 4) ^ (buf[6] & 0xF) ^ (buf[6] >> 4)) << 4 | (buf[2] & 0xF);
  101.             //Serial.print("out: "); Serial.print(buf[0]); Serial.print(" "); Serial.print(buf[1]); Serial.print(" "); Serial.println(buf[2]);
  102.         }
  103.         //Send the message to the car
  104.         CAN_Car.sendMsgBuf(canId, 0, len, buf);
  105.     }
  106. }
  107.  
  108. void canInit(MCPCAN &canToInit, String canName)
  109. //Loop endlessly to initialize the CANBUS interface, break if successful
  110. {
  111.     int count = 0;
  112.     while (true)
  113.     {
  114.         canToInit.init();
  115.         if (canName == "Car") //set up filter for car side CAN for defogger switch detection
  116.         {
  117.             canToInit.init_Mask(MCP_RXM0, 0, 0x7ff);
  118.             canToInit.init_Filter(MCP_RXF0, 0, 0x514);
  119.         }
  120.         if (CAN_OK == canToInit.begin(CAN_500KBPS)) {
  121.             //Serial.print(canName);
  122.             //Serial.println(" shield init ok!");
  123.             break;
  124.         }
  125.         else {
  126.             //Serial.print(canName);
  127.             //Serial.println(" shield failed init");
  128.             delay(1000);
  129.         }
  130.         count = (count + 1) % 7200; //reset the counter every 2 hour or so, as a reminder
  131.         if ((count % canAttempts) == 0 && count <= (3 * canAttempts) && speakerEnabled) {
  132.             beep(1);
  133.         }
  134.     }
  135. }
  136.  
  137. void beep(int mode) //mode 1 == error, mode 2 == success, mode 3 == debug
  138. {
  139.     switch (mode)
  140.     {
  141.     case(1): //beep 3 slow beeps times to announce a problem
  142.     {
  143.         buzz(1000, 3);
  144.         break;
  145.     }
  146.     case(2): //beep twice to signal its now working
  147.     {
  148.         buzz(60, 2);
  149.         break;
  150.     }
  151.     case(3): //one long beep used for scaling enabled state change notification
  152.     {
  153.         buzz(500, 1);
  154.         break;
  155.     }
  156.     }
  157. }
  158.  
  159. void buzz(long duration, int buzzes)
  160. {
  161.     int i;
  162.     for (i = 1; i <= buzzes; i++)
  163.     {
  164.         if (i > 1) delay(2 * duration);
  165.         tone(buzzerPin, 200, duration);
  166.     }
  167. }
  168.  
  169. /*
  170. Copyright (c) 2016 "Hired-Goon"
  171.  
  172. Permission is hereby granted, free of charge, to any person obtaining a
  173. copy of this software and associated documentation files (the "Software"),
  174. to deal in the Software without restriction, including without limitation
  175. the rights to use, copy, modify, merge, publish, distribute, sublicense,
  176. and/or sell copies of the Software, and to permit persons to whom the
  177. Software is furnished to do so, subject to the following conditions:
  178.  
  179. The above copyright notice and this permission notice shall be included in
  180. all copies or substantial portions of the Software.
  181.  
  182. THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
  183. IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
  184. FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL
  185. THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
  186. LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING
  187. FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS
  188. IN THE SOFTWARE.
  189. */
RAW Paste Data
We use cookies for various purposes including analytics. By continuing to use Pastebin, you agree to our use of cookies as described in the Cookies Policy. OK, I Understand
 
Top