Advertisement
FahmiG

Differential drive robot with speed control

Dec 25th, 2015 (edited)
4,461
0
Never
Not a member of Pastebin yet? Sign Up, it unlocks many cool features!
C++ 4.05 KB | None | 0 0
  1. /*+++++++++++++++++++++++++++++++++++++++++++++++++++
  2. Author      : Fahmi Ghani
  3. Date        : 11 July 2015
  4. Project     : Joystick control differential drive dc motor robot
  5. Component   : Analog Joystick
  6.               2Amp motor driver shield
  7.               DC Motor
  8. Description : Control DC motor direction using Joystick
  9. Video :https://www.youtube.com/watch?v=kfT3eoNAM-Q
  10. +++++++++++++++++++++++++++++++++++++++++++++++++++*/
  11.  
  12. //Set pin numbers:
  13. const byte joyStickYPin = A2;
  14. const byte joyStickXPin = A1;
  15.  
  16. //This pin is depend on what motor driver you use, and how you wired it.
  17. const byte motorLSpeedPin = 5; //Left Speed Pin
  18. const byte motorLDirPin = 4;   //Left Direction Pin
  19. const byte motorRSpeedPin = 6; //Right Speed Pin
  20. const byte motorRDirPin = 7;   //Right Direction Pin
  21.  
  22. //variables
  23. //Joystick input variables
  24. int joyXValue = 0;
  25. int joyYValue = 0;
  26. int joyValueMax = 2000;
  27. int joyValueMin = 1000;
  28. int joyValueMid = 1500; //Joystick value when in default position.
  29. int joyValueMidUpper = joyValueMid + 100; //Add +-100 as joystick deadzone
  30. int joyValueMidLower = joyValueMid - 100;
  31. //The "joystick deadzone" is the area around the center of a joystick that does not respond to movement.
  32. //It is designed to cut down on accidental movement caused by "jitter"
  33.  
  34. //DC motor variables
  35. int speedFwd = 0;
  36. int speedTurn = 0;
  37. int speedLeft = 0;
  38. int speedRight = 0;
  39.  
  40. byte motorSpeed = 0;   
  41. byte motorSpeedMax = 255; //Maximum Speed
  42. byte motorSpeedMin = 90; //set to smallest value that make motor move (default 0)
  43.                          // DC motor that I use start to move at 90 pwm value
  44.  
  45. void setup()
  46. {
  47.     Serial.begin(9600);
  48.     Serial.print("START" );
  49.      
  50.     pinMode(joyStickXPin, INPUT);
  51.     pinMode(joyStickYPin, INPUT);
  52.     pinMode(motorLSpeedPin, OUTPUT);
  53.     pinMode(motorLDirPin, OUTPUT);
  54.     pinMode(motorRSpeedPin, OUTPUT);
  55.     pinMode(motorRDirPin, OUTPUT);
  56. }
  57.  
  58. void loop()
  59. {
  60.    // joyXValue = analogRead(joyStickXPin); //Turn
  61.    // joyYValue = analogRead(joyStickYPin); //Forward/backward
  62.    
  63.     Read PWM value form receiver
  64.     joyXValue = pulseIn(joyStickXPin,HIGH);
  65.     joyYValue = pulseIn(joyStickYPin,HIGH);
  66.    
  67.     //Map joystick value to speed value
  68.     if(joyYValue > joyValueMidUpper)//forward
  69.     {
  70.         speedFwd = map(joyYValue, joyValueMidUpper, joyValueMax, motorSpeedMin, motorSpeedMax);
  71.     }
  72.     else if(joyYValue < joyValueMidLower) //backward
  73.     {
  74.         speedFwd = map(joyYValue, joyValueMidLower, joyValueMin, -motorSpeedMin, -motorSpeedMax);
  75.     }
  76.     else
  77.     {
  78.        speedFwd =0;
  79.     }
  80.    
  81.     if(joyXValue > joyValueMidUpper) //right
  82.     {
  83.         speedTurn = map(joyXValue, joyValueMidUpper, joyValueMax, motorSpeedMin, motorSpeedMax);
  84.     }
  85.     else if(joyXValue < joyValueMidLower) //left
  86.     {
  87.         speedTurn = map(joyXValue, joyValueMidLower, joyValueMin, -motorSpeedMin, -motorSpeedMax);
  88.     }
  89.     else
  90.     {
  91.        speedTurn =0;
  92.     }
  93.  
  94.     //Convert Forward speed and Turn Speed to Motor left and Motor Right speed
  95.     speedLeft = speedFwd + speedTurn;
  96.     speedRight = speedFwd - speedTurn;
  97.  
  98.     speedLeft = constrain(speedLeft, -255, 255);
  99.     speedRight = constrain(speedRight, -255, 255);
  100.    
  101.     //Send the Motor speed value to Motor Driver
  102.     MoveRobot(speedLeft,speedRight);
  103.  
  104.     //Display the value in Serial Monitor.
  105.     Serial.print(speedFwd);
  106.     Serial.print("\t" );
  107.     Serial.print(speedTurn);
  108.     Serial.print("\t" );
  109.     Serial.print(speedLeft);
  110.     Serial.print("\t" );
  111.     Serial.print(speedRight);
  112.     Serial.println(" ");
  113.    
  114.     delay(100);
  115.  
  116. }
  117.  
  118. void MoveRobot(int spdL, int spdR)
  119. {
  120.      //Set Motor Direction
  121.      if(spdL>0)
  122.      {
  123.         digitalWrite(motorLDirPin, HIGH);
  124.      }
  125.      else
  126.      {
  127.         digitalWrite(motorLDirPin, LOW);
  128.      }
  129.      if(spdR>0)
  130.      {
  131.         digitalWrite(motorRDirPin, HIGH);
  132.      }
  133.      else
  134.      {
  135.         digitalWrite(motorRDirPin, LOW);
  136.      }
  137.      
  138.      //Set Motor Speed
  139.      analogWrite(motorLSpeedPin, abs(spdL));
  140.      analogWrite(motorRSpeedPin, abs(spdR));    
  141. }
Advertisement
Add Comment
Please, Sign In to add comment
Advertisement