API tools faq
paste
Advertisement
Guest User

Untitled

a guest
Sep 20th, 2019
152
0
Never
Add comment
Not a member of Pastebin yet? Sign Up, it unlocks many cool features!
text 7.92 KB | None | 0 0
raw download clone embed print report
  1. #include <Wire.h> //Include the Wire Library
  2. #include <HTInfraredSeeker.h>
  3. #define leftUs 21 //port left us
  4. #define rightUs 23 //port right us
  5. #define backUs 22 //port back us
  6. #define delayUs 50 //delay betweem
  7. #define pwmA 10
  8. #define digital1A 12
  9. #define digital2A 11
  10. #define pwmB 4
  11. #define digital1B 5
  12. #define digital2B 6
  13. #define pwmC 9
  14. #define digital1C 7
  15. #define digital2C 8
  16. #define l1p A8 //
  17. #define l2p A7
  18. #define l3p A2
  19. #define l4p A1
  20. #define l5p A0
  21. #define l6p A6
  22. #define l7p A5
  23. #define l8p A3
  24. #define lap A4
  25.  
  26. class Tech
  27. {
  28. public:
  29. int l1,l2,l3,l4,l5,l6,l7,l8,la,l1k,l2k,l3k,l4k,l5k,l6k,l7k,l8k,lak;
  30. bool l1b,l2b,l3b,l4b,l5b,l6b,l7b,l8b;
  31. void motor(char a,int b);
  32. Tech(bool debug, bool bluetooth);
  33. int distL=-1,distR=-1,distB=-1,rezet=0;
  34. int OdistL=-1,OdistR=-1,OdistB=-1;
  35. int timeUs,azimut,degree=-1;
  36. unsigned long int timer;
  37. int numUs=0,Dir,li=0;
  38. int US(char port);
  39. int Distance();
  40. bool debug;
  41. bool bluetooth;
  42. void gyro();
  43. bool gete = 0;
  44. String beta;
  45. char c = ' ';
  46. void first(bool debu, bool bluetoot);
  47. void move(float a, float power,float error);
  48. int UP();
  49. void IRlego();
  50. void line();
  51.  
  52. };
  53. Tech::Tech(bool debu, bool bluetoot){
  54. Serial1.begin(115200);
  55. Serial1.println("\n");
  56. Serial.begin(115200);
  57. debug=debu;
  58. bluetooth=bluetoot;
  59. if(debug){
  60. Serial.begin(115200);
  61. }
  62. if(bluetoot){
  63. Serial2.begin(9600);
  64. }
  65. timer = millis();
  66. while (azimut == -1) {
  67. Tech::gyro();
  68. }
  69. azimut = degree;
  70.  
  71. }
  72. void Tech::IRlego(){
  73. InfraredResult InfraredBall = InfraredSeeker::ReadAC();
  74. Dir = InfraredBall.Direction;
  75.  
  76. }
  77. int Tech::UP(){
  78. return (degree+540-azimut)%360-180;
  79. }
  80. void Tech::move(float a, float power,float error){
  81. a = (a) / 57.3;
  82. float PB = -(-cos(a) * power)+error;
  83. float PC = -(0.5 * cos(a) * power + 0.87 * sin(a) * power)+error;
  84. float PA = (0.5 * cos(a) * power - 0.87 * sin(a) * power)-error;
  85.  
  86. if(a <= 0) {
  87. motor('A', PA);
  88. motor('B', PB);
  89. motor('C', PC);
  90. }
  91. else {
  92. motor('C', PC);
  93. motor('B', PB);
  94. motor('A', PA);
  95. }
  96. }
  97. void Tech::motor(char a,int b){
  98. if(b>255){
  99. b=255;
  100. }
  101. if(b<-255){
  102. b=-255;
  103. }
  104. if(a=='A'){
  105. if(b<0){
  106. digitalWrite(digital1A,0);
  107. digitalWrite(digital2A,1);
  108. analogWrite(pwmA,b*-1);
  109. }
  110. if(b>0){
  111. digitalWrite(digital1A,1);
  112. digitalWrite(digital2A,0);
  113. analogWrite(pwmA,b);
  114. }
  115. }
  116. if(a=='B'){
  117. if(b<0){
  118. digitalWrite(digital1B,0);
  119. digitalWrite(digital2B,1);
  120. analogWrite(pwmB,-b);
  121. }
  122. if(b>0){
  123. digitalWrite(digital1B,1);
  124. digitalWrite(digital2B,0);
  125. analogWrite(pwmB,b);
  126. }
  127. }
  128. if(a=='C'){
  129. if(b<0){
  130. digitalWrite(digital1C,0);
  131. digitalWrite(digital2C,1);
  132. analogWrite(pwmC,-b);
  133. }
  134. if(b>0){
  135. digitalWrite(digital1C,1);
  136. digitalWrite(digital2C,0);
  137. analogWrite(pwmC,b);
  138. }
  139. }
  140. }
  141. void Tech::first(bool debu, bool bluetoot){
  142.  
  143. InfraredSeeker::Initialize();
  144. Serial1.begin(115200);
  145. Serial1.println("\n");
  146. debug=debu;
  147. bluetooth=bluetoot;
  148. if(debug){
  149. Serial.begin(115200);
  150. }
  151. if(bluetoot){
  152. Serial2.begin(9600);
  153. }
  154. timer = millis();
  155. while (degree == -1) {
  156. Tech::gyro();
  157. }
  158. azimut = degree;
  159. pinMode(pwmA,OUTPUT);
  160. pinMode(pwmB,OUTPUT);
  161. pinMode(pwmC,OUTPUT);
  162. pinMode(digital1A,OUTPUT);
  163. pinMode(digital1B,OUTPUT);
  164. pinMode(digital1C,OUTPUT);
  165. pinMode(digital2A,OUTPUT);
  166. pinMode(digital2B,OUTPUT);
  167. pinMode(digital2C,OUTPUT);
  168. pinMode(l1p,INPUT);
  169. pinMode(l2p,INPUT);
  170. pinMode(l3p,INPUT);
  171. pinMode(l4p,INPUT);
  172. pinMode(l5p,INPUT);
  173. pinMode(l6p,INPUT);
  174. pinMode(l7p,INPUT);
  175. pinMode(l8p,INPUT);
  176. pinMode(lap,INPUT);
  177. l1k=analogRead(l1p);
  178. l2k=analogRead(l2p);
  179. l3k=analogRead(l3p);
  180. l4k=analogRead(l4p);
  181. l5k=analogRead(l5p);
  182. l6k=analogRead(l6p);
  183. l7k=analogRead(l7p);
  184. l8k=analogRead(l8p);
  185. lak=analogRead(lap);
  186. }
  187. void Tech::line(){
  188. l1=analogRead(l1p)-l1k;
  189. l2=analogRead(l2p)-l2k;
  190. l3=analogRead(l3p)-l3k;
  191. l4=analogRead(l4p)-l4k;
  192. l5=analogRead(l5p)-l5k;
  193. l6=analogRead(l6p)-l6k;
  194. l7=analogRead(l7p)-l7k;
  195. l8=analogRead(l8p)-l8k;
  196. if(l1>15)
  197. l1b=1;
  198. else
  199. l1b=0;
  200. if(l2>30)
  201. l2b=1;
  202. else
  203. l2b=0;
  204. if(l3>30)
  205. l3b=1;
  206. else
  207. l3b=0;
  208. if(l4>30)
  209. l4b=1;
  210. else
  211. l4b=0;
  212. if(l5>30)
  213. l5b=1;
  214. else
  215. l5b=0;
  216. if(l6>30)
  217. l6b=1;
  218. else
  219. l6b=0;
  220. if(l7>30)
  221. l7b=1;
  222. else
  223. l7b=0;
  224. if(l8>30)
  225. l8b=1;
  226. else
  227. l8b=0;
  228.  
  229. li++;
  230. if(debug&&li==100){
  231. Serial.print(" l1: ");
  232. Serial.print(l1b);
  233. Serial.print(" l2: ");
  234. Serial.print(l2b);
  235. Serial.print(" l3: ");
  236. Serial.print(l3b);
  237. Serial.print(" l4: ");
  238. Serial.print(l4b);
  239. Serial.print(" l5: ");
  240. Serial.print(l5b);
  241. Serial.print(" l6: ");
  242. Serial.print(l6b);
  243. Serial.print(" l7: ");
  244. Serial.print(l7b);
  245. Serial.print(" l8: ");
  246. Serial.println(l8b);
  247. li=0;
  248. }
  249. }
  250.  
  251. void Tech::gyro(){
  252. if(Serial1.available()) {
  253. // Serial.print("p");
  254.  
  255. // get the new byte:
  256. c = (char)Serial1.read();
  257. // add it to the inputString:
  258. beta += c;
  259. // if the incoming character is a newline, set a flag so the main loop can
  260. // do something about it:
  261. if (c == '\n') {
  262. Serial1.print("\n");
  263. if(debug)
  264.  
  265. Serial.println(degree);
  266. degree=beta.toInt();
  267. beta="";
  268. }
  269. }
  270. }
  271. int Tech::Distance(){
  272. if(debug){
  273. //Serial.print(" distL: ");
  274. //Serial.print(distL);
  275. //Serial.print(" distB: ");
  276. //Serial.print(distB);
  277. //Serial.print(" distR: ");
  278. //Serial.println(distR);
  279. }
  280. if(bluetooth){
  281. Serial2.print(" distL: ");
  282. Serial2.print(distL);
  283. Serial2.print(" distB: ");
  284. Serial2.print(distB);
  285. Serial2.print(" distR: ");
  286. Serial2.println(distR);
  287. }
  288. if(millis()-timeUs>delayUs){
  289. timeUs=millis();
  290. if(numUs==0){
  291. numUs=1;
  292. distL=Tech::US('L');
  293. if(distL==0){
  294. distL=OdistL;
  295. }
  296. else{
  297. OdistL=distL;
  298. }
  299. }
  300. else if(numUs==1){
  301. numUs=2;
  302. distB=Tech::US('B');
  303. if(distL==0){
  304. distB=OdistB;
  305. }
  306. else{
  307. OdistB=distB;
  308. }
  309. }
  310. else if(numUs==2){
  311. numUs=0;
  312. distR=Tech::US('R');
  313. if(distL==0){
  314. distR=OdistR;
  315. }
  316. else{
  317. OdistR=distR;
  318. }
  319. }
  320. }
  321. return 0;
  322. }
  323.  
  324. int Tech::US(char port){
  325. if(port=='L'){
  326. unsigned long duration, distance;
  327. pinMode(leftUs,OUTPUT);
  328. digitalWrite(leftUs, LOW);
  329. delayMicroseconds(2);
  330. digitalWrite(leftUs, HIGH);
  331. delayMicroseconds(10);
  332. digitalWrite(leftUs, LOW);
  333. pinMode(leftUs,INPUT);
  334. duration = pulseIn(leftUs, HIGH, 30000);
  335. distance = duration / 58;
  336. return distance;
  337. }
  338. if(port=='R'){
  339. unsigned long duration, distance;
  340. pinMode(rightUs,OUTPUT);
  341. digitalWrite(rightUs, LOW);
  342. delayMicroseconds(2);
  343. digitalWrite(rightUs, HIGH);
  344. delayMicroseconds(10);
  345. digitalWrite(rightUs, LOW);
  346. pinMode(rightUs,INPUT);
  347. duration = pulseIn(rightUs, HIGH, 30000);
  348. distance = duration / 58;
  349. return distance;
  350. }
  351.  
  352. if(port=='B'){
  353. unsigned long duration, distance;
  354. pinMode(backUs,OUTPUT);
  355. digitalWrite(backUs, LOW);
  356. delayMicroseconds(2);
  357. digitalWrite(backUs, HIGH);
  358. delayMicroseconds(10);
  359. digitalWrite(backUs, LOW);
  360. pinMode(backUs,INPUT);
  361. duration = pulseIn(backUs, HIGH, 30000);
  362. distance = duration / 58;
  363. return distance;
  364. }
  365.  
  366. }
  367. class PID{
  368. public:
  369. float ki=0,kp=0,kd=0,imax=0;
  370. long error,errorOld,errorIntegral,errorDeviante;
  371. unsigned long dt,dn;
  372. void first(float kpc,float kic,float kdc,float imaxc);
  373. double UI(int error);
  374. };
  375. void PID::first(float kpc,float kic,float kdc,float imaxc){
  376. ki=kic;
  377. kp=kpc;
  378. kd=kdc;
  379. imax=imaxc;
  380. }
  381. double PID::UI(int error){
  382. dn=millis();
  383. errorIntegral+=error*(dn-dt);
  384. if(errorIntegral>imax){
  385. errorIntegral=imax;
  386. }
  387. if(errorIntegral<-imax){
  388. errorIntegral=-imax;
  389. }
  390. errorDeviante=(error-errorOld)/(dn-dt);
  391. errorOld=error;
  392. dt=millis();
  393. return error*kp +errorIntegral*ki+errorDeviante*kd;
  394. }
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!