Advertisement
Guest User

MY code

a guest
Jun 20th, 2018
78
0
Never
Not a member of Pastebin yet? Sign Up, it unlocks many cool features!
text 5.61 KB | None | 0 0
  1. //My line follower code basic
  2.  
  3.  
  4. #define sensorNum 8
  5. #define maxSpeed 240
  6. int rotationSpeed=200;
  7.  
  8. int blackLimit[sensorNum];
  9. int digitalReading[sensorNum];
  10. const int motorPin1=6,motorPin2=5; //right motor
  11. const int motorPin3=9,motorPin4=10; //left motor
  12.  
  13. float error, prevError=0;
  14. float mappedValue, targetValue = 7;
  15.  
  16. float safety=0.35; //fixed
  17.  
  18. float kp=40;
  19. float kd=3;
  20. float kt;
  21.  
  22. int motorResponse;
  23. float correction;
  24.  
  25. int leftSpeed,rightSpeed;
  26. int digitalValue;
  27. int allBlack=0;
  28. int CRotateKey=0,ACRotateKey=0;
  29.  
  30. float lastAct;
  31. int time=3;
  32. int danceDelay = 350;
  33.  
  34. int prev, curr, diff;
  35. int leftSide,rightSide;
  36. int leftIR,rightIR;
  37. int allBlackcount=0;
  38.  
  39.  
  40.  
  41. void setup()
  42. {
  43. //initialize for IR pins
  44. for(int i = 0; i < sensorNum; i++)
  45. {
  46. pinMode(A0 + i, INPUT);
  47. }
  48.  
  49. //initialize motor pins
  50. pinMode(motorPin1, OUTPUT);
  51. pinMode(motorPin2, OUTPUT);
  52. pinMode(motorPin3, OUTPUT);
  53. pinMode(motorPin4, OUTPUT);
  54. delay(1000);
  55. calibration();
  56.  
  57. Serial.begin(9600);
  58. }
  59.  
  60.  
  61. void loop() {
  62.  
  63.  
  64. /*if(allBlack == 1) //depend upon track for acute angle
  65. {
  66. plannedCRotate();
  67. delay(500);// ive to check it for my bot
  68. }
  69.  
  70. if(leftSide == 1)
  71. {
  72. plannedACRotate();
  73. delay(500);
  74. }
  75.  
  76. //ei tukun ei sudhu
  77.  
  78. */
  79.  
  80. if(mappedValue != 100)
  81. {
  82. pid();
  83. motor(leftSpeed, rightSpeed);
  84. }
  85.  
  86. else
  87. {
  88. if(leftIR == 0 && rightIR == 1)
  89. {
  90. plannedCRotate();
  91. while(digitalReading[3] != 1)
  92. {
  93. sensorMapping();
  94. }
  95. rightIR = 0;
  96. pid();
  97. motor(leftSpeed, rightSpeed);
  98. }
  99.  
  100. else if(leftIR == 1 && rightIR == 0)
  101. {
  102. plannedACRotate();
  103. while(digitalReading[3] != 1)
  104. {
  105. sensorMapping();
  106. }
  107. leftIR = 0;
  108. pid();
  109. motor(leftSpeed, rightSpeed);
  110. }
  111.  
  112. else if (leftIR == 0 && rightIR == 0)
  113. {
  114. motor(maxSpeed, maxSpeed);
  115. }
  116. }
  117.  
  118.  
  119.  
  120. }
  121.  
  122.  
  123.  
  124. void sensorMapping()
  125. {
  126. int sum=0,count=0; int digitalReading[sensorNum];
  127. for(int i = 0; i < sensorNum; i++)
  128. {
  129. if(analogRead(i) < blackLimit[i])
  130. {
  131. sum += i*2;
  132. count++;
  133. digitalReading[i] = 1;
  134. }
  135.  
  136. else
  137. {
  138. digitalReading[i] = 0;
  139. }
  140.  
  141. }
  142.  
  143. if(count != 0)
  144. {
  145. mappedValue = sum/count;
  146. }
  147. else
  148. mappedValue = 100;
  149.  
  150. if(digitalReading[0] || digitalReading[sensorNum - 1])
  151. {
  152. leftIR = digitalReading[0];
  153. rightIR = digitalReading[1];
  154. }
  155.  
  156. if(count == sensorNum)
  157. {
  158. allBlack = 1;
  159. }
  160. else
  161. allBlack = 0;
  162.  
  163.  
  164. //turn full right or left depends upon track
  165. /* if(digitalReading[0] || digitaReading[1] || digitalReading[2])
  166. {
  167. leftSide = 1;
  168. }
  169. else
  170. leftSide = 0;
  171. if(digitalReading[5] || digitaReading[6] || digitalReading[7])
  172. {
  173. rightSide = 1;
  174. }
  175. else
  176. rightSide = 0;
  177.  
  178. */
  179.  
  180. }
  181.  
  182.  
  183.  
  184.  
  185. void pid()
  186. {
  187.  
  188. error=targetValue-mappedValue;
  189. correction=(kp*error)+(kd*(error-prevError));
  190. prevError=error;
  191. motorResponse=(int)correction;
  192.  
  193. if(motorResponse>maxSpeed) motorResponse=maxSpeed;
  194.  
  195. if(motorResponse<-maxSpeed) motorResponse=-maxSpeed;
  196.  
  197. if(motorResponse>0)
  198. {
  199. rightSpeed=maxSpeed;
  200. leftSpeed=maxSpeed-motorResponse;
  201. }
  202. else
  203. {
  204. rightSpeed=maxSpeed+ motorResponse;
  205. leftSpeed=maxSpeed;
  206. }
  207.  
  208. }
  209.  
  210.  
  211.  
  212.  
  213. void motor(int left, int right)
  214. {
  215.  
  216. if(right>0)
  217. {
  218. analogWrite(motorPin1,right);
  219. analogWrite(motorPin2,0);
  220. }
  221. else
  222. {
  223. analogWrite(motorPin1,0);
  224. analogWrite(motorPin2,-right);
  225. }
  226.  
  227. if(left>0)
  228. {
  229. analogWrite(motorPin3,left);
  230. analogWrite(motorPin4,0);
  231. }
  232. else
  233. {
  234. analogWrite(motorPin3,0);
  235. analogWrite(motorPin4,-left);
  236. }
  237.  
  238. }
  239.  
  240.  
  241.  
  242. void brake(void)
  243. {
  244. analogWrite(motorPin1, 0);
  245. analogWrite(motorPin2, 0);
  246. analogWrite(motorPin3, 0);
  247. analogWrite(motorPin4, 0);
  248. }
  249.  
  250. void plannedACRotate()
  251. {
  252. analogWrite(motorPin1,rotationSpeed);
  253. analogWrite(motorPin2, 0);
  254. analogWrite(motorPin3, 0);
  255. analogWrite(motorPin4,rotationSpeed);
  256.  
  257. }
  258.  
  259. void plannedCRotate()
  260. {
  261. analogWrite(motorPin1,0);
  262. analogWrite(motorPin2, rotationSpeed);
  263. analogWrite(motorPin3, rotationSpeed);
  264. analogWrite(motorPin4,0);
  265.  
  266. }
  267.  
  268.  
  269.  
  270. /*void dance(void)
  271. {
  272.  
  273. int loopCounter = (int) (danceDelay / diff);
  274.  
  275. for(int i = loopCounter; i > 0; i--)
  276. {
  277.  
  278. plannedCRotate();
  279.  
  280. sensorMapping();
  281.  
  282. if(mappedValue != 100)
  283. {
  284. pid();
  285. motor(leftSpeed, rightSpeed);
  286. return;
  287. }
  288.  
  289. }
  290.  
  291. for(int i = 2 * loopCounter; i > 0; i--)
  292. {
  293. plannedACRotate();
  294.  
  295. sensorMapping();
  296.  
  297. if(mappedValue != 100)
  298. {
  299. pid();
  300. motor(leftSpeed, rightSpeed);
  301. return;
  302. }
  303.  
  304. }
  305.  
  306. } */
  307.  
  308.  
  309. //auto calibration
  310. void calibration()
  311. {
  312. plannedCRotate();
  313. float upSum = 0,lowSum = 0;
  314. int sensorArray[sensorNum][2];
  315.  
  316. for(int i = 0; i < sensorNum; i++)
  317. {
  318. sensorArray[i][0] = analogRead(A0+i);
  319. sensorArray[i][1] = analogRead(A0+i);
  320. }
  321.  
  322.  
  323. int loopCounter = (int)(time * 1000 / 2.5);
  324. while(loopCounter)
  325. {
  326. for(int i = 0; i < sensorNum; i++)
  327. {
  328. if(analogRead(A0+i)<sensorArray[i][0]) sensorArray[i][0]=analogRead(A0+i);
  329. if(analogRead(A0+i)>sensorArray[i][1]) sensorArray[i][1]=analogRead(A0+i);
  330. }
  331. loopCounter--;
  332.  
  333. }
  334.  
  335. for(int i=0; i < sensorNum; i++)
  336. blackLimit[i] = (int)(sensorArray[i][0] + safety * (sensorArray[i][1] - sensorArray[i][0]));
  337. prev = millis();
  338. sensorMapping ();
  339. curr= millis();
  340. diff = curr - prev;
  341.  
  342. brake();
  343. delay(1000);
  344.  
  345. }
Advertisement
Add Comment
Please, Sign In to add comment
Advertisement