API tools faq
paste
Advertisement
ununununun

ASDAB2_PLDUINO_NOWIFI

ununununun
Jul 8th, 2019
137
0
Never
Add comment
Not a member of Pastebin yet? Sign Up, it unlocks many cool features!
text 29.14 KB | None | 0 0
raw download clone embed print report
  1. #include <EEPROM.h>
  2. #include <digitalWriteFast.h>
  3. #include <avr/wdt.h>
  4. //#include "ApplicationMonitor.h"
  5. //#include <AnalogSmooth.h>
  6. #include <TMRpcm_PLDuino.h>
  7. #include <SPI.h>
  8. #include <SD.h>
  9. //#include <PDQ_GFX.h>
  10. //#include <PDQ_ILI9341_config.h>
  11. //#include <PDQ_ILI9341.h>
  12. #include <Adafruit_GFX.h>
  13. #include <Adafruit_ILI9341.h>
  14. #include <PLDuino.h>
  15. #include <PLDTouch.h>
  16. #include <PLDuinoGUI.h>
  17. #include "utils1.h"
  18. #include <using_namespace_PLDuinoGUI.h>
  19. #include <DS3232RTC.h>
  20. #include <TimeLib.h>
  21. #include <avr/io.h>
  22. #include <SPI.h>
  23. #include <SD.h>
  24. #include "WiFiEsp.h"
  25. #include <WiFiEspUdp.h>
  26. #include <EEPROM.h>
  27. #include <Wire.h>
  28.  
  29.  
  30.  
  31. //Watchdog::CApplicationMonitor ApplicationMonitor;
  32.  
  33.  
  34.  
  35. // ASDA-B2
  36. // P1-08 POSITION SMOOTH (Position Low Pass Filter)
  37. // P1-44 E-GEAR RATIO [100]
  38. // P2-35 условие о превышении ошибки по положению AL009. default=480000
  39. // P1-34 время разгона [200мс] 30мс \\ для режима JOG_MODBUS < 3
  40. // P1-35 время торможения [200мс] 30мс \\ для режима JOG_MODBUS < 3
  41. // P1-68 фильтрр комманды позицианирования [0мс] 1мс
  42. // P2-33 полуавто. режим коррекции [0]
  43. // P2-32 выбор способа автонастройки [0] 1 - auto; 2 - au/to
  44.  
  45.  
  46.  
  47. #define D1_FW 39 //1 fw1 дв1 - вперёд
  48. #define D2_FW 41 //2 fw2 дв2 - вперёд
  49. #define D1_RW 38 //1 rw1 дв1 - назад
  50. #define D2_RW 42 //2 rw2 дв2 - назад
  51.  
  52. #define GK1 34 // геркон положения дв1
  53. #define GK2 35 // геркон положения дв2
  54. #define LBACK 10 // задняие диоды
  55. #define ASPEED A7
  56. #define ABREAK 10
  57. #define AANGLE A6
  58. #define LEDI 31
  59. #define A0SMOOTH 20
  60. #define A1SMOOTH 70
  61. #define A0SHIFT 506
  62. #define A1SHIFT 286//154 // в зависимости от положения датчика при установке датчика
  63. #define AENABLE true
  64. #define STARTPIN 31
  65. #define START_ENABLED false
  66. #define BAUDRATE 19200
  67. #define RSTPIN A5
  68. #define ANGLEC 1
  69. #define ANG 360
  70. #define SPEED0 284
  71. #define FILENAME "xtre.cfg"
  72.  
  73. // --------------------
  74. int D;
  75. int DR_DELAY_1 = 0;
  76. int DR_DELAY_2 = 0;
  77. int UDP_DELAY = 0;
  78. bool touched = false;
  79. TMRpcm tmrpcm; // create an object for use in this sketch
  80.  
  81. //AnalogSmooth asx = AnalogSmooth(10);
  82. File f;
  83. WiFiEspUDP udp;
  84. int status = WL_IDLE_STATUS;
  85. int clr = 0xF800;
  86. int shiftx=0;
  87. String s = "";
  88. String is,is1 = "" ;
  89. boolean sc = false;
  90. boolean i_s =false;
  91. bool CFG_OK = false;
  92. String ssid, pass;
  93. unsigned int localPort = 9999; // local port to listen on
  94. unsigned int rPort = 9990; // remote port
  95. IPAddress Ip(192, 168, 1, 255);
  96.  
  97. Label l("", ILI9341_WHITE);
  98. Adafruit_ILI9341 tft = Adafruit_ILI9341(PLDuino::LCD_CS, PLDuino::LCD_DC);
  99. PLDTouch touch(PLDuino::TOUCH_CS, PLDuino::TOUCH_IRQ);
  100. int Y,X1,Y1;
  101. // --------------------
  102.  
  103.  
  104. int ASPEED_OUT = 0;
  105. long millis100 = 0;
  106.  
  107. int prev_aspeed, prev_arb, aspeed, arot0, aangle;
  108. int x = 0;
  109. int cnt = 0;
  110. String inputString, oStr = ""; // a string to hold incoming data
  111. String spd1,spd2,pls1,pls2 = ""; //ccw_spd cw_spd impulses
  112. bool stringComplete = false; // whether the string is complete
  113. bool stringDelim = false;
  114. bool CORRECT = false;
  115. unsigned int CORRECTC = 0;
  116. long CNT_DELAY1,CNT_DELAY0, CNT_PULSES1, CNT_PULSES0, FFDELAY = 0;
  117. int dl1,dl0,speed1,speed1p,_speed1,speed2,speed2p,_speed2,D1,D2 = 0;
  118. long Pulses0, Pulses0CNT, Pulses1, Pulses1CNT;
  119. bool CNT1_,CNT0_,BRK0,BRK1,END1,END0,UP_DOWN = false;
  120. long A180 = 100000;
  121. #define GEAR_RATIO 40 // ASDA-B2 -> P1-44 -> VAL/100 = 40; текущее значние 4000
  122. #define RATIO_DELAY 20 //
  123. #define UDP_READ_TIMEOUT 2
  124. unsigned long micros0, micros1;
  125. long ar =0;
  126. float ar1,prev_arot = 0.0;
  127. int ari,ari1,arb,arb1,ledi = 0;
  128. bool abreakb = false;
  129. int abreakval = 0;
  130. unsigned int AREADDL = 0;
  131. char delim; // разделитель входных параметров
  132. String _str = "";
  133.  
  134. unsigned int ASDA_ALARM_RESET_CNT = 0;
  135.  
  136. long POSA,POSB; // счётчик абсолютного угла
  137. long POSA1,POSB1; // счётчик фактического угла
  138.  
  139. typedef (*SmtToDo)();
  140.  
  141.  
  142.  
  143.  
  144. class DelayAsync{
  145. private:
  146. bool b = false;
  147. unsigned dl = 0;
  148. unsigned long t = 0;
  149. public:
  150. DelayAsync(int Delay);
  151. Start();
  152. bool isDone();
  153. };
  154.  
  155. DelayAsync::Start()
  156. {
  157. if (b==false)
  158. {
  159. b = true;
  160. // t = micros();
  161. }
  162.  
  163. }
  164.  
  165. DelayAsync::DelayAsync(int Delay)
  166. {
  167. this->t = Delay;
  168. };
  169.  
  170. bool DelayAsync::isDone()
  171. {
  172. return true;
  173. };
  174.  
  175.  
  176. class MyDelayedTask
  177. {
  178. private:
  179. int cnt = -1;
  180. typedef (*Task1)();
  181. struct TaskEx{
  182. Task1 Task;
  183. String tName;
  184. int tDelay;
  185. };
  186. TaskEx Tasks[255];
  187. public:
  188. MyDelayedTask();
  189. Add(String taskName, Task1 myTask, unsigned int Delay);
  190. Start (String taskName);
  191. Stop (String taskName);
  192.  
  193. };
  194.  
  195.  
  196. void UDP_SEND(String s)
  197. {
  198. udp.beginPacket(Ip, rPort);
  199. udp.write(s.c_str()) ;
  200. udp.endPacket();
  201. }
  202.  
  203. const char* c_str1(String str) {
  204. char* buf = new char[str.length()];
  205. str.getBytes(buf, str.length()+1, 0);
  206. strcpy(reinterpret_cast<const char*>(buf),buf);
  207. return buf;
  208. }
  209.  
  210. void UpdStaticLog(String s, bool visible, uint16_t color)
  211. {
  212. //showl(is);
  213. tft.setCursor(0, 0);
  214. tft.setTextColor(0x0000);
  215. tft.println(s);
  216. if (!visible) return;
  217. tft.setTextColor(color);
  218. tft.setCursor(0, 0);
  219. tft.println(s);
  220. }
  221.  
  222.  
  223. void setup_PLC()
  224. {
  225.  
  226. //pinMode(42, OUTPUT);
  227.  
  228.  
  229.  
  230. PLDuino::init();
  231.  
  232. PLDuino::enableLCD();
  233. PLDuino::enableESP();
  234.  
  235. tft.begin();
  236. tft.fillScreen(ILI9341_BLACK);
  237. tft.setRotation(3);
  238. tft.setTextSize(1);
  239. touch.init(1);
  240.  
  241.  
  242.  
  243. Serial.begin(115200);
  244. // Serial2.begin(115200);
  245. // WiFi.init(&Serial2);
  246. tmrpcm.speakerPin = 9;
  247. delay(100);
  248. tone(9, 700); delay(50); noTone(9);
  249. tone(9, 700); delay(50); noTone(9);
  250.  
  251.  
  252. while (!SD.begin(PLDuino::SD_CS))
  253. {
  254.  
  255. UpdStaticLog("Failed to initialize SD card. Please check it.", true, 0xF800);
  256. delay(1000);
  257. UpdStaticLog("Failed to initialize SD card. Please check it.", false, 0x0000);
  258. delay(1000);
  259. }
  260.  
  261. UpdStaticLog("Failed to initialize SD card. Please check it.", false, 0x0000);
  262. Serial.println("SD Open...");
  263. UpdStaticLog("SD card init OK.", true, 0xFFFF);
  264. /*
  265.  
  266. File f = SD.open(FILENAME, FILE_READ);
  267. Serial.println("SD Open... OK");
  268. String fs = "SHITTTTTTTTTTT";
  269. {
  270. // Stop on error
  271. // UpdStaticLog("Failed to open " FILENAME ". Stop.", true, 0xF800);
  272. // return;
  273. }
  274. while (f.available() > 0)
  275. {
  276. fs = f.readStringUntil('\n');
  277.  
  278. if (fs.indexOf("_apc]") > 0)
  279. {
  280. ssid = f.readStringUntil('\n');
  281. pass = f.readStringUntil('\n');
  282. break;
  283. }
  284.  
  285. }
  286.  
  287. Serial.println(ssid);
  288. Serial.println(pass);
  289.  
  290. f.close();
  291. delay(1000);
  292. Serial.println(pass.c_str());
  293. UpdStaticLog("SD card init OK. Loading config...", false, 0x0000);
  294. UpdStaticLog("SSID=" + String(ssid) + "\nPASS=" + String(pass), true, 0xFFFF);
  295. delay(1000);
  296. UpdStaticLog("SSID=" + String(ssid) + "\nPASS=" + String(pass), false, 0x0000);
  297. byte l0 = ssid.length()+1;
  298. byte l1 = pass.length()+1;
  299. char ssid0[l0];
  300. char pass0[l1];
  301. ssid.toCharArray(ssid0,l0);
  302. pass.toCharArray(pass0,l1);
  303. if (WiFi.status() == WL_NO_SHIELD) {
  304. Serial.println("WiFi shield not present");
  305.  
  306. // don't continue:
  307. while (true);
  308. }
  309. ssid.replace(0xd,'\0');
  310. ssid.replace(' ','\0');
  311. pass.replace(' ','\0');
  312. pass.replace(0xd,'\0');
  313.  
  314.  
  315. String s0 = "Attempting to connect to WPA SSID: "+String(ssid.c_str());
  316.  
  317. while ( status != WL_CONNECTED) {
  318.  
  319. digitalWriteFast(42,HIGH);
  320. delay(500);
  321. digitalWriteFast(42,LOW);
  322. UpdStaticLog(s0,false,0x0000);
  323. UpdStaticLog(s0,true,0xFFFF);
  324. Serial.println(s0);
  325. tone(9, 500); delay(100); noTone(9);
  326.  
  327. // Connect to WPA/WPA2 network
  328. // ssid.remove(ssid.length());
  329. // pass.remove(pass.length());
  330.  
  331. status = WiFi.begin(c_str1(ssid),c_str1(pass));
  332. delay(500);
  333.  
  334. }
  335.  
  336. char bufIP[16];
  337. sprintf(bufIP, "IP:%d.%d.%d.%d", WiFi.localIP()[0], WiFi.localIP()[1], WiFi.localIP()[2], WiFi.localIP()[3] );
  338. UpdStaticLog(s0,false,0x0000);
  339. UpdStaticLog("Connected. " +String(bufIP),true,0xFFFF);
  340.  
  341. Serial.println(String(bufIP));
  342.  
  343. udp.begin(localPort);
  344. delay(2000);
  345. UpdStaticLog("Connected. " +String(bufIP),false,0x0000);
  346. tft.setTextSize(2);
  347. */
  348. const char *x = "x1.bmp";
  349. bmpDraw(tft,x,0,0);
  350.  
  351.  
  352.  
  353.  
  354. }
  355.  
  356. MyDelayedTask::MyDelayedTask()
  357. {
  358. cnt=0;
  359.  
  360. }
  361.  
  362. MyDelayedTask::Add(String taskName, Task1 myTask, unsigned int Delay)
  363. {
  364. TaskEx t;
  365. t.tName = "";
  366. t.Task = myTask;
  367. t.tDelay = Delay;
  368. Tasks[sizeof(Tasks)] = t;
  369. }
  370.  
  371. MyDelayedTask:: Start(String taskName)
  372. {
  373.  
  374. }
  375.  
  376. long P2A(long Pulses)
  377. {
  378. return (((Pulses*GEAR_RATIO)*ANG)/A180) ;
  379. }
  380.  
  381. void init1()
  382. { //INIT
  383. int prev_aspeed, prev_arb, aspeed, arot, prev_arot = 0;
  384. int x = 0;
  385. int cnt = 0;
  386. inputString = ""; // a string to hold incoming data
  387. stringComplete = false; // whether the string is complete
  388. long CNT_DELAY1,CNT_DELAY0, CNT_PULSES1, CNT_PULSES0;
  389. CNT_DELAY1 = 0;
  390. CNT_DELAY0 = 0;
  391. int dl1,dl0,speed1,_speed1,speed2,_speed2,D1,D2 = 0;
  392. long Pulses0, Pulses0CNT, Pulses1, Pulses1CNT = 0;
  393. bool CNT1_,CNT0_,BRK0,BRK1,END1,END0 = false;
  394. int ar,ar1 = 0;
  395. int ari,ari1,arb,arb1,ledi = 0;
  396. abreakb = false;
  397. abreakval = 0;
  398. }
  399.  
  400.  
  401.  
  402.  
  403. void POSAB()
  404. {
  405. int posa = 0; int posb = 0;
  406. if (POSA>POSB) {posa = (POSA-POSB)/2; posb = (POSA+POSB) /2;}
  407. if (POSA<POSB) {posa = (POSB-POSA)/2; posb = -(POSA+POSB) /2;}
  408. Serial.println(String(posa)+" "+String(posb));
  409. }
  410.  
  411. void Cnvrtr() //преобразовние положения двигателей в положение осей 2dof
  412. {
  413. int x1 = 0;
  414. int y1 = 0;
  415. int posa, posb =0;
  416. int posa1 = P2A(POSA1); int posb1 = P2A(POSB1); // преобразование посчитаных импульсов двигателей 1 2 в углы
  417.  
  418. if (posa1>posb1 && posa1>0 && posb1>=0) {posb = (posa1-posb1)/2; posa = posa1-posb; } else // _`_|_._
  419.  
  420. if (posb1>posa1 && posb1>0 && posa1>=0) {posb = -(posb1-posa1)/2; posa = posb1-abs(posb);} else // _._|_`_
  421.  
  422. // __|__
  423. if (abs(posa1)>abs(posb1) && posa1<0 && posb1<=0) {posb = -(abs(posa1)-abs(posb1))/2; posa = -(abs(posa1)-abs(posb));} else // . | `
  424.  
  425. // __|__
  426. if (abs(posb1)>abs(posa1) && posb1<0 && posa1<=0) {posb = (abs(posb1)-abs(posa1))/2; posa = -(abs(posb1)-abs(posb));} else // ` | .
  427.  
  428. if (abs(posa1)>abs(posb1) && posb1<=0 && posa1>0) {posb=(posa1+abs(posb1))/2; posa=posa1-posb; } else // _`_|_ _
  429. // | .
  430.  
  431. if (abs(posb1)>abs(posa1) && posa1<=0 && posb1>0) {posb=-(posb1+abs(posa1))/2; posa=posb1-abs(posb); } else // _ _|_`_
  432. // . |
  433.  
  434. if(abs(posa1)>posb1 && posa1<0 && posb1>0) {posb=-(abs(posa1)+abs(posb1))/2; posa = -(abs(posa1)-abs(posb)); } else // _`_|_ _ // если abs(`) > abs(.) и ` > 0 и . < 0
  435. // | .
  436.  
  437. if(abs(posb1)>posa1 && posb1<0 && posa1>0) {posb= (abs(posa1)+abs(posb1))/2; posa = -(abs(posb1)-abs(posb)); } else //_ _|_`_ // если abs(`) > abs(.) и ` > 0 и . < 0
  438. // . |
  439.  
  440. if(abs(posa1)==posb1 && posa1<0 && posb1>0) {posb= -(abs(posa1)); posa = 0; } else //_ _|_`_ // если abs(`) == abs(.) и ` > 0 и . < 0
  441. // . |
  442.  
  443. if(abs(posb1)==posa1 && posb1<0 && posa1>0) {posb= abs(posa1); posa = 0; } else //_`_|_ _ // если abs(`) == abs(.) и ` > 0 и . < 0
  444. // | .
  445.  
  446. if(abs(posa1)==posb1 && posa1>0 && posb1>0) {posb= 0; posa = abs(posa1); } else //_`_|_'_ // если abs(`) == abs(') и ` > 0 и ' > 0
  447. // |
  448.  
  449. if(posb1==posa1 && posb1<0 && posa1<0) {posb= 0; posa = -(abs(posa1)); } //_ _|_ _ // если abs(.) == abs(,) и . < 0 и , < 0
  450. // . | ,
  451.  
  452.  
  453. //текущие позицыи двигателей относительно их текущего положения
  454. x1 = pls1.toInt(); y1 = pls2.toInt();
  455. int x2 = x1; int y2 = y1;
  456.  
  457. x1 = x1 - posa;
  458. y1 = y1 - posb;
  459.  
  460. pls1 = String(x1 + y1);
  461. pls2 = String(x1 - y1);
  462.  
  463. POSA = x2;
  464. POSB = y2;
  465.  
  466. if (x2==0 && y2==0)
  467. {
  468. pls1 = String(-P2A(POSA1));
  469. pls2 = String(-P2A(POSB1));
  470. POSA=0;POSB=0;
  471. }
  472.  
  473. i_s = false;
  474.  
  475. }
  476.  
  477. int iofd(String s, char d) // проверка входной строки на корретность
  478. {
  479. int c = 0;
  480. for (int i=0; i<s.length(); i++)
  481. {if (s[i]==d) c++;}
  482. return c;
  483. }
  484.  
  485. void parseStr(String &str)
  486. {
  487.  
  488. if (str.indexOf("GLIST")>=0) { digitalWriteFast(42,HIGH); digitalWriteFast(42,HIGH);}
  489. if (str.indexOf("BLOXA")>=0) { digitalWriteFast(42,LOW); digitalWriteFast(42,LOW);}
  490. byte p0 = str.indexOf('[');
  491. byte p1 = str.indexOf(']');
  492. byte p2 = str.indexOf('.',p0);
  493. byte p3 = str.lastIndexOf('.',p0);
  494. String str0;
  495.  
  496. if (p1>p0)
  497. {
  498. //&& p2>p0 && p3<p1 && iofd(str,'.')==3
  499. str0 = str.substring(str.indexOf('[')+1,str.length()-(str.length()-str.indexOf(']'))); i_s = true;}
  500. else {spd1 = "0"; spd2 = "0"; pls1 = "0"; pls2="0"; };
  501.  
  502. // oStr = str0;
  503. //if (str[0]=='L') digitalWriteFast(LBACK, String(str[1]).toInt()); // вкл-выкл фонарей
  504.  
  505. if (delim=='.' && !UP_DOWN) _str = str;
  506. str.indexOf(',')>0?delim='.':delim='.';
  507.  
  508.  
  509.  
  510. /*
  511.  
  512. if ( iofd(str,' ')>0 || iofd(str,'+')>0 || iofd(str,'I')>0 || iofd(str,',')>0 ) // проверка строки на корректность, если входная строка кривая то не парсит
  513. {
  514. // oStr = str;
  515. spd1 = "0"; spd2 = "0"; pls1 = "0"; pls2="0";
  516. return;
  517. } // если вх строка не правильная то выхожу
  518.  
  519. */
  520. str = str0;
  521.  
  522. if (str.indexOf(delim)>0)
  523. {
  524. int pos,pos1;
  525. char chrs[str.length()];
  526. spd1 = ""; spd2 = ""; pls1 = ""; pls2="";
  527. for (int i=0; i!=(int)str.indexOf(delim); i++)
  528. {
  529. spd1 = spd1 + str[i];
  530. }
  531. pos = str.indexOf(delim,str.indexOf(delim)+1);
  532. for (int i=str.indexOf(delim)+1; i!=pos; i++)
  533. {
  534. spd2 = spd2 + str[i];
  535. }
  536. // pos = str.indexOf(delim,pos+1);
  537. pos1 = str.indexOf(delim,pos+1);
  538. for (int i=pos+1; i!=pos1; i++)
  539. {
  540. pls1 = pls1 + str[i];
  541. }
  542. for (int i=pos1+1; i!=str.length(); i++)
  543. {
  544. pls2 = pls2 + str[i];
  545. }
  546. speed1=0; speed2 = 0;
  547. }
  548.  
  549.  
  550.  
  551.  
  552.  
  553. Cnvrtr();
  554.  
  555.  
  556. //sSerial.println(String(POSA)+" "+String(POSB));
  557. //int posa, posb;
  558. // posa = POSA1; posb = POSB1;
  559. }
  560.  
  561.  
  562. void udp_read1()
  563. {
  564.  
  565. int packetSize = udp.parsePacket();
  566. if (packetSize)
  567. {
  568. udp.setTimeout(UDP_READ_TIMEOUT);
  569. is = udp.readStringUntil('\n');
  570. // Serial.println(is);
  571. parseStr(is);
  572. //is = is.substring(is.indexOf(':')+1);
  573. sc = true;
  574. }
  575.  
  576.  
  577.  
  578.  
  579. //is = is.substring(is.indexOf(':')+1);
  580. sc = true;
  581. }
  582.  
  583.  
  584.  
  585.  
  586. void SC()
  587. {
  588. if (stringComplete) {
  589. parseStr(inputString);
  590. inputString = "";
  591. stringComplete = false;
  592. }
  593. //str = inputString[1].concat(inputString[1]);
  594. }
  595.  
  596. void DriveIt(int srvNum,int Delay)
  597. {
  598. digitalWriteFast(srvNum, HIGH);
  599. delayMicroseconds(Delay);
  600. digitalWriteFast(srvNum, LOW);
  601. delayMicroseconds(Delay);
  602. }
  603.  
  604. void DriveItPls(int srvNum,int Delay,int Pulses)
  605. {
  606. for (int i=0; i++; i!=Pulses)
  607. {
  608. digitalWriteFast(srvNum, HIGH);
  609. delayMicroseconds(Delay);
  610. digitalWriteFast(srvNum, LOW);
  611. delayMicroseconds(Delay);
  612. }
  613. }
  614.  
  615. void DriveItAsync0(int CNT, int DELAY,int srvNum)
  616. {
  617. if (!CNT0_ && CNT==DELAY-1 && !BRK0)
  618. {
  619. CNT0_ = !CNT0_;
  620. Pulses0CNT++;
  621. srvNum==D1_RW?POSA1++:POSA1--;
  622. digitalWriteFast(srvNum, HIGH);
  623. }
  624.  
  625. if (CNT0_ && CNT==DELAY-1 && !BRK0)
  626. {
  627. CNT0_ = !CNT0_;
  628. Pulses0CNT++;
  629. digitalWriteFast(srvNum, LOW);
  630.  
  631. }
  632. // POSA = P2A(POSA1);
  633. }
  634.  
  635. void DriveItAsync1(int CNT, int DELAY,int srvNum)
  636. {
  637. if (!CNT1_ && CNT==DELAY-1 && !BRK1)
  638. {
  639. CNT1_ = !CNT1_;
  640. Pulses1CNT++;
  641. srvNum==D2_RW?POSB1++:POSB1--;
  642. digitalWriteFast(srvNum, HIGH);
  643.  
  644. }
  645.  
  646. if (CNT1_ && CNT==DELAY-1 && !BRK1)
  647. {
  648. CNT1_ = !CNT1_;
  649. Pulses1CNT++;
  650. digitalWriteFast(srvNum, LOW);
  651. }
  652.  
  653. // POSB = P2A(POSB1);
  654.  
  655. }
  656.  
  657. void DriveTo(int CW_CCW,int Delay, int pulses)
  658. {
  659. for (int i=0; i<pulses;i++)
  660. {
  661. if (CW_CCW==D1_RW) POSA1++;
  662. if (CW_CCW==D1_FW) POSA1--;
  663. if (CW_CCW==D2_RW) POSB1++;
  664. if (CW_CCW==D2_FW) POSB1--;
  665. DriveIt(CW_CCW, Delay);
  666. }
  667. //str1="";
  668. }
  669.  
  670. void OnRx()
  671. {
  672. // SC();
  673. }
  674.  
  675.  
  676. int SMOOTHPIN(int PIN) // фильтр цифрового входа
  677. {
  678. int a = LOW;
  679. for (int i=0; i!=10; i++) a|=digitalRead(PIN);
  680. return a;
  681. }
  682.  
  683.  
  684. void setup()
  685. {
  686. Serial.setTimeout(3);
  687.  
  688. pinMode(GK1, INPUT_PULLUP);
  689. pinMode(GK2, INPUT_PULLUP);
  690. pinMode(D1_FW, OUTPUT);
  691. pinMode(D2_FW, OUTPUT);
  692. pinMode(D1_RW, OUTPUT);
  693. pinMode(D2_RW, OUTPUT);
  694.  
  695. setup_PLC();
  696.  
  697. //pinMode(ASPEED, INPUT_PULLUP);
  698. // pinMode(AANGLE, INPUT_PULLUP);
  699.  
  700. // tmrpcm.play("modem.wav");
  701. // delay(14000);
  702.  
  703.  
  704. /*
  705. for (int i = 0 ; i < EEPROM.length() ; i++) {
  706. EEPROM.write(i, 0);
  707. }
  708. */
  709.  
  710.  
  711.  
  712.  
  713.  
  714.  
  715. // pinMode(STARTPIN, INPUT);
  716. // digitalWriteFast(LBACK,LOW);
  717.  
  718.  
  719. // wdt_disable();
  720.  
  721.  
  722.  
  723.  
  724. // Serial.begin(128000);
  725.  
  726. //detachInterrupt(0);
  727. //detachInterrupt(1);
  728.  
  729. //ApplicationMonitor.Dump(Serial);
  730.  
  731.  
  732.  
  733.  
  734. //analogReference(INTERNAL);
  735.  
  736. // Serial.begin(2000000);
  737. spd1 = "0";
  738. spd2 = "0";
  739. pls2 = "0";
  740. pls2 = "0";
  741.  
  742. POSA = 0 ; POSB = 0;
  743. POSA1 = 0 ; POSB1 = 0;
  744. // wdt_enable(WDTO_4S);
  745.  
  746. for (int i=0; i<400; i++)
  747. {
  748. DriveIt(D1_RW,690);
  749. DriveIt(D2_RW,690);
  750. }
  751.  
  752. // digitalWriteFast(LBACK,HIGH);
  753. Serial.println("MOVE...");
  754. while (
  755.  
  756. (digitalRead(GK1)!=LOW || digitalRead(GK2)!=LOW) ||
  757. ((digitalRead(GK1)!=LOW && digitalRead(GK2)!=LOW)) ||
  758. (digitalRead(GK1)!=HIGH || digitalRead(GK2)!=LOW) ||
  759. (digitalRead(GK1)!=LOW || digitalRead(GK2)!=LOW) // <<< - (!digitalRead(GK1)!=LOW || !digitalRead(GK2)!=LOW)
  760.  
  761. )
  762. {
  763.  
  764. // if (SMOOTHPIN(STARTPIN)==LOW) setup();
  765. int spd = 490;
  766. digitalRead(GK1)==LOW || digitalRead(GK2)==LOW?delayMicroseconds(20):delayMicroseconds(0);
  767. if (digitalRead(GK2)==HIGH) DriveIt(D1_RW,spd);
  768. if (digitalRead(GK1)==HIGH) DriveIt(D2_RW,spd);
  769.  
  770. if (digitalRead(GK1) == LOW && digitalRead(GK2) == LOW) break;
  771.  
  772. // if ( digitalRead(GK1)==LOW && digitalRead(GK2)==LOW ) break; // если оба геркона замкнулись то выхожу из цикла
  773.  
  774. }
  775. /// wdt_disable();
  776. Serial.println("MOVE OK");
  777. // digitalWriteFast(LBACK,LOW);
  778. tone(9, 300); delay(100); noTone(9);
  779. tone(9, 600); delay(50); noTone(9);
  780. //tmrpcm.play("modem.wav");
  781. // delay(14000);
  782. // tone(9, 600); delay(50); noTone(9);
  783. //tone(9, 300); delay(100); noTone(9);
  784.  
  785. /*
  786. wdt_enable(WDTO_1S);
  787. while (inputString.indexOf("tre")<1)
  788. {
  789. if (SMOOTHPIN(STARTPIN)==LOW) setup();
  790. for (int i=0; i<100; i++)
  791. {
  792. analogWrite(LEDI,i);
  793. delay(1);
  794. }
  795. Serial.println("matic");
  796. delay(100);
  797. serialEvent();
  798. wdt_reset();
  799.  
  800.  
  801. for (int i=50; i>1; i--)
  802. {
  803. analogWrite(LEDI,i);
  804. delay(1);
  805. }
  806.  
  807. // if (stringComplete)
  808. // Serial.println(String(">")+inputString);
  809. }
  810.  
  811. inputString = "";
  812. */
  813.  
  814. // for (int i=0; i<5; i++) {Serial.println("matic"); delay(100);}
  815. //ApplicationMonitor.EnableWatchdog(Watchdog::CApplicationMonitor::Timeout_1s);
  816.  
  817. wdt_enable(WDTO_2S);
  818. }
  819.  
  820.  
  821.  
  822. int smoothAnal(int APIN,int shift)
  823. {
  824. aspeed = analogRead(APIN);
  825. ar=ar+aspeed;
  826. ari++;
  827. if (ari==300)
  828. {
  829. ari=0;
  830. if (ar/300!=prev_aspeed) {int x = 981-(ar/300); x<0?x=0:x; Serial.print(x);}
  831. prev_aspeed=ar/300;
  832. ar=0;
  833. }
  834. }
  835.  
  836. void loop() {
  837.  
  838. wdt_reset();
  839.  
  840. if (Serial.available()){
  841. is = Serial.readStringUntil('\n');
  842. //// if(s!="[RST]")
  843. wdt_reset();
  844. parseStr(is);
  845. }
  846.  
  847. //ApplicationMonitor.IAmAlive(); // watchdog + eeprom log
  848.  
  849. //udp_read1();
  850.  
  851. //UpdStaticLog(is1,false,0x0000);
  852. //UpdStaticLog(is,true,0xFFFF);
  853. //is1=is;
  854. //SC();
  855. //Serial.println(_str);
  856.  
  857. if (delim=='.' && UP_DOWN)
  858. {
  859. UP_DOWN = false;
  860. parseStr(_str);
  861. // _str = "";
  862. }
  863.  
  864. if (delim==',' && !UP_DOWN)
  865. {
  866. UP_DOWN = true;
  867. }
  868.  
  869. if (spd1!="" && spd2!="" /*&& (pls1!="0" && pls2!="0")*/ ) // если на посл.порт пришли данные, то сброс
  870. {
  871.  
  872. // init1();
  873. speed1 = 0; speed2 = 0;
  874. // Pulses0CNT = 0; Pulses1CNT = 0;
  875. //CNT_DELAY0=-1;
  876. //CNT_DELAY1=-1;
  877. Pulses0 = 10; Pulses1 = 10;
  878. END0=true; //Pulses0 = 0; speed1 = 0;
  879. END1=true; //Pulses1 = 0; speed2 = 0;
  880. if (speed1==0){
  881. int _spd = spd1.toInt();
  882. _spd>9?_spd=9:_spd=_spd; _spd<=0?_spd=1:_spd=_spd;
  883. speed1 = 10 -_spd;
  884. }
  885.  
  886. if (speed2==0){
  887. int __spd = spd2.toInt();
  888. __spd>9?__spd=9:__spd=__spd; __spd<=0?__spd=1:__spd=__spd;
  889. speed2 = 10 - __spd;
  890. }
  891.  
  892. //if (POSA1 == POSA+1 || POSA1 == POSA-1) POSA1 = POSA;
  893. //if (POSB1 == POSB+1 || POSB1 == POSB-1) POSB1 = POSB;
  894.  
  895. if (Pulses0==0 || Pulses0==10)
  896. {
  897.  
  898. int pls_ = pls1.toInt();
  899.  
  900.  
  901. Pulses0 = ((A180 * abs(pls_)*ANGLEC)/GEAR_RATIO) / ANG;
  902.  
  903. if (pls_<0)
  904. {
  905. D1 = D1_FW;
  906. }
  907. else
  908. {
  909. D1 = D1_RW;
  910. }
  911.  
  912. if (abs(POSA-POSA1)>2)
  913. {
  914. // if (POSA-POSA1 > 0) D1 = D1_FW;
  915. // else D1 = D1_RW;
  916. }
  917.  
  918. }
  919. if (Pulses1==0 || Pulses1==10)
  920. {
  921. int pls__ = pls2.toInt();
  922. delim=='.'?Pulses1 = ((A180 * abs(pls__)*ANGLEC)/GEAR_RATIO) / ANG:Pulses1=abs(pls__);
  923. if (pls__<0)
  924. {
  925. // if (POSB+1>POSB1) pls__ = -1; // компенсация погрешности. если абсолютный градус отличается от фактического, то происходит коррекция
  926. D2 = D2_FW;
  927. }
  928. else
  929. {
  930. // if (POSB+1<POSB1) pls__ = 1; // компенсация погрешности. если абсолютный градус отличается от фактического, то происходит коррекция
  931. D2 = D2_RW;
  932. }
  933.  
  934. if (abs(POSB-POSB1)>2)
  935. {
  936. // if (POSB-POSB1 > 0) D2 = D2_FW;
  937. // else D2 = D2_RW;
  938. }
  939.  
  940. }
  941.  
  942. if (Pulses0<11 && Pulses1>11 ) speed2 = speed2*2; // если, в итоге, одному двигателю не нужно импульсов, то уменьшается нагрузка на цп
  943. if (Pulses1<11 && Pulses0>11 ) speed1 = speed1*2; // следовательно второму двигателю нужно уменьшать итоговую скорость в 2 раза.
  944.  
  945. // :} tone(9, abs(pls1.toInt()+pls2.toInt()));
  946.  
  947. }
  948.  
  949. //if (POSA<0)
  950. ///if (POSA<0) DriveItPls(D1_RW,290,((A180 * abs(pls__))/GEAR_RATIO) / 180)
  951.  
  952.  
  953. // Pulses0CNT - импульсы считаются при Low и High цыклах
  954. if (END0 && Pulses0 > 0 && Pulses0*2 > Pulses0CNT && speed1>0 ) /// добавить && speed!=0
  955. {
  956. // micros0 = micros();
  957. if (!i_s)
  958. DriveItAsync0( CNT_DELAY0,speed1,D1);
  959.  
  960. // Serial.println(Pulses0CNT);
  961. // micros0 = micros()-micros0;
  962. }
  963. if ( END0 && (Pulses0CNT+10) > Pulses0*2 )
  964. {
  965. Pulses0CNT=0; Pulses0 = 0; speed1 = 0; END0=false; Pulses0CNT=0; spd1="";
  966. //noTone(9);
  967. if (UP_DOWN) delim='.';
  968. };
  969.  
  970.  
  971. if (END1 && Pulses1 > 0 && Pulses1*2 > Pulses1CNT && speed2>0 )
  972. {
  973. // micros0 = micros();
  974. if (!i_s)
  975. DriveItAsync1( CNT_DELAY1,speed2,D2);
  976.  
  977. // micros0 = micros()-micros0;
  978. // Serial.println(Pulses1CNT);
  979.  
  980. }
  981. if ( END1 && (Pulses1CNT+10) > Pulses1*2 )
  982. {
  983. Pulses1CNT=0; Pulses1 = 0; speed2 = 0; END1=false; Pulses1CNT=0; spd2="";
  984. // noTone(9);
  985. if (UP_DOWN) delim='.';
  986. };
  987.  
  988. if (END0==false && Pulses0>10) {delayMicroseconds(GEAR_RATIO*2*(speed1+speed2)/2);}
  989. if (END1==false && Pulses1>10) {delayMicroseconds(GEAR_RATIO*2*(speed1+speed2)/2);}
  990. // если один из двигателей закончил движение быстрее, вводится доп задержка, чтобы компенсировать ускорение движения в одном из циклов
  991.  
  992.  
  993.  
  994.  
  995. if (speed1>0 && Pulses0*2 != (Pulses0CNT+10))
  996. {
  997. if (speed1p!=speed1) { speed1p = speed1; CNT_DELAY0 = 0; } // если новая скорость отличается от предыдущей то счётчик задержки в 0
  998. CNT_DELAY0++;
  999. if (CNT_DELAY0==speed1) CNT_DELAY0 = 0;
  1000. } // счётчики задержки
  1001.  
  1002. if (speed2>0 && Pulses1*2 != (Pulses1CNT+10))
  1003. {
  1004. if (speed2p!=speed2) { speed2p = speed2; CNT_DELAY1 = 0; } // если новая скорость отличается от предыдущей то счётчик задержки в 0
  1005. CNT_DELAY1++;
  1006. if (CNT_DELAY1==speed2) CNT_DELAY1 = 0;
  1007. } //
  1008.  
  1009. // глюк непоятного поведенеия при динамичской смене скорости заключается тут ^^^^^^
  1010.  
  1011. if (inputString == "xtrematicrst") {Serial.println("matic_reset"); inputString=""; setup(); }
  1012.  
  1013. //if (inputString=="" && AENABLE)
  1014.  
  1015. /*
  1016.  
  1017. { // датчик скорости
  1018. unsigned int x = 0;
  1019. aspeed = analogRead(ASPEED);
  1020. // delayMicroseconds(1);
  1021. ar=ar+aspeed;
  1022. ari++;
  1023. if (ari==A0SMOOTH)
  1024. {
  1025.  
  1026. ari=0;
  1027. if (ar/A0SMOOTH!=prev_aspeed) {
  1028. x = A0SHIFT-(ar/A0SMOOTH);// x<0?x=0:x;
  1029. // if (abreakval==1) x=-1;
  1030. {
  1031. x = SPEED0-x;
  1032. oStr += "S"+String(x)+'\n';
  1033. // oStr += "S"+String(analogRead(ASPEED))+'\n';
  1034.  
  1035. }
  1036. }
  1037. prev_aspeed=ar/A0SMOOTH;
  1038. ar=0;
  1039. // ledi++;
  1040. // analogWrite(LEDI,10+x);
  1041. //if (ledi==255) ledi==0;
  1042. }
  1043.  
  1044.  
  1045.  
  1046. //
  1047.  
  1048.  
  1049.  
  1050. //
  1051.  
  1052. /*
  1053. { // датчик угла руля
  1054. float x = 0;
  1055. int arot = analogRead(AANGLE);
  1056. //delayMicroseconds(1);
  1057. ar1=ar1+arot;
  1058. ari1++;
  1059. if (ari1==A1SMOOTH)
  1060. {
  1061. ari1=0;
  1062. long y = ar1/A1SMOOTH;
  1063. if (ar1/A1SMOOTH!=prev_arot)
  1064. {
  1065.  
  1066.  
  1067. y>prev_arot+1?x = A1SHIFT-(prev_arot+1):y<prev_arot-1?x = A1SHIFT-(prev_arot-1):x = A1SHIFT-prev_arot;
  1068. // ar1/A1SMOOTH>prev_arot+1?x = aangle++ :ar1/A1SMOOTH<prev_arot-1?x = aangle--:x = aangle;
  1069. // x = A1SHIFT-(ar1/A1SMOOTH);
  1070. // x<0?x=0:x = x;
  1071. //x>prev_arot+3?x=prev_arot:x<prev_arot-3?x=prev_arot:x=x;
  1072. oStr+="A"+String(x)+'\n';
  1073. // Serial.print("$"+String(x)+";");
  1074.  
  1075. // oStr+="A"+String(analogRead(AANGLE))+'\n';
  1076. prev_arot = (y+prev_arot)/2;
  1077. // prev_arot = ar1/A1SMOOTH ;
  1078. }
  1079.  
  1080. ar1=0;
  1081. // ledi++;
  1082. // analogWrite(LEDI,10+x);
  1083. //if (ledi==255) ledi==0;
  1084. }
  1085.  
  1086. // датчик тормоза
  1087. //int x;
  1088. abreakval = digitalRead(ABREAK);
  1089. }
  1090.  
  1091.  
  1092. //oStr+="A"+String(A1SHIFT-asx.analogReadSmooth(AANGLE))+'\n';
  1093.  
  1094. // AREADDL=0;
  1095. //}
  1096. */
  1097. { // NOISE DELAY
  1098. FFDELAY++;
  1099. ;
  1100. if (FFDELAY==40) {
  1101. if (oStr!="")
  1102. {
  1103. UDP_SEND(oStr);
  1104.  
  1105.  
  1106. oStr = "";
  1107. }
  1108. else
  1109. {
  1110. // POSA = POSA1;
  1111. // POSB = POSB1;
  1112. // отправляю градус поворота двигателя абсолютный и фактический(реально отработавшие такты)
  1113. // String s = String("X")+String((int)P2A(POSA1))+" "+String(POSA);
  1114. ///Serial.println(s);
  1115. // Serial.println( String("Y")+String((int)P2A(POSB1)) +" "+String(POSB) );
  1116. }
  1117.  
  1118. // analogWrite(LEDI,ar-ar1);
  1119. FFDELAY=0;
  1120.  
  1121. }
  1122. }
  1123.  
  1124.  
  1125.  
  1126. //if (SMOOTHPIN(STARTPIN)==LOW) setup();
  1127.  
  1128.  
  1129.  
  1130. delayMicroseconds(2);
  1131. }
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!