RenabaReD

Untitled

Dec 5th, 2019
95
Never
Not a member of Pastebin yet? Sign Up, it unlocks many cool features!
  1. #define INTERVAL 1000
  2. #define P_TURN 10 // en pour mille
  3.  
  4. #define PINDIR1 4
  5. #define PINPWM1 5
  6. #define PINDIR2 2
  7. #define PINPWM2 3
  8. #define PINTRIG 12
  9. #define PINECHO 11
  10.  
  11.  
  12.  
  13. /* Constantes pour le timeout */
  14. const unsigned long MEASURE_TIMEOUT = 25000UL; // 25ms = ~8m à 340m/s
  15.  
  16. typedef struct {
  17. bool far_l, far_r, close_l, close_r;
  18. } IR_Capture;
  19.  
  20. typedef enum { INIT, RUN, LEFT, RIGHT, PAUSE, STOP } State ;
  21. typedef enum { L, R } Direction;
  22. bool line_l, line_r; // ligne sur la gauche/droite (capteur infrarouge)
  23. bool obstacle_f; // obstacle devant (capteur ultrason)
  24. bool turn, turn_l; // y a une proba de tourner au cours de la course (marche aléatoire), et une proba pour savoir si on tourne à gauche ou droite
  25. bool setting_up, fatal_error, success, start;
  26. int speedL=0;
  27. int speedR=0;
  28. double prop =1.100;
  29. bool led=false;
  30. int vit=50;
  31.  
  32. State state;
  33.  
  34. void setup() {
  35. // port série
  36. Serial.begin(115200);
  37. //on initialise les pins du moteur 1
  38. pinMode(PINDIR1, OUTPUT);
  39. pinMode(PINPWM1, OUTPUT);
  40. pinMode(PINDIR2, OUTPUT);
  41. pinMode(PINPWM2, OUTPUT);
  42.  
  43. digitalWrite(PINDIR1,LOW);
  44. digitalWrite(PINDIR2,LOW);
  45.  
  46. pinMode(PINTRIG, OUTPUT);
  47. digitalWrite(PINTRIG, LOW); // La broche TRIGGER doit être à LOW au repos
  48. pinMode(PINECHO, INPUT);
  49.  
  50. pinMode(LED_BUILTIN, OUTPUT);
  51. digitalWrite(LED_BUILTIN, LOW);
  52.  
  53. state=INIT;
  54. }
  55.  
  56. IR_Capture detect_IR()
  57. {
  58. // capteurs infrarouges
  59. IR_Capture cap;
  60.  
  61. return cap;
  62. }
  63.  
  64. void detect_US()
  65. {
  66. // capteurs ultrasons
  67. digitalWrite(PINTRIG, HIGH);
  68. delayMicroseconds(10);
  69. digitalWrite(PINTRIG, LOW);
  70. long measure = pulseIn(PINECHO, HIGH, MEASURE_TIMEOUT);
  71. Serial.println(measure);
  72. obstacle_f = (measure!=0 && measure<2000);
  73. }
  74.  
  75. void refresh()
  76. {
  77. // met à jour les globals (conditions)
  78. detect_IR();
  79. detect_US();
  80. turn = (random(1000)<P_TURN);
  81. turn_l = (random(2)==0);
  82. }
  83.  
  84. void transition()
  85. {
  86. if (fatal_error || success)
  87. {
  88. state = STOP;
  89. return;
  90. }
  91. switch (state)
  92. {
  93. case INIT:
  94. if (setting_up)
  95. {
  96. state = INIT;
  97. }
  98. else
  99. {
  100. if (!obstacle_f && !line_l && !line_r && !turn)
  101. {
  102. state = RUN;
  103. }
  104. else if (turn && turn_l)
  105. {
  106. state = LEFT;
  107. }
  108. else if (turn && !turn_l)
  109. {
  110. state = RIGHT;
  111. }
  112. else
  113. {
  114. state = PAUSE;
  115. }
  116. }
  117. break;
  118. case RUN:
  119. if (!line_l && !line_r && !turn && !obstacle_f)
  120. {
  121. state = RUN;
  122. }
  123. else if (line_r || (!line_l && !line_r && turn && turn_l) && !obstacle_f)
  124. {
  125. state = LEFT;
  126. }
  127. else if (line_l || (!line_l && !line_r && turn && !turn_l) && !obstacle_f)
  128. {
  129. state = RIGHT;
  130. }
  131. else if (obstacle_f)
  132. {
  133. state = PAUSE;
  134. }
  135. else
  136. {
  137. state = STOP;
  138. }
  139. break;
  140. case LEFT:
  141. if (!line_l && !line_r && !obstacle_f)
  142. {
  143. state = RUN;
  144. }
  145. else if (line_l || line_r && !obstacle_f)
  146. {
  147. state = LEFT;
  148. }
  149. else if (obstacle_f)
  150. {
  151. state = PAUSE;
  152. }
  153. else
  154. {
  155. state = STOP;
  156. }
  157. break;
  158. case RIGHT:
  159. if (!line_l && !line_r && !obstacle_f)
  160. {
  161. state = RUN;
  162. }
  163. else if (line_l || line_r && !obstacle_f)
  164. {
  165. state = RIGHT;
  166. }
  167. else if (obstacle_f)
  168. {
  169. state = PAUSE;
  170. }
  171. else
  172. {
  173. state = STOP;
  174. }
  175. break;
  176. case PAUSE:
  177. if (!line_l && !line_r && !obstacle_f)
  178. {
  179. state = RUN;
  180. }
  181. else if (line_r && !obstacle_f)
  182. {
  183. state = LEFT;
  184. }
  185. else if (line_l && !obstacle_f)
  186. {
  187. state = RIGHT;
  188. }
  189. else if (obstacle_f)
  190. {
  191. state = PAUSE;
  192. }
  193. break;
  194. case STOP:
  195. if (start)
  196. {
  197. state = INIT;
  198. }
  199. else
  200. {
  201. state = STOP;
  202. }
  203. break;
  204. default:
  205. state = STOP;
  206. break;
  207. }
  208.  
  209. }
  210.  
  211. void loop() {
  212. // put your main code here, to run repeatedly:
  213. unsigned long t0=millis();
  214. refresh();
  215. transition();
  216. //vit=(vit+7)%255;
  217. //speedL=250;
  218.  
  219. switch (state)
  220. {
  221. case INIT:
  222. Serial.println(F("INIT"));
  223. state=RUN;
  224. break;
  225. case RUN:
  226. Serial.println(F("RUN"));
  227. led=true;
  228. speedL=vit;
  229. speedR=vit;
  230. break;
  231. case LEFT:
  232. Serial.println(F("LEFT"));
  233. led=true;
  234. speedL=0;
  235. speedR=vit;
  236. break;
  237. case RIGHT:
  238. Serial.println(F("RIGHT"));
  239. led=true;
  240. speedL=vit;
  241. speedR=0;
  242. break;
  243. case PAUSE:
  244. Serial.println(F("PAUSE"));
  245. led=false;
  246. speedL=0;
  247. speedR=0;
  248. break;
  249. case STOP:
  250. Serial.println(F("STOP"));
  251. led=false;
  252. speedL=0;
  253. speedR=0;
  254. break;
  255. default:
  256. break;
  257. }
  258.  
  259. analogWrite(PINPWM1,digitalRead(!PINDIR1 )?(int)(prop*speedL):(int)prop*(255-prop*speedL));
  260. analogWrite(PINPWM2,digitalRead(!PINDIR2 )?speedR:255-speedR);
  261. digitalWrite(LED_BUILTIN, (led)?HIGH:LOW);
  262. //Serial.print(F("Current state: "));
  263. //Serial.println(state);
  264.  
  265. unsigned long elapsed=millis()-t0;
  266. if (elapsed>INTERVAL) printf("Excessive execution time\n");
  267. else delay(INTERVAL-elapsed);
  268.  
  269. }
RAW Paste Data