Advertisement
Guest User

Brainv01

a guest
Oct 23rd, 2016
86
0
Never
Not a member of Pastebin yet? Sign Up, it unlocks many cool features!
text 5.74 KB | None | 0 0
  1. #include <NewPing.h>
  2. #include <Wire.h>
  3.  
  4. //L293D
  5. //Motor A
  6. const int motorPin1 = 5; // Pin 14 of L293
  7. const int motorPin2 = 6; // Pin 10 of L293
  8. //Motor B
  9. const int motorPin3 = 10; // Pin 7 of L293
  10. const int motorPin4 = 9; // Pin 2 of L293
  11.  
  12.  
  13. #define PAYLOAD_SIZE 14 // how many bytes to expect from each I2C salve node
  14. #define NODE_MAX 6 // maximum number of slave nodes (I2C addresses) to probe
  15. //#define START_NODE 2 // The starting I2C address of slave nodes
  16. #define NODE_READ_DELAY 100 // Some delay between I2C node reads
  17.  
  18. int nodeArray[6][16] = { // Node / Modules array
  19. {0x01,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0}, //Hallynode( i2c address, Mode, millis MSB, millis, millis, millis LSB, Data 0, 1, 2, 3, 4,5,6,7,8
  20. {0x02,3,0,0,0,0,0,0,0,0,0,0,0,0,0,0}, //Ping Sensors Hallynode( i2c address, Mode, millis MSB, millis, millis, millis LSB, Data 0, 1, 2, 3, 4,5,6,7,8
  21. {0x03,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0}, //Hallynode( i2c address, Mode, millis MSB, millis, millis, millis LSB, Data 0, 1, 2, 3, 4,5,6,7,8
  22. {0x68,9,0,0,0,0,0,0,0,0,0,0,0,0,0,0}, //Accelerometer
  23. {0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0}, //spare
  24. {0x3C,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0} //Screen
  25. };
  26.  
  27. //NodeArray
  28.  
  29.  
  30.  
  31. //This will run only one time.
  32. void setup(){
  33.  
  34. //Set motorpins as outputs
  35. pinMode(motorPin1, OUTPUT);
  36. pinMode(motorPin2, OUTPUT);
  37. pinMode(motorPin3, OUTPUT);
  38. pinMode(motorPin4, OUTPUT);
  39.  
  40. Serial.begin(9600);
  41. Serial.println("MASTER READER NODE");
  42. Serial.print("Maximum Slave Nodes: ");
  43. Serial.println(NODE_MAX);
  44. Serial.print("Payload size: ");
  45. Serial.println(PAYLOAD_SIZE);
  46. Serial.println("***********************");
  47.  
  48. Wire.begin(); // Activate I2C link
  49.  
  50. }
  51.  
  52.  
  53.  
  54. void loop(){
  55. getDataFromNodes(); //read data from nodes
  56.  
  57. if(nodeArray[1][1]==3){ // if 3 ping sensors
  58. int leftsensor = nodeArray[1][7];//left 7
  59. int frontsensor = nodeArray[1][8];//front 8
  60. int rightsensor = nodeArray[1][9];//right 9
  61. Serial.println("sensors");
  62. Serial.println(leftsensor);
  63. Serial.println(frontsensor);
  64. Serial.println(rightsensor);
  65.  
  66. int mindistance = 30;
  67.  
  68. if(frontsensor >= mindistance || frontsensor == 0){ //if nothing infront
  69.  
  70. if(leftsensor <= mindistance && leftsensor != 0){ //unless something to left
  71. right();
  72. if(rightsensor <= mindistance && rightsensor != 0){ //and something to right
  73. hardright(); // spin right
  74. }
  75. }
  76. if(rightsensor <= mindistance && rightsensor != 0){ //unless something to right
  77. left();
  78. if(leftsensor <= mindistance && leftsensor != 0){ //and something to left
  79. hardright(); // spin right
  80. }
  81. }
  82. if(rightsensor >= mindistance || rightsensor == 0){
  83. if(leftsensor >= mindistance || leftsensor == 0){
  84. forward(); //if nothing to the sides
  85. }
  86. }
  87. }
  88.  
  89. if(frontsensor <= mindistance && frontsensor != 0){ //if something infront
  90. hardright();
  91. if(leftsensor <= mindistance && leftsensor != 0){ //and something to left
  92. hardright();
  93. if(rightsensor <= mindistance && rightsensor !=0){ //and something to right
  94. backward(); // go backwards
  95. }
  96. }
  97. if(rightsensor <= mindistance && rightsensor != 0){ //and something to right
  98. hardleft();
  99. if(leftsensor <= mindistance && leftsensor !=0){ //and something to left
  100. backward(); // go backwards
  101. }
  102. }
  103. }
  104. }
  105. //displayOnScreen();
  106.  
  107. delay(NODE_READ_DELAY);
  108. }
  109.  
  110.  
  111. void getDataFromNodes(){
  112. for (int k=0; k < NODE_MAX-1; k++){
  113. if(nodeArray[k][1] != 0){
  114. Wire.requestFrom(nodeArray[k][0],PAYLOAD_SIZE); //request data from node
  115. if(Wire.available() == PAYLOAD_SIZE){
  116. for (int i = 0; i < PAYLOAD_SIZE; i++){
  117. nodeArray[k][i+2] = Wire.read(); // get nodes data
  118. }
  119. }//if wire available
  120. }//if mode != 0
  121. }//for #nodes
  122. }
  123.  
  124.  
  125.  
  126. //Motor Control -
  127. //Motor A: motorPin1,motorpin2
  128. //Motor B: motorpin3,motorpin4
  129. void left(){
  130. //
  131. digitalWrite(motorPin1, HIGH);
  132. digitalWrite(motorPin2, LOW);
  133. digitalWrite(motorPin3, LOW);
  134. digitalWrite(motorPin4, LOW);
  135. }
  136. void hardleft(){
  137. //
  138. digitalWrite(motorPin1, HIGH);
  139. digitalWrite(motorPin2, LOW);
  140. digitalWrite(motorPin3, LOW);
  141. digitalWrite(motorPin4, HIGH);
  142. Serial.println("Hard Left");
  143. }
  144. void forward(){
  145. //
  146. digitalWrite(motorPin1, HIGH);
  147. digitalWrite(motorPin2, LOW);
  148. digitalWrite(motorPin3, HIGH);
  149. digitalWrite(motorPin4, LOW);
  150. Serial.println("Forward");
  151. }
  152. void backward(){
  153. //
  154. digitalWrite(motorPin1, LOW);
  155. digitalWrite(motorPin2, HIGH);
  156. digitalWrite(motorPin3, LOW);
  157. digitalWrite(motorPin4, HIGH);
  158. Serial.println("Backward");
  159. }
  160. void right(){
  161. //
  162. digitalWrite(motorPin1, LOW);
  163. digitalWrite(motorPin2, LOW);
  164. digitalWrite(motorPin3, HIGH);
  165. digitalWrite(motorPin4, LOW);
  166. }
  167. void hardright(){
  168. //
  169. digitalWrite(motorPin1, LOW);
  170. digitalWrite(motorPin2, HIGH);
  171. digitalWrite(motorPin3, HIGH);
  172. digitalWrite(motorPin4, LOW);
  173. Serial.println("Hard Right");
  174. }
  175. void explode(){
  176. //And this code will stop motors
  177. digitalWrite(motorPin1, LOW);
  178. digitalWrite(motorPin2, LOW);
  179. digitalWrite(motorPin3, LOW);
  180. digitalWrite(motorPin4, LOW);
  181. }
  182.  
  183. void example(){
  184. delay(1000);
  185. forward();
  186. delay(100);
  187. left();
  188. delay(1000);
  189. hardleft();
  190. delay(1000);
  191. explode();
  192. delay(1000);
  193. hardright();
  194. delay(1700);
  195. right();
  196. delay(1000);
  197. backward();
  198. delay(2000);
  199. explode();
  200. delay(4000);
  201. }
Advertisement
Add Comment
Please, Sign In to add comment
Advertisement