API tools faq
paste
Advertisement
Guest User

Untitled

a guest
Apr 18th, 2017
70
0
Never
Add comment
Not a member of Pastebin yet? Sign Up, it unlocks many cool features!
text 20.89 KB | None | 0 0
raw download clone embed print report
  1. //define which car using
  2. //make sure before including LMotor.h
  3. #define kelvin
  4.  
  5. #include <p32mx250f128d.h>
  6. #include <string.h>
  7. #include "PPacket.h"
  8. #include "APacket.h"
  9. #include "RingBuffer.h"
  10. #include "ReadBuffer.h"
  11. #include "PID.h"
  12. #include "Counter.h"
  13. #include "RMotor.h"
  14. #include "LMotor.h"
  15. #include "Timer.h"
  16. #include "CommonFunc.h"
  17. #include "LED.h"
  18.  
  19. //AT Command
  20. #define AT "AT\r\n"
  21. #define CWMODE1 "AT+CWMODE=1\r\n"
  22. #define CWJAP "AT+CWJAP=\"CF005\",\"31053106\"\r\n"
  23. #define CIPMUX0 "AT\r\n"
  24. #define CIPSTART "AT+CIPSTART=\"UDP\",\"0\",0,3105,2\r\n"
  25.  
  26. //
  27.  
  28. //////////////////////////////// USB STUFF
  29.  
  30. extern "C" { // function provided by cdc stack
  31.  
  32. void USBDeviceInit(void);
  33. void USBDeviceTasks(void);
  34. void CDCDeviceTasks(void);
  35. void putUSBUSART(char *data, unsigned char length);
  36.  
  37. void USBTasks(void) {
  38. USBDeviceTasks();
  39. U1OTGIR = 0xFF;
  40. IFS1bits.USBIF = 0;
  41. CDCDeviceTasks();
  42. }
  43.  
  44. } //extern "C"
  45.  
  46. static RingBuffer tx;
  47. static ReadBuffer rx;
  48.  
  49. extern "C" // function required by cdc stack
  50. {
  51.  
  52. void CDCRxChars(char *c, int length) { // you got a message from cdc
  53. for (int i = 0; i < length; i++) {
  54. if (c[i] != '\n') { // skip linefeed
  55. tx.save(c[i]);
  56. if (c[i] == '\r') tx.save('\n');// add linefeed after carriage return
  57. }
  58. }
  59. IEC1bits.U2TXIE = 1;
  60. }
  61.  
  62. void CDCTxReadyNotify(void) { // you may send message to cdc
  63. static char buffer[64];
  64. unsigned char len = 0;
  65. while (!rx.empty()) buffer[len++] = rx.get();
  66. if (len) putUSBUSART(buffer, len);
  67. }
  68.  
  69. } //extern "C"
  70.  
  71. /////////////////////////////////
  72.  
  73. /////////////////////////////////
  74.  
  75. static PPacket P;
  76. static APacket A;
  77. static Counter left,right;
  78. static RMotor rm;
  79. static LMotor lm;
  80. static Timer timer;
  81. static LED led;
  82. static PID
  83. #ifdef clinton
  84. pid_speed(100,100,0,0),pid_track(35600,30,0,100);
  85. //#define PIDTargetSpeed 5
  86. volatile int PIDTargetSpeed = 5;
  87. #endif
  88. #ifdef harrison
  89. pid_speed(5000,100,0,0),pid_track(29600,50,0,100);
  90. #define PIDTargetSpeed 5
  91. #endif
  92. #ifdef kelvin
  93. pid_speed(6000,15,5,0),pid_track(3500,20,15,0),pid1_speed(6000,20,15,0),pid1_track(2500,30,15,0),shoot_speed(9000,20,0,0),shoot_track(5000,50,0,0);
  94. #define PIDTargetSpeed 5
  95. #define shootTargetSpeed 23 //?
  96. #define turnSpeed 20000
  97. #define turnFactor 0.37
  98. #endif
  99. #ifdef ctw
  100. pid_speed(8000,100,0,0),pid_track(29600,50,0,100),pid1_speed(6000,20,15,0),pid1_track(6000,30,15,0),shoot_speed(11000,0,0,0),shoot_track(6000,0,0,0);
  101. #define PIDTargetSpeed 5
  102. #define shootTargetSpeed 15
  103. #define turnFactor 0.3
  104. #define turnSpeed 15000
  105. #endif
  106. int offTrackD2Error(int i){
  107. bool neg = i < 0 ? true : false;
  108. i = i > 0 ? i :-i;
  109. //this need to be tune
  110. if(i <5) return 0;
  111. if(i < 12) return 1 * neg ? -1 : 1;
  112. if(i < 17) return 2 * neg ? -1 : 1;
  113. return 3 * neg ? -1 : 1;
  114. }
  115. //end tune
  116.  
  117.  
  118.  
  119. //ctw control code
  120. volatile int turnDegree = 0;
  121. volatile bool turnDirection = 0;
  122. volatile bool turnSingleFinished = 0;
  123. volatile bool turnFinishedSide = 0;//0 = left; 1 = right
  124.  
  125. volatile int state = 0; //for completing tasks
  126. volatile int mode = 0; //0 = stop;1 = move;2 = turn;
  127. //3 = get bearing(need add a delay to timer5 maybe 500ms?)
  128. // use pid with no error to move a bit;
  129. //4 = wait ball stop
  130. //5 = delay
  131. volatile int count = 0;
  132. volatile double lineM = 0; //Line: y = Mx + C
  133. volatile double lineC = 0;
  134. volatile bool lineDirection = 0; //0 = X to x; 1 = x to X
  135. volatile bool blind = false; //0 = normal; 1 = move backward
  136. volatile bool shoot = false; //0 = no; 1 = yes
  137. volatile int pidx = 0; //0 = normal 1 = slower one
  138.  
  139. volatile int destinationX = 0;
  140. volatile int destinationY = 0;
  141.  
  142. volatile int startX = 0; //for get bearing
  143. volatile int startY = 0;
  144.  
  145. volatile double bearingM = 0; //store bearing with slope and facing direction
  146. volatile bool bearingD = false;
  147.  
  148. volatile int previousBallX = 0;
  149. volatile int previousBallY = 0;
  150. volatile bool stopDebounce = false;
  151.  
  152. volatile bool nextRealState = true;
  153.  
  154. char UDPString[30];
  155.  
  156. #define red
  157. int getSelfX(void){
  158. #ifdef orange
  159. return P.getI3();
  160. #endif
  161. #ifdef red
  162. return P.getI5();
  163. #endif
  164. #ifdef blue
  165. return P.getI7();
  166. #endif
  167. }
  168. int getSelfY(void){
  169. #ifdef orange
  170. return P.getI4();
  171. #endif
  172. #ifdef red
  173. return P.getI6();
  174. #endif
  175. #ifdef blue
  176. return P.getI8();
  177. #endif
  178. }
  179.  
  180. void startMove(double x1,double y1, double x2,double y2,bool b1,bool b2,int spd = 0){
  181. pidx = spd;
  182. if(x1 == x2) x2 += 0.1; // prevent vertical line
  183. if(y1 == y2) y2 += 0.1; // prevent horizontal line
  184. lineM = (y1 - y2)/(x1 - x2); // calculate line slope
  185. lineC = y1 - lineM * x1;
  186. destinationX = x2; //log the destination
  187. destinationY = y2;
  188. blind = b1; //see if move backward
  189. shoot = b2; //see if shoot
  190. lineDirection = x1 < x2 ? true: false; // if x1 < x2 we need to go to the direction facing positive x axis
  191. mode = 1; // set mode to move mode
  192. left.setMode(0); // set counter to pid mode
  193. right.setMode(0);
  194. pid_speed.reset(); //reset pid
  195. pid_track.reset();
  196. shoot_speed.reset();
  197. shoot_track.reset();
  198. pid1_speed.reset();
  199. }
  200.  
  201. int offTrackDistance(double p,double q){
  202. //giving shortest distance from point to line
  203. //positive for offtrack to the left, negative for right
  204. double xi = (q+(1/lineM)*p-lineC)/(lineM+(1/lineM));
  205. double yi = lineM*xi + lineC;
  206. int d = distance(p,q,xi,yi);
  207. if(lineDirection){
  208. //positive x-axis
  209. return q > yi ? d : -d;
  210. }else{
  211. //negative y-axis
  212. return q > yi ? -d : d;
  213. }
  214. }
  215.  
  216. void move(void){
  217. //PID content
  218. int rt = right.getCount(), lt = left.getCount();
  219. int d = offTrackDistance(getSelfX(),getSelfY()); // blue
  220. int control_speed, control_track;
  221. if(shoot){control_speed = pid_speed.control(rt + lt, PIDTargetSpeed);
  222. control_track = pid_track.control(rt - lt, offTrackD2Error(d) * (blind ? -1 : 1));
  223. control_speed = shoot_speed.control(rt + lt, shootTargetSpeed);
  224. control_track = shoot_track.control(rt - lt, 0);
  225. }else if(pidx == 0){
  226. control_speed = pid_speed.control(rt + lt, PIDTargetSpeed);
  227. control_track = pid_track.control(rt - lt, offTrackD2Error(d) * (blind ? -1 : 1));
  228. }else{
  229. control_speed = pid1_speed.control(rt + lt, 4);
  230. control_track = pid1_track.control(rt - lt, offTrackD2Error(d) * (blind ? -1 : 1));
  231. }
  232. rm.setSpeed((control_speed+control_track)* (blind ? -1 : 1));
  233. lm.setSpeed((control_speed-control_track)* (blind ? -1 : 1));
  234. }
  235.  
  236. int degree2count(int i){return (int)((((double)i)*turnFactor)+0.5);}
  237.  
  238. //1 = left; 0 = right
  239. void startTurn(bool direction,int i ){
  240. mode = 2; //set control class mode
  241. left.setMode(1); //set counter to turn mode
  242. right.setMode(1);
  243. //set speed
  244. turnDegree = i;
  245. turnDirection = direction;
  246. turnSingleFinished = false;
  247. pid_speed.reset(); //reset pid
  248. pid_track.reset();
  249. }
  250.  
  251.  
  252. volatile int debug1 = 0;
  253. volatile int debug2 = 0;
  254. //only run this right after getBearingMode
  255. void startTurn2Coordinate(int x,int y){
  256. double p = getSelfX();
  257. double q = getSelfY();
  258. if(p == x) p += 0.1;
  259. if(q == y) q += 0.1;
  260. double m = ((double)(y-q))/((double)(x-p));
  261. bool d = p < x ? true:false;
  262. int i1 = trueBearing(bearingM,bearingD);
  263. int i2 = trueBearing(m,d);//i1 = present bearing; i2 = target bearing
  264. debug1 = i1;
  265. debug2 = i2;
  266. if(abs(i1 - i2)>180){
  267. startTurn(i1>i2?0:1,360 - abs(i1-i2));
  268. }else{
  269. startTurn(i1>i2?1:0,abs(i1-i2));
  270. }
  271. }
  272.  
  273. void move4turn(void){
  274. int rt = right.getCount(), lt = left.getCount();
  275. int control_speed = shoot_speed.control(rt + lt, 3);
  276. int control_track = shoot_track.control(rt - lt, 0);
  277. if(turnSingleFinished){
  278. if(turnDirection){
  279. //left
  280. lm.setSpeed((-turnSpeed)*(turnFinishedSide?1:0));
  281. rm.setSpeed((turnSpeed)*(turnFinishedSide?0:1));
  282. }else{
  283. //right
  284. lm.setSpeed((turnSpeed)*(turnFinishedSide?1:0));
  285. rm.setSpeed((-turnSpeed)*(turnFinishedSide?0:1));
  286. }
  287. }else{
  288. rm.setSpeed((control_speed+control_track)* (turnDirection ? 1 : -1));
  289. lm.setSpeed((control_speed-control_track)* (turnDirection ? -1 : 1));
  290. }
  291. /**/
  292. }
  293.  
  294. void stop(void){
  295. lm.setSpeed(0);
  296. rm.setSpeed(0);
  297. }
  298.  
  299. #define getBearingTime 500//ms
  300. void startGetBearing(void){
  301. startX = getSelfX(); //Assume using blue mat
  302. startY = getSelfY(); //record the init coordinate
  303. mode = 3; //set it to get bearing mode
  304. left.setMode(0); // set counter to pid mode
  305. right.setMode(0);
  306. pid_speed.reset(); //reset pid
  307. pid_track.reset();
  308. timer.setTimeout(getBearingTime);//count 500ms
  309. }
  310.  
  311. #define bearingTargetSpeed 3
  312. void move4bearing(void){
  313. int rt = right.getCount(), lt = left.getCount();
  314. int control_speed = pid_speed.control(rt + lt, bearingTargetSpeed);
  315. int control_track = pid_track.control(rt - lt, 0);
  316. rm.setSpeed(control_speed+control_track);
  317. lm.setSpeed(control_speed-control_track);
  318. }
  319.  
  320. void calculateBearing(void){
  321. double i = getSelfX();
  322. double j = getSelfY();
  323. if(i == startX) i += 0.1; // prevent vertical line
  324. if(j == startY) j += 0.1; // prevent horizontal line
  325. bearingM = (double)(startY - j)/(double)(startX - i);
  326. bearingD = startX < i ? true : false; // if startX < i we facing positive x axis
  327. }
  328. #define debounceTime 300
  329. void startWaitBallStop(void){
  330. previousBallX = P.getI1();
  331. previousBallY = P.getI2();
  332. stopDebounce = false;
  333. mode = 4;
  334. }
  335.  
  336. void startDelay(int i){
  337. timer.setTimeout(i);
  338. mode = 5;
  339. }
  340.  
  341. bool moveDetermine(void){
  342. switch(state){
  343. case 2:
  344. if(getSelfX()>210 && getSelfY()>143) return true;
  345. break;
  346. case 3:
  347. if(getSelfX()<201 && getSelfY()<131) return true;
  348. break;
  349. case 6:
  350. if(abs((getSelfY()-destinationY))<33) return true;
  351. break;
  352. case 8:
  353. if((destinationX-getSelfX()<30)) return true;
  354. break;
  355. case 9:
  356. if(abs(getSelfX()-destinationX)<33 && abs(getSelfY()-destinationY)<33) return true;
  357. break;
  358. case 10:
  359. return true;
  360. break;
  361. case 11:
  362. if(abs(getSelfX()-destinationX)<39 && abs(getSelfY()-destinationY)<39) return true;
  363. break;
  364. case 17:
  365. if(abs(getSelfX()-destinationX)<33 && abs(getSelfY()-destinationY)<33) return true;
  366. break;
  367. case 20:
  368. if(abs(getSelfX()-destinationX)<10)return true;
  369. break;
  370. case 19:
  371. if(abs(getSelfX()-destinationX)<10 && abs(getSelfY()-destinationY)<10) return true;
  372. break;
  373. case 22:
  374. if(abs(getSelfY()-destinationY)<10) return true;
  375. break;
  376. case 74:
  377. if((getSelfX()-destinationX)<60) return true;
  378. break;
  379. case 27:
  380. if(abs(getSelfX()-destinationX)<20 && abs(getSelfY()-destinationY)<30) return true;
  381. break;
  382. case 46:
  383. if(getSelfX()>962) return true;
  384. break;
  385. case 13:
  386. if(abs(getSelfX()-destinationX)<28 && abs(getSelfY()-destinationY)<28) return true;
  387. break;
  388. default:
  389. break;
  390. }
  391. return false;
  392. }
  393.  
  394. void nextState(void){
  395. switch(state){
  396. case 0:
  397. startMove(getSelfX(),getSelfY(),P.getI1(),P.getI2(),0,0,0);
  398. state = 2;
  399. break;
  400. case 1:
  401. startDelay(1000);//mode 1
  402. state = 2;
  403. break;
  404. case 2:
  405. startMove(getSelfX(),getSelfY(),167,111,1,0,0);//mode 1
  406. state = 8;
  407. break;
  408. case 3:
  409. startGetBearing();//mode 3
  410. state = 4;
  411. break;
  412. case 4:
  413. startTurn2Coordinate(getSelfX(),P.getI2());
  414. state = 5;
  415. break;
  416. case 5:
  417. startMove(getSelfX(),getSelfY(),getSelfX(),P.getI2(),0,0);
  418. state = 6;
  419. break;
  420. case 6:
  421. startGetBearing();//mode 3
  422. state = 7;
  423. break;
  424. case 7:
  425. startTurn2Coordinate(P.getI1(),P.getI2());
  426. state = 30;
  427. break;
  428. case 30:
  429. startMove(getSelfX(),getSelfY(),P.getI1(),P.getI2(),0,0,0);//shoot mode
  430. state = 8;
  431. break;
  432. case 8:
  433. startMove(getSelfX(),getSelfY(),P.getI1(),P.getI2(),0,1,0);//shoot mode
  434. state = 9;
  435. break;
  436. case 9:
  437. startDelay(3500);
  438. state = 10;
  439. //startMove(getSelfX(),getSelfY(),P.getI1(),P.getI2(),0,0);// wait ball
  440. break;
  441. case 10:
  442. startMove(getSelfX(),getSelfY(),P.getI1(),P.getI2(),0,0,0);
  443. state = 11;
  444. break;
  445. case 11:
  446. startMove(getSelfX(),getSelfY(),P.getI1(),P.getI2(),0,1,0);
  447. state = 13;
  448. break;
  449. case 13:
  450. startDelay(1000);
  451. state = 19;
  452. break;
  453. case 14:
  454. startTurn(true,10);
  455. state = 99;
  456. break;
  457. case 15:
  458. startTurn2Coordinate(getSelfX(),getSelfY()-30);//turn right 90
  459. state = 16;
  460. break;
  461. case 16:
  462. startMove(getSelfX(),getSelfY(),getSelfX(),getSelfY()-110,0,0,0);
  463. state = 17;
  464. break;
  465. case 17:
  466. startTurn(false,90);
  467. state = 19;
  468. break;
  469. case 18:
  470. startTurn2Coordinate(getSelfX()+50,getSelfY());
  471. state = 19;
  472. break;
  473. case 19:
  474. startMove(getSelfX(),getSelfY(),879,getSelfY(),0,0,0);
  475. state = 20;
  476. break;
  477. case 20:
  478. startTurn(false,90);
  479. state = 21;
  480. break;
  481. case 21:
  482. startMove(getSelfX(),getSelfY(),getSelfX(),P.getI2(),0,0);
  483. state = 22;
  484. break;
  485. case 22:
  486. startTurn(false,70);
  487. state = 29;
  488. break;
  489. case 23:
  490. startGetBearing();
  491. state = 24;
  492. break;
  493. case 24:
  494. startTurn2Coordinate(123,123);
  495. state = 25;
  496. break;
  497. case 25:
  498. startMove(getSelfX(),getSelfY(),P.getI1(),P.getI2(),0,0,1);
  499. state = 74;
  500. break;
  501. case 26:
  502. startMove(getSelfX(),getSelfY(),P.getI1(),P.getI2(),0,1);
  503. state = 27;
  504. break;
  505. case 74:
  506. startDelay(700);
  507. state = 26;
  508. break;
  509. case 27:
  510. startDelay(1000);
  511. state = 50;
  512. break;
  513. case 50:
  514. startTurn(true,90);
  515. state = 51;
  516. break;
  517. case 51:
  518. startMove(getSelfX(),getSelfY(),getSelfX(),getSelfY()+70,0,0,0);
  519. state = 99;
  520. break;
  521. case 29:
  522. startMove(getSelfX(),getSelfY(),getSelfX()+20,getSelfY(),1,0,0);
  523. state = 46;
  524. break;
  525. case 46:
  526. startDelay(1000);
  527. state = 25;
  528. break;
  529. case 90:
  530. startWaitBallStop();//mode = 4
  531. state = 91;
  532. break;
  533. case 91:
  534. startGetBearing(); // mode = 3
  535. state = 92;
  536. break;
  537. case 92:
  538. startTurn2Coordinate(0,0);
  539. state = 93;
  540. break;
  541. case 93:
  542. startMove(getSelfX(),getSelfY(),0,0,0,0);//mode = 1
  543. state = 94;
  544. break;
  545. case 94:
  546. mode = 0;
  547. state = 95;
  548. break;
  549. case 98:
  550. startMove(getSelfX(),getSelfY(),P.getI1(),P.getI2(),0,0,0);
  551. state = 99;
  552. break;
  553. case 99:
  554. mode = 0;
  555. state = 9527;
  556. break;
  557. default:
  558. break;
  559. }
  560. }
  561.  
  562. #define car1
  563. #ifdef car1
  564. void determineMode(void){
  565. bool end = false;
  566. if(mode == 1){
  567. end = moveDetermine();
  568. }else if(mode == 2){
  569. //determine next mode for car in turn mode
  570. //check if finish
  571. //if(state == 1){//in the state you desire
  572. if((left.getTurnCount()>=degree2count(turnDegree)) && (right.getTurnCount()>=degree2count(turnDegree))){
  573. //finished turn
  574. end = true;
  575. }else if(left.getTurnCount()>=degree2count(turnDegree)){
  576. turnSingleFinished = true;
  577. turnFinishedSide = 0;
  578. }else if(right.getTurnCount()>=degree2count(turnDegree)){
  579. turnSingleFinished = true;
  580. turnFinishedSide = 1;
  581. }
  582. //}
  583. }else if(mode == 3){
  584. //get bearing mode
  585. if(timer.isTimeout()){
  586. //500ms passed
  587. lm.setSpeed(0);//stop both motor
  588. rm.setSpeed(0);
  589. calculateBearing();
  590. end = true;startMove(getSelfX(),getSelfY(),P.getI1(),P.getI2(),0,0);
  591. }
  592. }else if(mode == 4){
  593. //wait ball stop
  594. if(P.receivedNewPacket()){
  595. if(stopDebounce){
  596. if(timer.isTimeout() && P.getI1() == previousBallX && P.getI2() == previousBallY){
  597. end = true;//same coordinate after 300 ms will lead to next state
  598. }else{
  599. stopDebounce = false;
  600. previousBallX = P.getI1();
  601. previousBallY = P.getI2();
  602. }
  603. }else if(P.getI1() == previousBallX && P.getI2() == previousBallY){
  604. //if ball coordinate not changing
  605. stopDebounce = true;//start debouncing
  606. timer.setTimeout(debounceTime);
  607. }else{
  608. //update ball coordinate
  609. previousBallX = P.getI1();
  610. previousBallY = P.getI2();
  611. }
  612. }
  613. }else if(mode == 5){
  614. if(timer.isTimeout()) end = true;
  615. }else if(mode == 0){
  616. //determine if stop mode end
  617. }
  618. if(end){
  619. nextState();
  620. }
  621. }
  622. #endif
  623. #ifdef car3
  624.  
  625. #endif
  626.  
  627. void loop(void){
  628. determineMode();
  629. if(mode == 1){
  630. move();
  631. }else if(mode == 2){
  632. //anything need to do?
  633. move4turn();
  634. }else if(mode == 3){
  635. move4bearing();
  636. }else if(mode == 4){
  637. stop();
  638. }else if(mode == 5){
  639. stop();
  640. }else if(mode == 0){
  641. stop();
  642. }
  643. }
  644.  
  645. /////////////////////////////////
  646.  
  647. void waitForDevice(void){
  648. const char* cmd = AT;
  649. int cmdlen = strlen(cmd);
  650. for(int i = 0;i<cmdlen;i++){
  651. tx.save(cmd[i]);
  652. }
  653. IEC1bits.U2TXIE = 1;
  654. while(!rx.OKready());
  655. }
  656.  
  657. void ESP8266Init(void){
  658. int cmdlen;
  659. char cmd[60];
  660. strcpy(cmd,CWMODE1);
  661. cmdlen = strlen(cmd);
  662. for(int i = 0;i<cmdlen;i++){
  663. tx.save(cmd[i]);
  664. }
  665. IEC1bits.U2TXIE = 1;
  666. while(!rx.OKready());
  667. strcpy(cmd,CIPMUX0);
  668. cmdlen = strlen(cmd);
  669. for(int i = 0;i<cmdlen;i++){
  670. tx.save(cmd[i]);
  671. }
  672. IEC1bits.U2TXIE = 1;
  673. while(!rx.OKready());
  674. strcpy(cmd,CWJAP);
  675. cmdlen = strlen(cmd);
  676. for(int i = 0;i<cmdlen;i++){
  677. tx.save(cmd[i]);
  678. }
  679. IEC1bits.U2TXIE = 1;
  680. while(!rx.OKready());
  681. strcpy(cmd,CIPSTART);
  682. cmdlen = strlen(cmd);
  683. for(int i = 0;i<cmdlen;i++){
  684. tx.save(cmd[i]);
  685. }
  686. IEC1bits.U2TXIE = 1;
  687. //while(!rx.OKready());
  688. }
  689.  
  690. void CIPSTARTUDP(void){
  691. int cmdlen;
  692. char cmd[60];
  693. strcpy(cmd,CIPSTART);
  694. cmdlen = strlen(cmd);
  695. for(int i = 0;i<cmdlen;i++){
  696. tx.save(cmd[i]);
  697. }
  698. IEC1bits.U2TXIE = 1;
  699. }
  700.  
  701. void usshort(unsigned u,char* c){
  702. int i, t, r;
  703. for(i =0;i<4;i++) c[i] = ' ';
  704. c[i] = '0';
  705. while(u){
  706. t = u /10;
  707. r = u - t*10;
  708. u = t;
  709. c[i--] = '0' +r;
  710. }
  711. }
  712.  
  713. /////////////////////////////////
  714.  
  715. int main(void) {
  716. int debugCount = 0;
  717. USBDeviceInit();
  718. waitForDevice();
  719. led.on();
  720. //ESP8266Init(); // only uncomment this when u are the first time to connect the wifi(or ssid/passwd changed)
  721. CIPSTARTUDP();
  722. /*bool flag = true;
  723. while(flag){
  724. if(rx.getString()){
  725. strcpy(UDPString,rx.getString());
  726. if(UDPString[0] == 'P') flag = false;
  727. }
  728. }
  729. P.write(UDPString);*/
  730. while(!P.receivedNewPacket());
  731. //first state
  732. startDelay(3000);//first state init
  733. if(getSelfX()>640)
  734. {state=19;}
  735. else{state = 8;}
  736. //state = 98;
  737. //startMove(getSelfX(),getSelfY(),P.getI1(),P.getI2(),0,0);//present
  738. IEC0bits.T5IE = 1; //timer5 interrupt enable
  739. //the software timer i wrote wont fucking work before timer5 interrupt enabled
  740. //also the loop wont run and counter wont be sampling
  741. char debugString[] = " \r\0";
  742. //01234567890123456789012345678901234567890123467890
  743. int debugStringlength = strlen(debugString);
  744. while (1){
  745. USBTasks();
  746. if(++debugCount > 10000){
  747. //add the data u want to monitor here
  748. //dd = offTrackDistance(getSelfX(),getSelfY());
  749. //usshort(debug2,&debugString[37]);
  750. //usshort(debug1,&debugString[30]);
  751. //usshort(turnDegree,&debugString[25]);
  752. //usshort(turnDirection,&debugString[20]);
  753. //usshort(turnDegree,&debugString[35]);
  754. usshort(left.getTurnCount(),&debugString[25]);
  755. usshort(right.getTurnCount(),&debugString[32]);
  756. putUSBUSART(debugString,debugStringlength);
  757. debugCount = 0;
  758. }
  759. //above should be done in timer interrupt but too lazy to do
  760.  
  761. }
  762. }
  763.  
  764. extern "C" void __attribute__ ((interrupt)) t5ISR(void) {
  765. //this function run every 2.5ms
  766. left.sample(TMR4); // sample for left wheel counter
  767. right.sample(TMR2); // sample for right wheel counter
  768. timer.tick(); // better not use this timer in this function
  769. loop(); // main control function needed to run very often
  770. IFS0bits.T5IF = 0;
  771. }
  772.  
  773. extern "C" void __attribute__ ((interrupt)) u2ISR(void) {
  774. while (U2STAbits.URXDA) rx.save(U2RXREG);
  775. IFS1bits.U2RXIF = 0;
  776. if (IEC1bits.U2TXIE) {
  777. while (!U2STAbits.UTXBF) {
  778. if (tx.empty()) { IEC1bits.U2TXIE = 0; break; }
  779. else U2TXREG = tx.get();
  780. }
  781. IFS1bits.U2TXIF = 0;
  782. }
  783. if(rx.getString()){//see if there is new packet
  784. led.toggle();
  785. strcpy(UDPString,rx.getString());//copy to buffer
  786. rx.free();//free read buffer to receive next packet
  787. if(UDPString[0] == 'A'){
  788. A.write(UDPString);
  789. }else{
  790. P.write(UDPString);
  791. }
  792. }
  793. }
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!