API tools faq
paste
Advertisement
Guest User

Untitled

a guest
Jan 21st, 2019
79
0
Never
Add comment
Not a member of Pastebin yet? Sign Up, it unlocks many cool features!
text 8.78 KB | None | 0 0
raw download clone embed print report
  1. const int trigRight = 4;
  2. const int echoRight = 2;
  3. int time1,d=1;
  4. const int trigLeft = 12;
  5. const int echoLeft = 13;
  6. long right, left, front;
  7. const int trigFront = 8;
  8. const int echoFront = 7;
  9. //const int FRight =
  10. int dem=1;
  11.  
  12. int controlLeftWheel = 9;
  13. int const in1 = 6;
  14. int const in2 = 11;
  15. int controlRightWheel = 10;
  16. int const in3 = 3;
  17. int const in4 = 5;
  18.  
  19. float Kp=40, Ki=0 , Kd=185;
  20. float error=0, P=0, I=0, D=0, PID=0;
  21. float previous_error=0;
  22. int sensor[5]={0, 0, 0, 0, 0};
  23. int tdd=190;
  24. int a=1, b=0;
  25.  
  26. void park1(void);
  27. void read_sensor_values(void);
  28. void calculate_pid(void);
  29. void motor_control(void);
  30.  
  31. void setup()
  32. {
  33. Serial.begin(9600);
  34. pinMode(trigLeft, OUTPUT);
  35. pinMode(echoLeft, INPUT);
  36.  
  37. pinMode(trigRight, OUTPUT);
  38. pinMode(echoRight, INPUT);
  39.  
  40. pinMode(trigFront, OUTPUT);
  41. pinMode(echoFront, INPUT);
  42.  
  43. pinMode(controlLeftWheel,OUTPUT); //PWM Pin 1
  44. pinMode(controlRightWheel,OUTPUT); //PWM Pin 2
  45. pinMode(in1,OUTPUT); //Left Motor Pin 1
  46. pinMode(in2,OUTPUT); //Left Motor Pin 2
  47. pinMode(in3,OUTPUT); //Right Motor Pin 1
  48. pinMode(in4,OUTPUT); //Right Motor Pin 2
  49. }
  50.  
  51.  
  52. void loop()
  53.  
  54. {
  55.  
  56. read_sensor_values();
  57. //if ((error == previous_error)&&(dem==1)) {park1(); dem++;}
  58.  
  59. // read_sensor_values();
  60. calculate_pid();
  61. motor_control();
  62. // front = getDistance(trigFront, echoFront);
  63. Serial.println(d);
  64. }
  65. //------------------------------------------------------------------------------------------
  66. //MAZE
  67. void moveStop() {
  68. analogWrite(controlLeftWheel, 0);
  69. analogWrite(controlRightWheel, 0);
  70.  
  71. digitalWrite(in1, LOW);
  72. digitalWrite(in2, LOW);
  73. digitalWrite(in3, LOW);
  74. digitalWrite(in4, LOW);
  75. }
  76.  
  77. void moveForward() {
  78. analogWrite(controlLeftWheel, 100);
  79. analogWrite(controlRightWheel, 100);
  80.  
  81. digitalWrite(in1, HIGH);
  82. digitalWrite(in2, LOW);
  83. digitalWrite(in3, HIGH);
  84. digitalWrite(in4, LOW);
  85. }
  86.  
  87. void moveBack() {
  88. analogWrite(controlLeftWheel, 100);
  89. analogWrite(controlRightWheel, 100);
  90.  
  91. digitalWrite(in1, LOW);
  92. digitalWrite(in2, HIGH);
  93. digitalWrite(in3, LOW);
  94. digitalWrite(in4, HIGH);
  95. }
  96.  
  97. int getDistance(int trig, int echo) {
  98. /* Phát xung từ chân trig */
  99. int distance; // biến đo khoảng cách
  100. int duration; // biến đo thoời gian
  101. digitalWrite(trig,0); // tắt chân trig
  102. delayMicroseconds(2);
  103. digitalWrite(trig,1); // phát xung từ chân trig
  104. delayMicroseconds(5); // xung có độ dài 5 microSeconds
  105. digitalWrite(trig,0); // tắt chân trig
  106. duration = pulseIn(echo,tdd);
  107. distance = (int) (duration/2/29.412);
  108. //Serial.println(distance);
  109. return distance;
  110. delay(5);
  111. }
  112.  
  113.  
  114. void stepForward(int time, int td) {
  115. moveStop();
  116. analogWrite(controlLeftWheel, td);
  117. analogWrite(controlRightWheel, td);
  118. digitalWrite(in1, HIGH);
  119. digitalWrite(in2, LOW);
  120. digitalWrite(in3, HIGH);
  121. digitalWrite(in4, LOW);
  122. delay(time);
  123. moveStop();
  124. delay(500);
  125. }
  126. void stepBack(int time, int td) {
  127. moveStop();
  128. analogWrite(controlLeftWheel, td);
  129. analogWrite(controlRightWheel, td);
  130. digitalWrite(in1, LOW);
  131. digitalWrite(in2, HIGH);
  132. digitalWrite(in3, LOW);
  133. digitalWrite(in4, HIGH);
  134. delay(time);
  135. moveStop();
  136. delay(500);
  137. }
  138. void readsensor(){
  139. front = getDistance(trigFront, echoFront);
  140. left = getDistance(trigLeft, echoLeft);
  141. right = getDistance( trigRight, echoRight);
  142. delay(5);
  143. // Serial.print( left);
  144. // Serial.print(' ');
  145. // Serial.print( front);
  146. // Serial.print(' ');
  147. // Serial.println( right);
  148. //
  149. }
  150. void turnRightRotation() {
  151. moveStop();
  152. analogWrite(controlLeftWheel, 150);
  153. analogWrite(controlRightWheel, 150);
  154. digitalWrite(in1, HIGH);
  155. digitalWrite(in2, LOW);
  156. digitalWrite(in3, LOW);
  157. digitalWrite(in4, HIGH);
  158. delay(350);
  159. moveStop();
  160. //delay(500);
  161. }
  162.  
  163. void turnLeftRotation() {
  164. moveStop();
  165. analogWrite(controlLeftWheel, 150);
  166. analogWrite(controlRightWheel, 150);
  167. digitalWrite(in1, LOW);
  168. digitalWrite(in2, HIGH);
  169. digitalWrite(in3, HIGH);
  170. digitalWrite(in4, LOW);
  171. delay(350);
  172. moveStop();
  173. //delay(500);
  174. }
  175. void turnBackRotation() {
  176.  
  177. moveStop();
  178. analogWrite(controlLeftWheel, 150);
  179. analogWrite(controlRightWheel, 150);
  180.  
  181. digitalWrite(in1, LOW);
  182. digitalWrite(in2, HIGH);
  183. digitalWrite(in3, HIGH);
  184. digitalWrite(in4, LOW);
  185. delay(500);
  186. moveStop();
  187. delay(500);
  188. }
  189. //-------------------------------------------------------------------------------------------------------------
  190. //PID-LINE
  191. void park1()
  192. {
  193. // while (error == 5){
  194. analogWrite(controlLeftWheel,100);
  195. analogWrite(controlRightWheel,100);
  196. digitalWrite(in1, HIGH);
  197. digitalWrite(in2, LOW);
  198. digitalWrite(in3, HIGH);
  199. digitalWrite(in4, LOW);
  200. delay(1500);
  201. read_sensor_values();
  202. // }
  203. // if (error == -5){
  204. turnRightRotation();
  205. // stepBack(10);
  206. // stepForward(500);
  207. //break;
  208. // }
  209. }
  210. void read_sensor_values()
  211. {
  212. sensor[0]=digitalRead(A4);
  213. sensor[1]=digitalRead(A3);
  214. sensor[2]=digitalRead(A2);
  215. sensor[3]=digitalRead(A1);
  216. sensor[4]=digitalRead(A0);
  217.  
  218. Serial.print(sensor[0]);
  219. Serial.print(sensor[1]);
  220. Serial.print(sensor[2]);
  221. Serial.print(sensor[3]);
  222. Serial.println(sensor[4]);
  223.  
  224. if((sensor[0]==a)&&(sensor[1]==a)&&(sensor[2]==a)&&(sensor[3]==a)&&(sensor[4]==b))
  225. error=4;
  226. else if((sensor[0]==a)&&(sensor[1]==a)&&(sensor[2]==a)&&(sensor[3]==b)&&(sensor[4]==b))
  227. error=3;
  228. else if((sensor[0]==a)&&(sensor[1]==a)&&(sensor[2]==a)&&(sensor[3]==b)&&(sensor[4]==a))
  229. error=2;
  230. else if((sensor[0]==a)&&(sensor[1]==a)&&(sensor[2]==b)&&(sensor[3]==b)&&(sensor[4]==a))
  231. error=1;
  232. else if((sensor[0]==a)&&(sensor[1]==a)&&(sensor[2]==b)&&(sensor[3]==a)&&(sensor[4]==a))
  233. error=0;
  234. else if((sensor[0]==a)&&(sensor[1]==b)&&(sensor[2]==b)&&(sensor[3]==a)&&(sensor[4]==a))
  235. error=-1;
  236. else if((sensor[0]==a)&&(sensor[1]==b)&&(sensor[2]==a)&&(sensor[3]==a)&&(sensor[4]==a))
  237. error=-2;
  238. else if((sensor[0]==b)&&(sensor[1]==b)&&(sensor[2]==a)&&(sensor[3]==a)&&(sensor[4]==a))
  239. error=-3;
  240. else if((sensor[0]==b)&&(sensor[1]==a)&&(sensor[2]==a)&&(sensor[3]==a)&&(sensor[4]==a))
  241. error=-4;
  242. else if((sensor[0]==a)&&(sensor[1]==a)&&(sensor[2]==a)&&(sensor[3]==a)&&(sensor[4]==a))
  243. error=previous_error;
  244. else if((sensor[0]==b)&&(sensor[1]==b)&&(sensor[2]==b)&&(sensor[3]==b)&&(sensor[4]==b))
  245. error=previous_error;
  246. else if((sensor[0]==b)&&(sensor[1]==b)&&(sensor[2]==b)&&(sensor[3]==a)&&(sensor[4]==a))
  247.  
  248. nhien();
  249. else if((sensor[0]==a)&&(sensor[1]==a)&&(sensor[2]==b)&&(sensor[3]==b)&&(sensor[4]==b))
  250. nhien();
  251. // else if((sensor[0]==a)&&(sensor[1]==b)&&(sensor[2]==b)&&(sensor[3]==b)&&(sensor[4]==b))
  252. // nhien();
  253. // else if((sensor[0]==b)&&(sensor[1]==b)&&(sensor[2]==b)&&(sensor[3]==b)&&(sensor[4]==a))
  254. // nhien();
  255. //Serial.println(error);
  256. }
  257. void nhien()
  258. {
  259. moveStop(); delay(50);
  260. if (getDistance(trigFront, echoFront) < 15)
  261. {
  262. d++;
  263. Serial.println("Chan");
  264. Serial.println(getDistance(trigFront, echoFront));
  265. moveStop();
  266. delay(5000);
  267. }
  268. else {
  269. stepForward(100,120);
  270. }
  271.  
  272. // if (d==2){ moveStop();
  273. // delay(100);
  274. // turnRightRotation();
  275. // stepForward(1000,120);
  276. // turnLeftRotation();
  277. // stepForward(400,120);
  278. // turnLeftRotation();
  279. // stepForward(1000,120);
  280. // d++;
  281. // }
  282. // if (d==5){moveStop();
  283. // delay(10000000);}
  284.  
  285. }
  286. void calculate_pid()
  287. {
  288. P = error;
  289. I = I + error;
  290. D = error - previous_error;
  291. PID = (Kp*P) + (Ki*I) + (Kd*D);
  292. previous_error=error;
  293. // Serial.print(sensor[0]);
  294. // Serial.print(sensor[1]);
  295. // Serial.print(sensor[2]);
  296. // Serial.print(sensor[3]);
  297. // Serial.println(sensor[4]);
  298. // Serial.println(error);
  299. // Serial.println( PID );
  300. // Serial.println( PID );
  301.  
  302. }
  303.  
  304. void motor_control()
  305. { // readsensor();
  306. // if (front < 10) stepBack(3000,10);
  307. // if (getDistance(trigFront, echoFront) < 15) stepForward(1000,40);
  308. // Calculating the effective motor speed:
  309. int leftspeed = tdd+PID;
  310. int rightspeed = tdd-PID;
  311.  
  312. // The motor speed should not exceed the max PWM value
  313. //constrain(leftspeed,0,150);
  314. //constrain(rightspeed,0,150);
  315. if (rightspeed < 0)
  316. rightspeed = 0;
  317. if (rightspeed > tdd)
  318. rightspeed = tdd;
  319. if (leftspeed<0)
  320. leftspeed = 0;
  321. if (leftspeed > tdd)
  322. leftspeed = tdd;
  323. //Serial.print(rightspeed);
  324. // Serial.print(' ');
  325. // Serial.println(leftspeed);
  326. // readsensor();
  327. // if (front < 15) { moveStop();}
  328. analogWrite(controlRightWheel, rightspeed);
  329. analogWrite(controlLeftWheel, leftspeed);
  330. digitalWrite(in1,HIGH); // left
  331. digitalWrite(in2,LOW); // left
  332. digitalWrite(in3,HIGH); // right
  333. digitalWrite(in4,LOW); // right
  334. }
Advertisement
Add Comment
Please, Sign In to add comment
Advertisement
Public Pastes
Advertisement
We use cookies for various purposes including analytics. By continuing to use Pastebin, you agree to our use of cookies as described in the Cookies Policy.  OK, I Understand
Not a member of Pastebin yet?
Sign Up, it unlocks many cool features!