SHARE
TWEET

Untitled

a guest Jun 18th, 2017 148 Never
Not a member of Pastebin yet? Sign Up, it unlocks many cool features!
  1. // RC Power Wheels
  2. // By Roy El-Rayes
  3. // http://www.dadinasmarthome.com
  4.  
  5. // Power Wheels Wire Harness
  6. // Battery + (Wht1)   - (Blk1)
  7.  
  8. // Pedal
  9. // P1     P2            P3
  10. // Org1   Red1          Wht1
  11.  
  12. // <- Switch Top
  13. // White Switch 1
  14. // I1    I2    I3
  15. // Wht2  Red4  Blu1/Red3
  16. // I4    I5    I6
  17. // Wht2  Org2  Blk4/Blk3
  18.  
  19. // <- Switch Top
  20. // Red Switch 2
  21. // I1             I2           I3
  22. // Red3/Red2      Red1         Blk2
  23. // I4             I5           I6
  24. // Blk3/Blk2      Blk1/Org1    Red2
  25.  
  26. // Motor Driver Side
  27. // Blk4 Red4
  28.  
  29. // Motor Passenger Side
  30. // Org2 Blu1
  31.  
  32. // Black4 and Orange2 are combined
  33. // Red4 and Blue1 are combined
  34.  
  35. // (Black4 + Orange2 + Ground) + (Red4 + Blue1 + Power) = Forward
  36. // (Black4 + Orange2 + Power) + (Red4 + Blue1 + Ground) = Reverse
  37.  
  38. // Pedal
  39. // P1   P2      P3
  40. // Blk  Yel     Red
  41. // Blk = GND
  42. // Yel = Input (pedalInputPin)
  43. // Red = 5V
  44.  
  45. // White Switch (I) Off = Reverse
  46. // White Switch (I) On = Forward
  47. // Red Switch (II) On = Fast
  48. // Red Switch (II) Off = Slow
  49.  
  50. #define FORWARD 1
  51. #define BACKWARD 2
  52.  
  53. const byte cPedalPin = 2;
  54. const byte cThrottleForwardReversePin = 3;
  55. const byte cThrottleFastSlowPin = 4;
  56. const byte cRPWMPin = 6;
  57. const byte cLPWMPin = 7;
  58. const byte cRENPin = 24;
  59. const byte cLENPin = 22;
  60.  
  61. const byte cRCForwardBackPin = 8;
  62. const byte cRCSpeedPin = 9;
  63. const byte cRCOverridePin = 10;
  64.  
  65. bool RC_FULL_CONTROL;
  66. bool hasRCInput;
  67.  
  68. const byte FULL_MAX_THROTTLE = 255;
  69. byte MAX_THROTTLE = 255;
  70. // -MAX_THROTTLE    <->    0    <->    MAX_THROTTLE
  71. //  REVERSE                0           FORWARD
  72. // When in reverse, we pass in -MAX_THROTTLE for the PWM
  73.  
  74. int throttleAmount = 0;
  75. int lastThrottleAmount = 0;
  76. byte throttleChangeAmount = 17; // 17, 51
  77.  
  78. int vehicleState = FORWARD;
  79. int rcForwardBackData;
  80. int rcOverrideData;
  81. int rcSpeedData;
  82.  
  83. void setup()
  84. {
  85.   // Start with setting all of our pins appropriately
  86.   pinMode(LED_BUILTIN, OUTPUT);
  87.   pinMode(cPedalPin, INPUT);
  88.   pinMode(cThrottleForwardReversePin, INPUT);
  89.   pinMode(cThrottleFastSlowPin, INPUT);
  90.  
  91.   pinMode(cRCForwardBackPin, INPUT);
  92.   pinMode(cRCOverridePin, INPUT);
  93.   pinMode(cRCSpeedPin, INPUT);
  94.  
  95.   pinMode(cLPWMPin, OUTPUT);
  96.   pinMode(cRPWMPin, OUTPUT);
  97.  
  98.   pinMode(cLENPin,OUTPUT);
  99.   pinMode(cRENPin,OUTPUT);
  100.   digitalWrite(cLENPin,HIGH);
  101.   digitalWrite(cRENPin,HIGH);
  102.  
  103.   setForward();
  104.  
  105.   RC_FULL_CONTROL = false;
  106.  
  107.   // Start us off in a stopped state
  108.   analogWrite(cRPWMPin, 0);
  109.   analogWrite(cLPWMPin, 0);
  110.   Serial.begin(9600);
  111. }
  112.  
  113. void loop()
  114. {
  115.   int pedalState = digitalRead(cPedalPin); // read the pedal state
  116.   int forwardState = digitalRead(cThrottleForwardReversePin); // read the forward state
  117.  
  118.   hasRCInput = readRCInput(); // check if we have RC Input
  119.   if (!RC_FULL_CONTROL && !hasRCInput) // if we don't have RC input and we haven't taken over full control over RC, let the driver control the vehicle
  120.   {
  121.     if (forwardState == HIGH)
  122.     {
  123.       setReverse();
  124.     } else {
  125.       setForward();
  126.     }
  127.     if (pedalState == HIGH) {
  128.       increaseThrottle();
  129.     } else {
  130.       decreaseThrottle();
  131.     }
  132.   }
  133.  
  134.   sendCommandToMotors(); // command the motors
  135.   delay(50);
  136. }
  137.  
  138. bool readRCInput()
  139. {
  140.   bool hasInput = false;
  141.   rcOverrideData = pulseIn(cRCOverridePin, HIGH, 15000); // give this one a timeout incase the controller is off
  142.  
  143.   if (rcOverrideData > 0) // that means we are on
  144.   {    
  145.     rcForwardBackData = pulseIn(cRCForwardBackPin, HIGH);
  146.     rcSpeedData = pulseIn(cRCSpeedPin, HIGH);
  147.      
  148.     if (rcOverrideData < 1100)
  149.     {
  150.       RC_FULL_CONTROL = true;
  151.     } else {
  152.       RC_FULL_CONTROL = false;
  153.     }
  154.  
  155.     if (rcSpeedData > 1800)
  156.     {
  157.       MAX_THROTTLE = FULL_MAX_THROTTLE;
  158.     } else if (rcSpeedData > 1700) {
  159.       MAX_THROTTLE = FULL_MAX_THROTTLE - 50;
  160.     } else {
  161.       MAX_THROTTLE = FULL_MAX_THROTTLE - 100; // this is about a walking speed
  162.     }
  163.  
  164.     if (throttleAmount > MAX_THROTTLE)
  165.     {
  166.       throttleAmount = MAX_THROTTLE;
  167.     }
  168.  
  169.     if (rcForwardBackData > 1600)
  170.     {
  171.       setForward();
  172.       increaseThrottle();
  173.       hasInput = true;
  174.     } else if (rcForwardBackData < 1300) {
  175.       setReverse();
  176.       increaseThrottle();
  177.       hasInput = true;
  178.     } else if (RC_FULL_CONTROL) {
  179.       setForward();
  180.       decreaseThrottle();
  181.       hasInput = true;
  182.     }
  183.   } else {
  184.     RC_FULL_CONTROL = false;
  185.   }
  186.   return hasInput;
  187. }
  188. void sendCommandToMotors()
  189. {
  190.   if (lastThrottleAmount != throttleAmount)
  191.   {
  192.     if (vehicleState == FORWARD)
  193.     {
  194.       if (throttleAmount >= 0) // if we are already moving forward
  195.       {
  196.         digitalWrite(cRPWMPin, 0); // keep moving forward
  197.         analogWrite(cLPWMPin, throttleAmount);
  198.         Serial.print("Forward : ");
  199.         Serial.println(throttleAmount);
  200.       } else {
  201.         int inverseThrottleAmount =  -1 * throttleAmount;
  202.         analogWrite(cRPWMPin, inverseThrottleAmount); // we're not yet throttled back into forward position, so keep slowing our reverse down
  203.         digitalWrite(cLPWMPin, 0);
  204.         Serial.print("Forward-: ");
  205.         Serial.println(inverseThrottleAmount);
  206.       }
  207.     } else { // we want to go reverse
  208.       if (throttleAmount <= 0) // if we are already going in reverse
  209.       {
  210.         int inverseThrottleAmount =  -1 * throttleAmount;
  211.         analogWrite(cRPWMPin, inverseThrottleAmount); // keep going in reverse
  212.         digitalWrite(cLPWMPin, 0);  
  213.         Serial.print("Reverse-: ");
  214.         Serial.println(inverseThrottleAmount);
  215.       } else {
  216.         digitalWrite(cRPWMPin, 0); // we're not yet going in reverse, so keep slowing down our forward
  217.         analogWrite(cLPWMPin, throttleAmount);
  218.         Serial.print("Reverse : ");
  219.         Serial.println(throttleAmount);
  220.       }
  221.     }
  222.     lastThrottleAmount = throttleAmount;
  223.   }
  224. }
  225. void setForward()
  226. {
  227.   vehicleState = FORWARD;
  228. }
  229. void setReverse()
  230. {
  231.   vehicleState = BACKWARD;
  232. }
  233. void increaseThrottle()
  234. {
  235.   if (vehicleState == FORWARD) // if we want to go forward
  236.   {
  237.     if (throttleAmount + throttleChangeAmount <= MAX_THROTTLE) // if we're not less than max throttle
  238.     {
  239.       throttleAmount = throttleAmount + throttleChangeAmount; // increase throttle
  240.     }
  241.   } else { // we want to go backward
  242.     if (throttleAmount - throttleChangeAmount >= -MAX_THROTTLE) // if we're not less than -max throttle
  243.     {
  244.       throttleAmount = throttleAmount - throttleChangeAmount; // increase throttle in the opposite direction
  245.     }
  246.   }
  247. }
  248. void decreaseThrottle()
  249. {
  250.   if (throttleAmount > 0)
  251.   {
  252.     if (throttleAmount - throttleChangeAmount >= 0)
  253.     {
  254.       throttleAmount = throttleAmount - throttleChangeAmount;
  255.     } else {
  256.       throttleAmount = 0;
  257.     }
  258.   } else {
  259.     if (throttleAmount + throttleChangeAmount <= 0)
  260.     {
  261.       throttleAmount = throttleAmount + throttleChangeAmount;
  262.     } else {
  263.       throttleAmount = 0;
  264.     }    
  265.   }
  266. }
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