Advertisement
Guest User

Untitled

a guest
Jul 16th, 2019
92
0
Never
Not a member of Pastebin yet? Sign Up, it unlocks many cool features!
text 2.96 KB | None | 0 0
  1.  
  2. #include <MAX14870MotorShield.h>
  3. #include <stdio.h>
  4. #include <Servo.h>
  5.  
  6. Servo myservo; // create servo object to control a servo
  7.  
  8. int closee = 52;
  9. int openn = 65 + closee;
  10.  
  11. int sensorPin = A0; // select the input pin for LDR
  12. int sensorValue = 0; // variable to store the value coming from the sensor
  13. bool opened = false;
  14. int sensor_thresh = 750;
  15.  
  16. int PWM1 = 10, DIR1 = 11, FLT1 = 12, EN1 = 13;
  17. int PWM2 = 3, DIR2 = 5, FLT2 = 4, EN2 = 6;
  18. // motor-shield - Version: Latest
  19. int relay1 = 8, relay2 = 2;
  20.  
  21. MAX14870MotorShield motor1, motor2;
  22. String str;
  23. int vr, vl, ms;
  24. int curr;
  25.  
  26. void setup(){
  27. Serial.begin(9600);
  28. motor1 = MAX14870MotorShield(DIR1,PWM1,EN1,FLT1);
  29. motor2 = MAX14870MotorShield(DIR2,PWM2,EN2,FLT2);
  30. myservo.attach(9); // attaches the servo on pin 9 to the servo object
  31. myservo.write(closee);
  32. pinMode(relay1,OUTPUT);
  33. pinMode(relay2,OUTPUT);
  34. motor1.setSpeed(0);
  35. motor2.setSpeed(0);
  36. batteryToDriver();
  37. delay(5000);
  38. }
  39.  
  40. void loop(){
  41. if(Serial.available() < 12){
  42. sensorValue = analogRead(sensorPin); // read the value from the sensor
  43. if ((sensorValue < sensor_thresh) & (opened == false)) {
  44. delay(1000);
  45. myservo.write(openn);
  46. opened = true;
  47. }
  48. if ((sensorValue > sensor_thresh) & (opened == true)) {
  49. delay(250);
  50. myservo.write(closee);
  51. opened = false;
  52. }
  53. }
  54. else {
  55. curr = Serial.read() - 48;
  56. vr = 100 * curr;
  57. curr = Serial.read() - 48;
  58. vr += 10 * curr;
  59. curr = Serial.read() - 48;
  60. vr += curr;
  61.  
  62. curr = Serial.read() - 48;
  63. vl = 100 * curr;
  64. curr = Serial.read() - 48;
  65. vl += 10 * curr;
  66. curr = Serial.read() - 48;
  67. vl += curr;
  68.  
  69. curr = Serial.read() - 48;
  70. ms = 1000 * curr;
  71. curr = Serial.read() - 48;
  72. ms += 100 * curr;
  73. curr = Serial.read() - 48;
  74. ms += 10 * curr;
  75. curr = Serial.read() - 48;
  76. ms += curr;
  77.  
  78. vr = vr - 500;
  79. vl = vl - 500;
  80. Serial.println(vl);Serial.println(vr);Serial.println(ms);
  81. int x = Serial.read() - 48;
  82. int y = Serial.read() - 48;
  83. if (x == 1 && y == 1) {
  84. motor1.setSpeed(vr);
  85. motor2.setSpeed(vl);
  86. delay(ms+50);
  87. //motor1.setSpeed(0);
  88. //motor2.setSpeed(0);
  89. delay(30);
  90. }
  91. else {
  92. for(int i = 0; i<100; i++){
  93. int j = Serial.read();
  94. }
  95. }
  96.  
  97. }
  98. //motor1.setSpeed(0);
  99. //motor2.setSpeed(0);
  100. delay(30);
  101. }
  102.  
  103. void batteryToDriver(){
  104. motor1.setSpeed(0);
  105. motor2.setSpeed(0);
  106. setRelay2(false);
  107. delay(50);
  108. setRelay1(false);
  109. delay(50);
  110. }
  111.  
  112. void driverToBattery(){
  113. motor1.setSpeed(0);
  114. motor2.setSpeed(0);
  115. setRelay1(true);
  116. delay(50);
  117. setRelay2(true);
  118. delay(50);
  119. }
  120.  
  121. void setRelay1(bool amir){
  122. if(amir){
  123. digitalWrite(relay1,LOW);
  124. }
  125. else{
  126. digitalWrite(relay1,HIGH);
  127. }
  128. }
  129.  
  130. void setRelay2(bool amir){
  131. if(amir){
  132. digitalWrite(relay2,HIGH);
  133. }
  134. else{
  135. digitalWrite(relay2,LOW);
  136. }
  137. }
Advertisement
Add Comment
Please, Sign In to add comment
Advertisement