Arduino Code

Here you can find a code that you need to upload to your Arduino Uno board. This was my first larger project in C, therefore, there is a space for optimizing the code.
All the communication between Mobile App, Computer Server and the Car goes through serial port.

  1. // Name Commands
  2. // mt -> motor temperature
  3. // sp -> car speed
  4. // bt -> battery type
  5. // bv -> battery voltage
  6. // rs -> range sensor problem
  7. #include <Wire.h>
  8. #include <string.h>
  9. // Servo Motor
  10. #include <Servo.h>
  11. // Timer
  12. #include <SimpleTimer.h>
  13. // Range Sensors
  14. #include <NewPing.h>
  15. Servo driveSrv;
  16. Servo turnservo;
  17. Servo rSensServo;
  18. SimpleTimer timer;
  19. const int drivePin = 11;
  20. int speedFactor = 120;
  21. const int steer = 9;
  22. int driveMode = 1;
  23. String inByte;
  24. int connectionT;
  25. int tempMotorT;
  26. int rangeT;
  27. int batVolt;
  28. int neutralT;
  29. //char *endChar = "X";
  30. // Steer Calibrated Angle
  31. int steerCalib = 0;
  32. // LM35 Temp. Sensor
  33. const int tempPin = A0;
  34. // Battery Voltage
  35. const int voltPin = A1;
  36. // Car Lights
  37. byte lightsState = 0;
  38. const int lightsLED = 12;
  39. byte longLightsState = 0;
  40. const int longLightLED = 4;
  41. // Bottom Lights
  42. const int redLED = 5;
  43. const int blueLED = 6;
  44. //const int greenLED = 3;
  45. // Car Blinkers
  46. byte blinkersState = 0;
  47. const int blinkLLED = 7;
  48. const int blinkRLED = 8;
  49. byte blinkON = 0;
  50. char blinkSide;
  51. byte ledState = 0;
  52. unsigned long currentMillis;
  53. long previousMillis = 0;
  54. long interval = 500;
  55. // All 4 Blinkers
  56. byte blinkers4State = 0;
  57. // Stop Lights
  58. const int stopLED = 13;
  59. // Car Speed
  60. volatile int rpmcount = 0;
  61. volatile int direction = 2;     // Neutral
  62. int rpm = 0;
  63. unsigned long lastmillis = 0;
  64. int carSpeed = 0;
  65. volatile char speedDirection;
  66. const int directionPin = 3;
  67. // Range Sensors (Front)
  68. byte rsensorsOn = 1;
  69. const int frsServo = 10;
  70. const int frsTrig = A3;
  71. const int frsEcho = A2;
  72. int currPos = 90;
  73. long prevMill = 0;
  74. int rInterval = 100;    // (ms)
  75. int rDeg = 5;        // (koraki v °)
  76. int frsDistance = 400;
  77. NewPing sonarFr(frsTrig, frsEcho, frsDistance);
  78. int servoCalib = 0;
  79. byte preventNeutral = 0;
  80. byte preventBackward = 0;
  81. // Range Sensors (Back)
  82. const int bcsTrig = A5;
  83. const int bcsEcho = A4;
  84. int bcsDistance = 20;
  85. NewPing sonar(bcsTrig, bcsEcho, bcsDistance);
  86. void setup() {
  87.     Serial.begin(19200);
  88.     // Drive
  89.     driveSrv.attach(drivePin);
  90.     driveSrv.write(90);
  91.     // Steer
  92.     turnservo.attach(steer);
  93.     turnservo.write(90);
  94.     // PINS CONFIG
  95.     pinMode(steer, OUTPUT);
  96.     pinMode(tempPin, INPUT);
  97.     pinMode(voltPin, INPUT);
  98.     pinMode(lightsLED, OUTPUT);
  99.     pinMode(longLightLED, OUTPUT);
  100.     pinMode(blinkLLED, OUTPUT);
  101.     pinMode(blinkRLED, OUTPUT);
  102.     pinMode(stopLED, OUTPUT);
  103.     pinMode(frsTrig, OUTPUT);
  104.     pinMode(frsEcho, INPUT);
  105.     pinMode(bcsTrig, OUTPUT);
  106.     pinMode(bcsEcho, INPUT);
  107.     pinMode(redLED, OUTPUT);
  108.     pinMode(blueLED, OUTPUT);
  109.     //pinMode(greenLED, OUTPUT);
  110.     pinMode(directionPin, INPUT);
  111.         
  112.     // Set Timer
  113.     connectionT = timer.setInterval(300, stopCar);
  114.     tempMotorT = timer.setInterval(3000, tempMotor);
  115.     rangeT = timer.setInterval(35, frangeS);
  116.     batVolt = timer.setInterval(30000, battVoltage);
  117.     // DEFAULT STATES    
  118.     // LED
  119.     analogWrite(redLED, 0);
  120.     analogWrite(blueLED, 0);
  121.     //analogWrite(greenLED, 0);
  122.     // Car Lights
  123.     digitalWrite(lightsLED, 0);
  124.     digitalWrite(longLightLED, 0);
  125.     // Car Blinkers
  126.     digitalWrite(blinkLLED, 0);
  127.     digitalWrite(blinkRLED, 0);
  128.     // Stop Lights
  129.     digitalWrite(stopLED, 0);
  130.     // Range Sensors (Front/Back)
  131.     rSensServo.attach(frsServo);
  132.     rSensServo.write(90);
  133.     digitalWrite(frsTrig, 0);
  134.     digitalWrite(frsEcho, 0);
  135.     digitalWrite(bcsTrig, 0);
  136.     digitalWrite(bcsEcho, 0);
  137.     // read RPM
  138.     attachInterrupt(0, car_rpm, FALLING);
  139. }
  140. void loop() {
  141.     // Start Timer
  142.     timer.run();
  143.     // Start Millis Function (Blinkers)
  144.     currentMillis = millis();
  145.     // Blinkers
  146.     if (blinkersState == 1 && blinkON == 1) {
  147.         if(currentMillis - previousMillis > interval) {
  148.             previousMillis = currentMillis;
  149.             blinkers(blinkSide);
  150.         }
  151.     }
  152.     // All 4 Blinkers
  153.     if (blinkers4State == 1) {
  154.         if(currentMillis - previousMillis > interval) {
  155.             previousMillis = currentMillis;
  156.             blinkers4();
  157.         }
  158.     }
  159.     // Car Speed
  160.     if (currentMillis - lastmillis > 500) { //Uptade every one second, this will be equal to reading frecuency (Hz).
  161.         detachInterrupt(0);
  162.         rpm = rpmcount * 60; // Convert frequency to RPM, note: this works for one interruption per full rotation. For two interrupts per full rotation use rpmcount * 30.
  163.         carSpeed = rpm * 0.245 * 0.06;
  164.         Serial.print(String("sp") + carSpeed + String("X"));
  165.         rpmcount = 0;
  166.         lastmillis = currentMillis;
  167.         attachInterrupt(0, car_rpm, FALLING);
  168.     }
  169.     // Move Front Servo for Range Sensor
  170.     if(rsensorsOn == 1) {
  171.         moveServo();
  172.     }
  173.     if(Serial.available()) {
  174.                 
  175.         inByte = Serial.readStringUntil('\n');
  176.         // Keep Alive
  177.         if(inByte.startsWith("kp")) {
  178.             // Restart Timer
  179.             timer.restartTimer(connectionT);
  180.             analogWrite(redLED, 0);
  181.             analogWrite(blueLED, 255);
  182.         }
  183.         // Stop the Car On Exit DriveMode 2 (Accelerometers)
  184.         if(inByte.startsWith("st")) {
  185.             stopCar();
  186.         }
  187.         /////////////////
  188.         // DRIVE MODE //
  189.         ////////////////
  190.         if(inByte.startsWith("dm")) {
  191.             String dm = inByte.substring(2);
  192.             driveMode = conToInt(dm);
  193.         }
  194.         
  195.         // Drive with Buttons
  196.         if(driveMode == 1) {
  197.             if(inByte.startsWith("db")) {
  198.             
  199.                 String comm = inByte.substring(2);
  200.                 // Forward
  201.                 if(comm == "w") {
  202.                     driveSrv.write(speedFactor);
  203.                     preventNeutral = 0;
  204.                     preventBackward = 0;
  205.                     // Stop Lights
  206.                     digitalWrite(stopLED, 0);
  207.                 }
  208.                 // Reverse
  209.                 if(comm == "s" && preventBackward != 1) {
  210.                     driveSrv.write(15);
  211.                     preventNeutral = 0;
  212.                     // All 4 Blinkers
  213.                     blinkers4State = 1;
  214.                     // Stop Lights
  215.                     digitalWrite(stopLED, 0);
  216.                 }
  217.                 // Stop
  218.                 if(comm == "x" && preventNeutral != 1) {
  219.                     driveSrv.write(90);
  220.                     // All 4 Blinkers (Off)
  221.                     blinkers4State = 0;
  222.                     ledState = 0;
  223.                     digitalWrite(blinkLLED, 0);
  224.                     digitalWrite(blinkRLED, 0);
  225.                     // Stop Lights
  226.                     digitalWrite(stopLED, 1);
  227.                 }
  228.                 // Left
  229.                 if(comm == "a") {
  230.                     turnservo.write(65 + steerCalib);
  231.                     // Blinkers
  232.                     if(blinkersState == 1 && blinkers4State == 0) {
  233.                         blinkON = 1;
  234.                         blinkSide = 'l';
  235.                     }
  236.                 }
  237.                 // Right
  238.                 else if(comm == "d") {
  239.                     turnservo.write(115 + steerCalib);
  240.                     // Blinkers
  241.                     if(blinkersState == 1 && blinkers4State == 0) {
  242.                         blinkON = 1;
  243.                         blinkSide = 'r';
  244.                     }
  245.                 }
  246.                 // Aligned
  247.                 else if(comm == "g") {
  248.                     turnservo.write(90 + steerCalib);
  249.                     // Blinkers (Off)
  250.                     blinkON = 0;
  251.                     ledState = 0;
  252.                     digitalWrite(blinkLLED, 0);
  253.                     digitalWrite(blinkRLED, 0);
  254.                 }
  255.             }
  256.         }
  257.         // Drive with Accelerometers
  258.         else if (driveMode == 2) {
  259.             String st = inByte.substring(2);
  260.             int steerAng = conToInt(st);
  261.             // Forward & Backward
  262.             if(inByte.startsWith("ad")) {
  263.                 driveSrv.write(steerAng);
  264.             }
  265.             // Left/Right
  266.             else if(inByte.startsWith("as")) {
  267.                 turnservo.write(90 + steerAng + steerCalib);
  268.             }
  269.         }
  270.         //////////////
  271.         // OPTIONS //
  272.         /////////////
  273.         
  274.         // Steer Calibrated Angle
  275.         if(inByte.startsWith("sc")) {
  276.             String sc = inByte.substring(2);
  277.             steerCalib = conToInt(sc);
  278.             turnservo.write(90 + steerCalib);
  279.         }
  280.         // Range Sensor Servo Calibrate
  281.         if(inByte.startsWith("rc")) {
  282.             String rc = inByte.substring(2);
  283.             servoCalib = conToInt(rc);
  284.             rSensServo.write(90 - servoCalib);
  285.         }
  286.         // Car Lights
  287.         if(inByte.startsWith("cl")) {
  288.             String cl = inByte.substring(2);
  289.             lightsState = conToInt(cl);
  290.             if(lightsState == 1) {
  291.                 digitalWrite(lightsLED, 1);
  292.             } else {
  293.                 digitalWrite(lightsLED, 0);
  294.             }
  295.         }
  296.         // Car Long Lights
  297.         if(inByte.startsWith("ll")) {
  298.             String ll = inByte.substring(2);
  299.             longLightsState = conToInt(ll);
  300.             if (longLightsState == 1) {
  301.                 digitalWrite(longLightLED, 1);
  302.             } else {
  303.                 digitalWrite(longLightLED, 0);
  304.             }
  305.         }
  306.         // Car Blinkers
  307.         if(inByte.startsWith("bl")) {
  308.             String bl = inByte.substring(2);
  309.             blinkersState = conToInt(bl);
  310.         }
  311.         // Car All 4 Blinkers
  312.         if (inByte.startsWith("b4")) {
  313.             String b4 = inByte.substring(2);
  314.             blinkers4State = conToInt(b4);
  315.             if (blinkers4State == 0) {
  316.                 ledState = 0;
  317.                 digitalWrite(blinkLLED, 0);
  318.                 digitalWrite(blinkRLED, 0);
  319.             }
  320.         }
  321.         // Speed Factor
  322.         if (inByte.startsWith("sf")) {
  323.             String sf = inByte.substring(2);
  324.             speedFactor = conToInt(sf);
  325.         }
  326.         // Range Sensors
  327.         if(inByte.startsWith("rs")) {
  328.             String rs = inByte.substring(2);
  329.             rsensorsOn = conToInt(rs);
  330.             if(rsensorsOn == 1) {
  331.                 timer.enable(rangeT);
  332.             } else {
  333.                 timer.disable(rangeT);
  334.                 rSensServo.write(90 - servoCalib);
  335.             }
  336.         }
  337.         // Clear the data in the serial buffer
  338.         //Serial.flush();
  339.     }
  340. }
  341. // Stop The Car
  342. void stopCar() {
  343.     if(carSpeed > 0 && speedDirection == 'f') {
  344.         driveSrv.write(15);
  345.         turnservo.write(90 + steerCalib);
  346.         preventNeutral = 1;
  347.     } else {
  348.         driveSrv.write(90);
  349.         turnservo.write(90 + steerCalib);
  350.         preventNeutral = 0;
  351.     }
  352.     analogWrite(redLED, 255);
  353.     analogWrite(blueLED, 0);
  354. }
  355. // Car Speed (read RPM)
  356. void car_rpm() {
  357.     rpmcount++;
  358.     car_dir();
  359.     /*if(direction == 0 && speedDirection == 'f') { // OK
  360.         speedDirection = 'f';
  361.     } else if(direction == 0 && speedDirection == 'b') { // OK
  362.         speedDirection = 'b';
  363.     } else if(direction == 1 && speedDirection == 'b') { // OK
  364.         speedDirection = 'f';
  365.     } else { // OK
  366.         speedDirection = 'f';
  367.     }
  368.     direction = 1;*/
  369. }
  370. // Car Direction
  371. void car_dir() {
  372.     volatile int readDirection = digitalRead(directionPin);
  373.     if(readDirection == 1) {
  374.         speedDirection = 'b';
  375.     } else {
  376.         speedDirection = 'f';
  377.     }
  378.     /*if(direction == 1 && speedDirection == 'f') { // OK
  379.         speedDirection = 'f';
  380.     } else if(direction == 1 && speedDirection == 'b') { // OK
  381.         speedDirection = 'b';
  382.     } else if(direction == 0 && speedDirection == 'f') { // OK
  383.         speedDirection = 'b';
  384.     } else { // OK
  385.         speedDirection = 'b';
  386.     }
  387.     direction = 0;*/
  388. }
  389. // Motor Temperature
  390. void tempMotor() {
  391.     // LM35
  392.     int tempC = (5.0 * analogRead(tempPin) * 100.0) / 1024;
  393.     Serial.print(String("mt") + tempC + String("X"));
  394.     // Critical Temperature
  395.     int criticalTemp = 50;
  396.     if (tempC >= criticalTemp) {
  397.         stopCar();
  398.     }
  399. }
  400. // Battery Voltage
  401. void battVoltage() {
  402.     // MAX (16,8V) => 675 => 3,3V
  403. // (15,8V) => 614 => 3V
  404. // 1V => 60 enot
  405. // 16mV => 1 enota
  406.     int battData = analogRead(voltPin);
  407.     Serial.print(String("bv") + battData + String("X"));
  408. }
  409. // Car Blinkers
  410. void blinkers(char side) {
  411.     // Left
  412.     if(side == 'l') {
  413.         if (ledState == 0) {
  414.             ledState = 1;
  415.         } else {
  416.             ledState = 0;
  417.         }
  418.         digitalWrite(blinkLLED, ledState);
  419.     }
  420.     // Right
  421.     else if (side == 'r') {
  422.         if (ledState == 0) {
  423.             ledState = 1;
  424.         } else {
  425.             ledState = 0;
  426.         }
  427.         digitalWrite(blinkRLED, ledState);
  428.     }
  429. }
  430. // Car All 4 Blinkers
  431. void blinkers4() {
  432.     if (ledState == 0) {
  433.         ledState = 1;
  434.     } else {
  435.         ledState = 0;
  436.     }
  437.     digitalWrite(blinkLLED, ledState);
  438.     digitalWrite(blinkRLED, ledState);
  439. }
  440. // Front Range Sensors
  441. void frangeS() {
  442.     // Range Sensor (Front)
  443.     // Wait 50ms between pings (about 20 pings/sec). 29ms should be the shortest delay between pings.
  444.     unsigned int frsRange = sonarFr.ping_cm();    
  445.     // 1. verzija => ko bo smer OK
  446.     //if(frsRange != 0 && frsRange < obstacleDist) {
  447.     //if(frsRange != 0 && frsRange < carSpeed*10) {
  448.     if(frsRange != 0 && frsRange < speedFactor*2.4) {
  449.         // Brake
  450.         if(speedDirection == 'f') {
  451.             driveSrv.write(15);
  452.             preventNeutral = 1;
  453.             // Stop Lights
  454.             digitalWrite(stopLED, 1);
  455.         }
  456.         // To Neutral
  457.         else if(speedDirection = 'b' && preventNeutral == 1) {
  458.             driveSrv.write(90);
  459.             preventNeutral = 0;
  460.         }
  461.         analogWrite(redLED, 255);
  462.         analogWrite(blueLED, 0);
  463.     }
  464.     // 2. verzija => Casovno
  465.     /*if(frsRange != 0 && frsRange < carSpeed*10) {
  466.         // Brake
  467.         if(speedDirection == 'f') {
  468.             driveSrv.write(15);
  469.             preventNeutral = 1;
  470.             neutralT = timer.setTimer(carSpeed*100, toNeutral, 1);
  471.             // Stop Lights
  472.             digitalWrite(stopLED, 1);
  473.         }
  474.         analogWrite(redLED, 255);
  475.         analogWrite(blueLED, 0);
  476.     }*/
  477.     // Range Sensor (Back)
  478.     unsigned int bcsRange = sonar.ping_cm();
  479.     if(bcsRange != 0 && bcsRange < bcsDistance) {
  480.         preventBackward = 1;    // prevent to go Backward if the obstacle is too close
  481.     }
  482. }
  483. /*void toNeutral() {
  484.     driveSrv.write(90);
  485.     speedDirection = 'n';    // zakomentirat, ko bo smer OK
  486. }*/
  487. // Servo - Front Range Sensor
  488. void moveServo() {
  489.     if(currentMillis - prevMill > rInterval) {
  490.         prevMill = currentMillis;
  491.         currPos = (currPos - (rDeg));
  492.         rSensServo.write(currPos - servoCalib);
  493.         if(currPos == 75) {
  494.             rDeg = -5;
  495.         } else if(currPos == 105) {
  496.             rDeg = 5;
  497.         }
  498.     }
  499. }
  500. // Convert to INT
  501. int conToInt(String data) {
  502.     char carray[data.length() + 1];
  503.     data.toCharArray(carray, sizeof(carray));
  504.     return atoi(carray);
  505. }
.