KRITSADA

Robot Control

Feb 24th, 2021
858
Never
Not a member of Pastebin yet? Sign Up, it unlocks many cool features!
  1. //add RGB ✔
  2. //add Buzzer + POT ✔
  3. //add Ultrasonic ✔
  4. //add LDR
  5. //add LEDs
  6. //<2✔><3~✔><4✔><5~✔><6~✔><7✔><8✔><9~✔><10~✔><11~✔><12✔><13>
  7. //<A0✔><A1><A2><A3><A4✔><A5✔>
  8. #include <Wire.h>
  9. #include "SparkFun_Qwiic_Joystick_Arduino_Library.h"
  10. //If library not found
  11. //Click here to get the library: http://librarymanager/All#SparkFun_joystick
  12.  
  13. //RGB LED
  14. #define redPin 5
  15. #define greenPin 6
  16. #define bluePin 9
  17.  
  18. //Buzzer + POT
  19. #define buzzerPin 12
  20.  
  21. //Ultrasonic
  22. #define trigPin 7
  23. #define echoPin 8
  24. long duration;
  25. int distanceCm, distanceInch;
  26.  
  27. //LDR + LEDs
  28. #define ldrPin A0
  29. #define ledPin 10
  30.  
  31. // Clockwise and counter-clockwise definitions.
  32. // This relies on how motors are wired, check and may need to swap.
  33. #define FORWARD  0
  34. #define REVERSE 1
  35.  
  36. // Motor definitions to make life easier:
  37. #define MOTOR_A 0
  38. #define MOTOR_B 1
  39.  
  40. // Pin Assignments //
  41. //Default pins:
  42. #define DIRA 2 // Direction control for motor A
  43. #define PWMA 3  // PWM control (speed) for motor A
  44. #define DIRB 4 // Direction control for motor B
  45. #define PWMB 11 // PWM control (speed) for motor B
  46.  
  47. ////Alternate pins:
  48. //#define DIRA 8 // Direction control for motor A
  49. //#define PWMA 9 // PWM control (speed) for motor A
  50. //#define DIRB 7 // Direction control for motor B
  51. //#define PWMB 10 // PWM control (speed) for motor B
  52.  
  53. int MapXValue = 0;      // joystick readings scaled  -255 to 255
  54. int MapYValue = 0;      // scaled  -255 to 255
  55.  
  56. byte motorSpeed = 0;
  57.  
  58. uint8_t Address = 0x20; //Start address (Default 0x20)
  59.  
  60. JOYSTICK joystick; //Create instance of this object
  61.  
  62.  
  63. // setupArdumoto initialize all pins
  64. void setupArdumoto()
  65. {
  66.   // All pins should be setup as outputs:
  67.   pinMode(PWMA, OUTPUT);
  68.   pinMode(PWMB, OUTPUT);
  69.   pinMode(DIRA, OUTPUT);
  70.   pinMode(DIRB, OUTPUT);
  71.  
  72.   // Initialize all pins as low:
  73.   digitalWrite(PWMA, LOW);
  74.   digitalWrite(PWMB, LOW);
  75.   digitalWrite(DIRA, LOW);
  76.   digitalWrite(DIRB, LOW);
  77. }
  78.  
  79. void setup()
  80. {
  81.   setupArdumoto();
  82.   pinMode(redPin, OUTPUT);
  83.   pinMode(greenPin, OUTPUT);
  84.   pinMode(bluePin, OUTPUT);
  85.   pinMode(buzzerPin, OUTPUT);
  86.   pinMode(trigPin, OUTPUT);
  87.   pinMode(echoPin, INPUT);
  88.   pinMode(ledPin, OUTPUT);
  89.   pinMode(ldrPin, INPUT);
  90.   digitalWrite(ledPin, 0);
  91.   Serial.begin(9600);   //begin serial communication with the computer
  92.   //warning: need to open serial monitor or it may wait here
  93.  
  94.   if (joystick.begin(Wire, Address) == false)
  95.   {
  96.     Serial.println("Joystick does not appear to be connected. Please check wiring. Freezing...");
  97.     while (1);
  98.   }
  99. }
  100.  
  101. void loop()
  102. {
  103.   //read analog in pins from joystick Qwiic
  104.   int Joystick_X = joystick.getHorizontal();
  105.   int Joystick_Y = joystick.getVertical();
  106.   //
  107.   //Scale values from joystick to range for motor -255 and 255 --center is 0
  108.   MapXValue = map (Joystick_X, 0, 1023, -255, 255);
  109.   MapYValue = map (Joystick_Y, 0, 1023, -255, 255);
  110.   //
  111.   if (analogRead(ldrPin) < 80) {
  112.     led_ON();
  113.     buzzer();
  114.     RGBflashing();
  115.     distanceM();
  116.     if (distanceCm < 25) {
  117.       digitalWrite(DIRA, REVERSE);
  118.       digitalWrite(DIRB, REVERSE);
  119.       delay(2000);
  120.     }
  121.     else {
  122.       Motor_A(MapXValue);//drive the right wheel speed and direction from X
  123.       Motor_B(MapYValue); //drive the left wheel speed and direction from Y
  124.     }
  125.   }
  126.   else {
  127.     led_OFF();
  128.     buzzer();
  129.     RGBflashing();
  130.     distanceM();
  131.     if (distanceCm < 25) {
  132.       digitalWrite(DIRA, REVERSE);
  133.       digitalWrite(DIRB, REVERSE);
  134.       delay(2000);
  135.     }
  136.     else {
  137.       Motor_A(MapXValue);//drive the right wheel speed and direction from X
  138.       Motor_B(MapYValue); //drive the left wheel speed and direction from Y
  139.     }
  140.   }
  141.   /*
  142.   //read analog in pins from joystick Qwiic
  143.   int Joystick_X = joystick.getHorizontal();
  144.   int Joystick_Y = joystick.getVertical();
  145.   //
  146.   //Scale values from joystick to range for motor -255 and 255 --center is 0
  147.   MapXValue = map (Joystick_X, 0, 1023, -255, 255);
  148.   MapYValue = map (Joystick_Y, 0, 1023, -255, 255);
  149.   //
  150.   distanceM(); //distanceCm
  151.   buzzer();
  152.   RGBflashing();
  153.   led_ON();
  154.   led_OFF();
  155.   //Output to motor
  156.   Motor_A(MapXValue);//drive the right wheel speed and direction from X
  157.   Motor_B(MapYValue); //drive the left wheel speed and direction from Y
  158.   //
  159.     Serial.print("X: ");         //print to serial monitor the X-coordinates of joystick
  160.     Serial.print(Joystick_X);
  161.     Serial.print(" ");
  162.     Serial.print(MapXValue);
  163.  
  164.     Serial.print(" Y: ");        //print to serial monitor theY-coordinates of joystick
  165.     Serial.print(Joystick_Y);
  166.     Serial.print(" ");
  167.     Serial.println(MapYValue);
  168.   */
  169.  
  170. }// end of loop
  171. //functions:
  172. void Motor_A(int motorSpeed)                          //function for driving Motor_A
  173. {
  174.   if (motorSpeed > 0)                                 //if the motor should drive forward (positive speed)
  175.   {
  176.     digitalWrite(DIRA, FORWARD);                      //set pin 2 to high or FORWARD
  177.   }
  178.   else if (motorSpeed < 0)                            //if the motor should drive backwar (negative speed)
  179.   {
  180.     digitalWrite(DIRA, REVERSE);                      //set pin 1 to low or REVERSE
  181.   }
  182.   else                                                //if the motor should stop
  183.   {
  184.     digitalWrite(DIRA, 0);
  185.     analogWrite(PWMA, 0);
  186.   }
  187.   analogWrite(PWMA, abs(motorSpeed));                 //now that the motor direction is set, drive it at the entered Joystick speed
  188. }
  189.  
  190. /********************************************************************************/
  191. void Motor_B(int motorSpeed)                           //function for driving the Motor_B
  192. {
  193.   if (motorSpeed > 0)                                  //if the motor should drive forward (positive speed)
  194.   {
  195.     digitalWrite(DIRB, FORWARD);                       //set pin 1 to high
  196.   }
  197.   else if (motorSpeed < 0)                             //if the motor should drive backward (negative speed)
  198.   {
  199.     digitalWrite(DIRB, REVERSE);                       //set pin 1 to low
  200.   }
  201.   else                                                //STOP Motor_B
  202.   {
  203.     digitalWrite(DIRB, 0);                              // STOP motor_B
  204.     analogWrite(PWMB, 0);
  205.   }
  206.   analogWrite(PWMB, abs(motorSpeed));                 //now that the motor direction is set, drive it at the entered Joystick speed
  207.  
  208. }
  209.  
  210. void RGBflashing() {
  211.   setColor(255, 0, 0);  // red
  212.   delay(50);
  213.   setColor(0, 255, 0);  // green
  214.   delay(50);
  215.   setColor(0, 0, 255);  // blue
  216.   delay(50);
  217.   setColor(255, 255, 0);  // yellow
  218.   delay(50);
  219.   setColor(80, 0, 80);  // purple
  220.   delay(50);
  221.   setColor(0, 255, 255);  // aqua
  222.   delay(50);
  223. }
  224.  
  225. void setColor(int red, int green, int blue)
  226. {
  227. #ifdef COMMON_ANODE
  228.   red = 255 - red;
  229.   green = 255 - green;
  230.   blue = 255 - blue;
  231. #endif
  232.   analogWrite(redPin, red);
  233.   analogWrite(greenPin, green);
  234.   analogWrite(bluePin, blue);
  235. }
  236.  
  237. void buzzer() {
  238.   digitalWrite(buzzerPin, 1);
  239.   delay(50);
  240.   digitalWrite(buzzerPin, 0);
  241.   delay(50);
  242. }
  243.  
  244. void distanceM() {
  245.   digitalWrite(trigPin, LOW);
  246.   delayMicroseconds(2);
  247.   digitalWrite(trigPin, HIGH);
  248.   delayMicroseconds(10);
  249.   digitalWrite(trigPin, LOW);
  250.   duration = pulseIn(echoPin, HIGH);
  251.   distanceCm = duration * 0.0340 / 2;
  252. }
  253.  
  254. void led_ON() {
  255.   digitalWrite(ledPin, 1);
  256. }
  257. void led_OFF() {
  258.   digitalWrite(ledPin, 0);
  259. }
  260.  
RAW Paste Data

Adblocker detected! Please consider disabling it...

We've detected AdBlock Plus or some other adblocking software preventing Pastebin.com from fully loading.

We don't have any obnoxious sound, or popup ads, we actively block these annoying types of ads!

Please add Pastebin.com to your ad blocker whitelist or disable your adblocking software.

×