自主避障代码.doc

1. int E1 = 5;2. int M1 = 4;3. int E2 = 6;4. int M2 = 7;5. int LightSensorPin = 0;6. int LeftDistanceSensorPin = 9;7. int RightDistanceSensorPin = 8;8. int LEDPin = 13;9. int spd = 200;10. 11. void stop(void) 12. analogWrite(E1,0);13. analogWrite(E2,0);14. 15. 16. void forward(char a,char b) 17. analogWrite (E1,a);18. digitalWrite(M1,HIGH);19. analogWrite (E2,b);20. digitalWrite(M2,HIGH);21. 22. 23. void backward(char a,char b) 24. analogWrite (E1,a);25. digitalWrite(M1,LOW);26. analogWrite (E2,b);27. digitalWrite(M2,LOW);28. 29. 30. void turn_L(char a,char b) 31. analogWrite (E1,a);32. digitalWrite(M1,HIGH);33. analogWrite (E2,b);34. digitalWrite(M2,LOW);35. 36. 37. void turn_R(char a,char b) 38. analogWrite (E1,a);39. digitalWrite(M1,LOW);40. analogWrite (E2,b);41. digitalWrite(M2,HIGH);42. 43. 44. void checkEnv()45. int light,R_dist,L_dist;46. 47. Serial.println(- checkEnv -);48. 49. light=analogRead(LightSensorPin); /Read Light Sensor50. Serial.print(Light Sensor: );51. Serial.println(light, DEC); /Print result to serial52. 53. L_dist=digitalRead(LeftDistanceSensorPin); /Read Distance Sensor54. Serial.print(LeftDistance Sensor: );55. Serial.println(L_dist, DEC); /Print result to serial56. 57. R_dist=digitalRead(RightDistanceSensorPin); /Read Distance Sensor58. Serial.print(RightDistance Sensor: );59. Serial.println(R_dist, DEC); /Print result to serial60. 61. /If Light Sensor great than 400, light the LED62. if (light 400) digitalWrite(LEDPin, HIGH);63. else digitalWrite(LEDPin, LOW);64. 65. if (!L_dist & R_dist) / turn right66. move(1);67. else if (L_dist & !R_dist) /turn left68. move(0);69. else if (!L_dist & !R_dist) /random turn 70. move(random(2);71. 72. 73. int clearPath()74. int R_dist,L_dist;75. Serial.println(- clearPath -);76. L_dist=digitalRead(LeftDistanceSensorPin); /Read Distance Sensor77. R_dist=digitalRead(RightDistanceSensorPin); /Read Distance Sensor78. return (L_dist & R_dist);79. 80. 81. 82. void move(int dir) 83. int flag;84. 85. Serial.println(- move -);86. flag = clearPath();87. while (!flag) 88. if (dir) /right89. turn_R(spd,spd);90. else91. turn_L(spd,spd);92. delay (50);93. flag=clearPath();94. 95. 96. 97. void setup() 98. pinMode(E1, OUTPUT);99. pinMode(E2, OUTPUT);100. pinMode(M1, OUTPUT);101. pinMode(M2, OUTPUT);102. pinMode(LeftDistanceSensorPin, INPUT);103. pinMode(RightDistanceSensorPin, INPUT);104. pinMode(LEDPin, OUTPUT);105. Serial.b