Guest User

Untitled

a guest
Mar 28th, 2017
225
0
Never
Not a member of Pastebin yet? Sign Up, it unlocks many cool features!
text 3.52 KB | None | 0 0
  1. long count1,count2,error1;
  2. int Kp_1=6;
  3. int sensor1, sensor2, sensor3, sensor4, a, sensor5, sensor6,error,Kp,P,Ki,pwmR,pwmL,pwmB,correct,Kd,D,pre_error,last_error;
  4. unsigned long timex;
  5. void setup() {
  6. Serial.begin(9600);
  7. pinMode(2, INPUT); //Right Motor Encoder
  8. pinMode(3, INPUT); //Left Motor Encoder
  9. pinMode(11, OUTPUT); //Pin C2 Driving Motor Output 2 Right
  10. pinMode(6, OUTPUT); //Pin C1 Driving Motor Output 1 Left
  11. pinMode(8, OUTPUT); //Pin C5 Driving Motor Output 2 Right
  12. pinMode(7, OUTPUT); //Pin C4 Driving Motor Output 2 Right
  13. pinMode(10, OUTPUT); //Pin A4 Driving Motor Output 1 Left
  14. pinMode(12, OUTPUT); //Pin A5 Driving Motor Output 1 Left
  15. pinMode(14, INPUT); //QTR Sensor Number 2 (-3)
  16. pinMode(15, INPUT); //QTR Sensor Number 3 (-2)
  17. pinMode(16, INPUT); //QTR Sensor Number 4 (-1)
  18. pinMode(17, INPUT); //QTR Sensor Number 5 (+1)
  19. pinMode(18, INPUT); //QTR Sensor Number 6 (+2)
  20. pinMode(19, INPUT); //QTR Sensor Number 7 (+3)
  21. //attachInterrupt(digitalPinToInterrupt(2),counting1, RISING);
  22. //attachInterrupt(digitalPinToInterrupt(3),counting2, RISING);
  23. delay(2500);
  24. starttime = millis()
  25. }
  26.  
  27. void sag_geri(int pwmR){
  28.  
  29. digitalWrite(8, LOW); //SAG
  30. digitalWrite(7, HIGH); //SAG
  31. analogWrite(6, pwmR);
  32. }
  33. void sol_geri(int pwmL){
  34. digitalWrite(10, HIGH); //SOL
  35. digitalWrite(12, LOW); //SOL
  36. analogWrite(11, pwmL);
  37. }
  38.  
  39. void sag_ileri(int pwmR){
  40. digitalWrite(8, HIGH);
  41. digitalWrite(7, LOW);
  42. analogWrite(6, pwmR);
  43.  
  44. }
  45. void sol_ileri(int pwmL){
  46. digitalWrite(12, HIGH);
  47. digitalWrite(10, LOW);
  48. analogWrite(11, pwmL);
  49. }
  50.  
  51. void loop() {
  52.  
  53.  
  54. sensor1=analogRead(14);
  55. sensor2=analogRead(15);
  56. sensor3=analogRead(16);
  57. sensor4=analogRead(17);
  58. sensor5=analogRead(18);
  59. sensor6=analogRead(19);
  60.  
  61.  
  62.  
  63.  
  64. if(sensor1<=512)
  65. {
  66. sensor1=1;
  67. }
  68. else
  69. {
  70. sensor1=0;
  71. }
  72.  
  73. if(sensor2<=512)
  74. {
  75. sensor2=1;
  76. }
  77. else
  78. {
  79. sensor2=0;
  80. }
  81. if(sensor3<=512)
  82. {
  83. sensor3=1;
  84. }
  85. else
  86. {
  87. sensor3=0;
  88. }
  89. if(sensor4<=512)
  90. {
  91. sensor4=1;
  92. }
  93. else
  94. {
  95. sensor4=0;
  96. }
  97. if(sensor5<=512)
  98. {
  99. sensor5=1;
  100. }
  101. else
  102. {
  103. sensor5=0;
  104. }
  105. if(sensor6<=512)
  106. {
  107. sensor6=1;
  108. }
  109. else
  110. {
  111. sensor6=0;
  112. }
  113. /*
  114. Serial.print(sensor1 );
  115. Serial.print(sensor2 );
  116. Serial.print(sensor3 );
  117. Serial.print(sensor4 );
  118. Serial.print(sensor5 );
  119. Serial.println(sensor6 );*/
  120. sensor1=(-8)*sensor1;
  121. sensor2=(-6)*sensor2;
  122. sensor3=(-1)*sensor3;
  123. sensor4=(+1)*sensor4;
  124. sensor5=(+6)*sensor5;
  125. sensor6=(+8)*sensor6;
  126.  
  127. error=sensor1+sensor2+sensor3+sensor4+sensor5+sensor6; //Error of robot from line
  128. Kp=7; //Proportional Constant (found by trial and error)
  129. Kd=15;
  130. P=error*Kp; // asıl hız
  131. D=(error-pre_error)*Kd;
  132. correct=P+D;
  133.  
  134. timex=millis();
  135. Serial.println(timex);
  136.  
  137.  
  138.  
  139. pwmL=50-correct;
  140. pwmR=50+correct;
  141.  
  142.  
  143. if (pwmL > 255)
  144. {
  145. pwmL=255;
  146. }
  147. else if (pwmL<0)
  148. {
  149. pwmL=0;
  150. }
  151. else
  152. {
  153. pwmL=pwmL;
  154. }
  155. if (pwmR > 255)
  156. {
  157. pwmR=255;
  158. }
  159. else if (pwmR<0)
  160. {
  161. pwmR=0;
  162. }
  163. else
  164. {
  165. pwmR=pwmR;
  166. }
  167. starttime = millis();
  168. endtime = starttime;
  169. While ((endtime - starttime) <=1000) // do this loop for up to 1000mS
  170. {
  171. sag_ileri(50);
  172. loopcount = loopcount+1;
  173. endtime = millis();
  174. }
  175. serial.print (loopcount,DEC);
  176.  
  177. /*
  178. if (sensor1==0 && sensor2==0 && sensor3==0 && sensor4==0 && sensor5==0 && sensor6==0)
  179. {
  180. pwmL=100;
  181. pwmR=100;
  182. sol_geri(pwmL);
  183. sag_geri(pwmR);
  184.  
  185.  
  186. }
  187. else if(error>=8){
  188. sag_geri(50);
  189. sol_ileri(150);
  190.  
  191. }
  192. else if(error<=-8){
  193. sag_ileri(150);
  194. sol_geri(50);
  195.  
  196. }
  197.  
  198. else
  199. {
  200. sag_ileri(pwmR);
  201. sol_ileri(pwmL);
  202.  
  203. }
  204. pre_error=error;*/
  205. }
Advertisement
Add Comment
Please, Sign In to add comment