safwan092

TEST-CODE

Nov 16th, 2020 (edited)
1,311
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