API tools faq
paste
Advertisement
Guest User

Untitled

a guest
Jan 29th, 2020
88
0
Never
Add comment
Not a member of Pastebin yet? Sign Up, it unlocks many cool features!
text 12.72 KB | None | 0 0
raw download clone embed print report
  1. #include <Arduino.h>
  2. //#include <BluetoothSerial.h>
  3.  
  4. #define clamp(value, min, max) (value < min ? min : value > max ? max : value)
  5. #define roomba Serial1
  6. #define debug Serial2
  7.  
  8.  
  9. int pinout[] = {11,10};
  10.  
  11. // PID
  12. int target_distance = 150;
  13. bool driveEnable = true;
  14. float prev_dis = 0;
  15.  
  16. int pid_p = 3;
  17. int pid_d = -30;
  18.  
  19. float pid_i = 0;
  20. float pid_s = 250;
  21. float integral = 0;
  22. float reg = 0;
  23.  
  24. float lastValues[3] = {-1, -1, -1};
  25.  
  26. float readDistance(int i)
  27. {
  28. int echoPin = pinout[2 * i];
  29. int trigPin = pinout[2 * i + 1];
  30.  
  31. pinMode(trigPin, OUTPUT);
  32. pinMode(echoPin, INPUT);
  33.  
  34. float duration, cm;
  35. digitalWrite(trigPin, LOW);
  36. delayMicroseconds(2);
  37. digitalWrite(trigPin, HIGH);
  38. delayMicroseconds(10);
  39. digitalWrite(trigPin, LOW);
  40. duration = pulseIn(echoPin, HIGH);
  41. cm = duration / 58.0f;
  42.  
  43. // 3 -> 3
  44. // 4 -> 4
  45.  
  46. if (lastValues[i] == -1)
  47. {
  48. lastValues[i] = cm;
  49. }
  50.  
  51. if (abs(cm - lastValues[i]) > 2000)
  52. {
  53. cm = lastValues[i];
  54. }
  55.  
  56. lastValues[i] = cm;
  57.  
  58. //delay(100);
  59.  
  60. return cm;
  61. }
  62.  
  63. int ddPin = 5; //device detect
  64.  
  65. void wakeUp()
  66. { // импульс на device detect
  67. pinMode(ddPin, OUTPUT);
  68. digitalWrite(ddPin, HIGH);
  69. delay(100);
  70. digitalWrite(ddPin, LOW);
  71. delay(500);
  72. digitalWrite(ddPin, HIGH);
  73. delay(2000);
  74. }
  75.  
  76. bool debrisLED;
  77. bool spotLED;
  78. bool dockLED;
  79. bool warningLED;
  80. byte color;
  81. byte intensity;
  82.  
  83. void setDirtLED(bool enable)
  84. {
  85. debrisLED = enable;
  86. spotLED = enable;
  87. dockLED = enable;
  88. warningLED = enable;
  89. roomba.write(139);
  90. roomba.write((debrisLED ? 1 : 0) + (spotLED ? 2 : 0) + (dockLED ? 4 : 0) + (warningLED ? 8 : 0));
  91. roomba.write((byte)color);
  92. roomba.write((byte)intensity);
  93. }
  94.  
  95. void startFull()
  96. {
  97. roomba.write(128);
  98. roomba.write(132);
  99. delay(1000);
  100. }
  101.  
  102. void driveDirect(int right, int left)
  103. {
  104. clamp(right, -500, 500);
  105. clamp(left, -500, 500);
  106.  
  107. roomba.write(145);
  108. roomba.write(right >> 8);
  109. roomba.write(right);
  110. roomba.write(left >> 8);
  111. roomba.write(left);
  112. }
  113.  
  114. #define C3 48
  115. #define C4 60
  116. #define Em4 51
  117. #define Em5 63
  118. #define Am3 44
  119. #define Am4 56
  120. #define G3 43
  121. #define G4 55
  122.  
  123. void startStream(const uint8_t *arr, uint8_t len)
  124. {
  125. roomba.write(148);
  126. roomba.write(len);
  127.  
  128. for (byte i = 0; i < len; i++)
  129. {
  130. roomba.write(arr[i]);
  131. }
  132. }
  133.  
  134. // void jingleBells()
  135. // {
  136. // roomba.write(140);
  137. // roomba.write(0);
  138. // roomba.write(16);
  139.  
  140. // byte melody[] = {C3, C3, C4, C4, Em4, Em5, C3, C4, Am3, Am3, Am4, C4, G3, G3, G4, G4};
  141. // //byte len[] = {16, 16, 16, 16, 16, 16, 16, 16, }
  142.  
  143. // for (int i = 0; i < 16; i++)
  144. // {
  145. // roomba.write(melody[i] + 12);
  146. // roomba.write(8);
  147. // }
  148.  
  149. // // roomba.write(141);
  150. // // // roomba.write(0);
  151.  
  152. // // for (int i = 0; i < 8; i++)
  153. // // {
  154. // // setDirtLED(true);
  155. // // delay(250);
  156. // // setDirtLED(false);
  157. // // delay(250);
  158. // // }
  159. // }
  160.  
  161. // float data[7] = {0, 0, 0, 0, 0, 0, 0};
  162. // float filtDis = 0;
  163. // int numStep = 0;
  164. int IRpin = A0;
  165.  
  166. float irDistance()
  167. {
  168. float volts = analogRead(IRpin) * 0.0048828125f;
  169.  
  170. if (volts < 0.15f) {
  171. pid_p = 6;
  172. pid_d = -25;
  173. return 250;
  174. }
  175.  
  176. float distance = 65 * pow(volts, -1.10);
  177.  
  178. if (distance > 250) {
  179. pid_p = 6;
  180. pid_d = -25;
  181. return 250;
  182. }
  183.  
  184. pid_p = 4;
  185. pid_d = -30;
  186.  
  187. return distance;
  188.  
  189. // data[numStep] = distance;
  190. // if (numStep == 6)
  191. // {
  192. // for (int i = 0; i < 5; i++)
  193. // {
  194. // data[i] = (data[i] + data[i + 1] + data[i + 2]) / 3;
  195. // }
  196. // for (int i = 0; i < 3; i++)
  197. // {
  198. // data[i] = (data[i] + data[i + 1] + data[i + 2]) / 3;
  199. // }
  200. // filtDis = (data[0] + data[1] + data[2]) / 3;
  201. // numStep = 0;
  202. // }
  203. // else
  204. // {
  205. // numStep++;
  206. // }
  207.  
  208. // return filtDis;
  209. }
  210.  
  211. int lastTime = 0;
  212.  
  213. int state = 0;
  214. byte waitSize = 0;
  215. byte checksum = 0;
  216. byte packetId = 0;
  217. int newEncoderLeft = 0;
  218. int newEncoderRight = 0;
  219. int encoderLeft = 0;
  220. int encoderRight = 0;
  221.  
  222. #define defaultValue 0xFE
  223. int defaultLeftEnc = defaultValue;
  224. int defaultRightEnc = defaultValue;
  225.  
  226. #define STOP -1
  227. #define GO_FORWARD 0
  228. #define GOING_FORWARD 1
  229. #define ROTATE_LEFT 2
  230. #define ROTATING_LEFT 3
  231. #define ROTATE_RIGHT 4
  232. #define ROTATING_RIGHT 5
  233.  
  234. #define GO_ONE_METER_FORWARD 6
  235.  
  236.  
  237. bool hasDetectedWall = false;
  238. float ir_distance = 0;
  239. int iRsensorTime = 0;
  240. int ledTime = 0;
  241. bool ledState = true;
  242.  
  243. void setupIrMinimum() {
  244. for (int i = 0; i < 7; i++) {
  245. target_distance = irDistance();
  246. delay(10);
  247. }
  248. }
  249.  
  250. void setup()
  251. {
  252. // BT.begin("Roomba");
  253.  
  254. Serial.begin(9600);
  255. roomba.begin(115200);
  256.  
  257. wakeUp();
  258. startFull();
  259.  
  260. driveDirect(0, 0);
  261.  
  262. //setupIrMinimum();
  263.  
  264. delay(2000);
  265.  
  266. for (int i = 0; i < 5; i++) {
  267. target_distance = readDistance(0);/*clamp(readDistance(0), 0, 150);*/
  268. }
  269.  
  270. uint8_t arr[] = {13, 43, 44 /*,18,43,44*/};
  271. startStream(arr, 3);
  272.  
  273. state = GO_FORWARD;
  274. //state = STOP;
  275. }
  276.  
  277. void waitForInput()
  278. {
  279. while (!roomba.available()) {
  280. yield();
  281. }
  282. }
  283. char readByte()
  284. {
  285. waitForInput();
  286. int c = roomba.read();
  287. return c;
  288. }
  289.  
  290. int isStop = 0;
  291. int isReset = 0;
  292.  
  293. byte handlePacket(byte id)
  294. {
  295. if (id == 7)
  296. {
  297. byte r = readByte();
  298. if (r & 1 || r & 0)
  299. {
  300. state = STOP;
  301. }
  302. return r;
  303. }
  304. if (id == 13)
  305. {
  306. byte r = readByte();
  307.  
  308. hasDetectedWall = r == 1;
  309.  
  310. waitSize -= 1;
  311. return r;
  312. }
  313. if (id == 18)
  314. {
  315. byte r = readByte();
  316. waitSize -= 1;
  317. // if (r & 1)
  318. // isStop = 1;
  319. // if (r & 4)
  320. // isReset = 1;
  321. return r;
  322. }
  323. if (id == 43)
  324. {
  325. byte lbmm = readByte();
  326. byte rbmm = readByte();
  327. newEncoderLeft = lbmm << 8 | rbmm;
  328. waitSize -= 2;
  329. return lbmm + rbmm;
  330. }
  331. if (id == 44)
  332. {
  333. byte lbmm = readByte();
  334. byte rbmm = readByte();
  335. newEncoderRight = lbmm << 8 | rbmm;
  336. waitSize -= 2;
  337. return lbmm + rbmm;
  338. }
  339.  
  340. return 0;
  341. }
  342.  
  343. int lastLeftEnc = 0, lastRightEnc = 0;
  344.  
  345. void driveNearWall()
  346. {
  347.  
  348. float dis = (readDistance(0) - target_distance) * 0.01f;
  349. reg = dis * pid_p - (dis - prev_dis) * pid_d - integral * pid_i;
  350.  
  351. integral += dis;
  352.  
  353. if (driveEnable)
  354. {
  355. float right = pid_s * (1 - 0.95f*clamp(reg, -1, 1));
  356. float left = pid_s * (1 + 0.95f*clamp(reg, -1, 1));
  357.  
  358. driveDirect(right, left);
  359. }
  360. else
  361. {
  362. //driveDirect(0, 0);
  363. }
  364.  
  365. prev_dis = dis;
  366. }
  367.  
  368. int driveTime = 500;
  369. int STATE_AFTER_ROTATED = STOP;
  370. void handleSensors()
  371. {
  372. if (roomba.available())
  373. {
  374. if (readByte() == 19)
  375. {
  376. waitSize = readByte();
  377. byte sum = 19 + waitSize;
  378.  
  379. while (waitSize > 0)
  380. {
  381. packetId = readByte();
  382. sum += packetId;
  383. waitSize--;
  384. sum += handlePacket(packetId);
  385. }
  386.  
  387. byte last = readByte(); // TODO CHECKSUM
  388.  
  389. if ((sum + last) % 256 == 0)
  390. {
  391. if (defaultLeftEnc == defaultValue && newEncoderLeft > 0)
  392. {
  393. defaultLeftEnc = newEncoderLeft;
  394. }
  395. if (defaultRightEnc == defaultValue && newEncoderRight > 0)
  396. {
  397. defaultRightEnc = newEncoderRight;
  398. }
  399.  
  400. encoderLeft = newEncoderLeft - defaultLeftEnc;
  401. encoderRight = newEncoderRight - defaultRightEnc;
  402.  
  403. if (state == GO_FORWARD)
  404. {
  405. driveDirect(100, 100);
  406.  
  407. hasDetectedWall = false; // ИНОГДА ЛАГАЕТ!
  408.  
  409. state = GOING_FORWARD;
  410. }
  411. else if (state == GOING_FORWARD)
  412. {
  413. if (millis() - driveTime > 100)
  414. {
  415. driveTime = millis();
  416. driveNearWall();
  417. }
  418.  
  419. // Едем... увидели стенку
  420. if (hasDetectedWall)
  421. {
  422. driveDirect(0, 0);
  423. STATE_AFTER_ROTATED = GO_ONE_METER_FORWARD;
  424. Serial.println("Detected wall!");
  425. state = ROTATE_LEFT;
  426. setDirtLED(true);
  427. }
  428. }
  429. else if (state == ROTATE_LEFT)
  430. {
  431. defaultLeftEnc = newEncoderLeft;
  432. defaultRightEnc = newEncoderRight;
  433.  
  434. state = ROTATING_LEFT;
  435. }
  436. else if (state == ROTATING_LEFT)
  437. {
  438. int dl = encoderLeft - lastLeftEnc;
  439. int dr = encoderRight - lastRightEnc;
  440. int deltaRot = abs(dl + dr);
  441.  
  442. int speedLeft = 90;
  443. int speedRight = 90;
  444.  
  445. if (abs(dl) > abs(dr))
  446. {
  447. speedLeft -= 10 * deltaRot;
  448. speedRight += 10 * deltaRot;
  449. }
  450. else
  451. {
  452. speedLeft += 10 * deltaRot;
  453. speedRight -= 10 * deltaRot;
  454. }
  455.  
  456. driveDirect(speedRight, -speedLeft);
  457.  
  458. if (abs(encoderLeft - encoderRight) * 0.444564998f >= 1460 / 4)
  459. {
  460. driveDirect(0, 0);
  461. state = STATE_AFTER_ROTATED;
  462.  
  463. defaultLeftEnc = newEncoderLeft;
  464. defaultRightEnc = newEncoderRight;
  465. }
  466. }
  467. else if (state == STOP)
  468. {
  469. driveDirect(0, 0);
  470. }
  471. else if (state == GO_ONE_METER_FORWARD)
  472. {
  473. float delta = abs(encoderLeft - encoderRight);
  474.  
  475. if (encoderLeft > encoderRight)
  476. {
  477. driveDirect(100 + (int)(delta * 1.2f), 100 - (int)(delta * 1.2f));
  478. }
  479. else
  480. {
  481. driveDirect(100 - (int)(delta * 1.2f), 100 + (int)(delta * 1.2f));
  482. }
  483.  
  484. float distance = (encoderLeft + encoderRight) / 2.0f * 0.444564998f;
  485.  
  486. if (distance > 1000)
  487. {
  488. driveDirect(0,0);
  489. STATE_AFTER_ROTATED = GOING_FORWARD;
  490. state = ROTATE_LEFT;
  491. }
  492. }
  493.  
  494. // if (isStop) {
  495. // state = STOP;
  496. // driveDirect(0, 0);
  497. // isStop = false;
  498. // }
  499.  
  500. // float distance = (encoderLeft+encoderRight)/2.0f * 0.444564998f;
  501.  
  502. //Serial.print("Left: ");
  503. //Serial.print(encoderLeft);
  504. // Serial.print("Right: ");
  505. // Serial.println(encoderRight);
  506. // Serial.print("State: ");
  507. // Serial.println(state);
  508. // Serial.println();
  509.  
  510. // if (state == GO_FORWARD) {
  511. // driveDirect(300, 300);
  512. // state = GOING_FORWARD;
  513. // } else if (state == GOING_FORWARD) {
  514. // float delta = abs(encoderLeft - encoderRight);
  515.  
  516. // if (encoderLeft > encoderRight) {
  517. // driveDirect(300 + (int)(delta * 1.5f), 300 - (int)(delta *1.5f));
  518. // } else {
  519. // driveDirect(300 - (int)(delta * 1.5f), 300 + (int)(delta *1.5f));
  520. // }
  521.  
  522. // if (abs(distance - nextDistance) < 5) {
  523. // state = ROTATE_RIGHT;
  524. // }
  525. // }
  526. // else if (state == ROTATE_RIGHT) {
  527. // driveDirect(-300, 300);
  528.  
  529. // defaultLeftEnc = newEncoderLeft;
  530. // defaultRightEnc = newEncoderRight;
  531.  
  532. // state = ROTATING_RIGHT;
  533. // } else if (state == ROTATING_RIGHT) {
  534. // int dl = encoderLeft - lastLeftEnc;
  535. // int dr = encoderRight - lastRightEnc;
  536. // int deltaRot = abs(dl + dr);
  537.  
  538. // int speedLeft = 300;
  539. // int speedRight = 300;
  540.  
  541. // if (abs(dl) > abs(dr)) {
  542. // speedLeft -= 25 * deltaRot;
  543. // speedRight += 25 * deltaRot;
  544. // } else {
  545. // speedLeft += 25 * deltaRot;
  546. // speedRight -= 25 * deltaRot;
  547. // }
  548.  
  549. // driveDirect(-speedRight, speedLeft);
  550.  
  551. // if (abs(encoderLeft - encoderRight) * 0.444564998f >= (1632-1000)/4) {
  552. // speedLeft /= 2;
  553. // }
  554.  
  555. // if (abs(encoderLeft - encoderRight) * 0.444564998f >= 1460/4) {
  556. // state = GO_FORWARD;
  557. // defaultLeftEnc = newEncoderLeft;
  558. // defaultRightEnc = newEncoderRight;
  559. // if (nextDistance == 500) {
  560. // nextDistance = 800;
  561. // } else {
  562. // nextDistance = 500;
  563. // }
  564. // }
  565. // } else if (state == STOP) {
  566. // setDirtLED(true);
  567. // delay(250);
  568. // setDirtLED(false);
  569. // delay(250);
  570. // driveDirect(0, 0);
  571. // }
  572.  
  573. lastLeftEnc = encoderLeft;
  574. lastRightEnc = encoderRight;
  575. }
  576. else
  577. {
  578. //Serial.println("bad hashsum");
  579. hasDetectedWall = false;
  580. }
  581.  
  582. packetId = 0;
  583. waitSize = 0;
  584. }
  585. }
  586. }
  587.  
  588. void loop()
  589. {
  590. int mils = millis();
  591. // if (mils - iRsensorTime > 10)
  592. // {
  593. // iRsensorTime = mils;
  594. // ir_distance = irDistance();
  595. // }
  596.  
  597. if (mils - ledTime > 500) {
  598. setDirtLED(ledState);
  599. ledState = !ledState;
  600. ledTime = mils;
  601. }
  602.  
  603. handleSensors();
  604. }
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!