API tools faq
paste
Advertisement
Guest User

Untitled

a guest
Jul 15th, 2018
253
0
Never
Add comment
Not a member of Pastebin yet? Sign Up, it unlocks many cool features!
text 17.61 KB | None | 0 0
raw download clone embed print report
  1. //------------------------------------------------CONFIGURAÇOES INICIAIS------------------------------------------------
  2. #include <VarSpeedServo.h>
  3. #include <Ultrasonic.h>
  4. #include <Stepper.h>
  5.  
  6. // CONFIGURAÇÃO ULTRASÔNICO
  7. #define pino_trigger 35
  8. #define pino_echo 34
  9. Ultrasonic ultrasonic(pino_trigger, pino_echo);
  10.  
  11. // CONFIGURAÇAO STEPPER
  12. const int stepsPerRevolution = 500;
  13. Stepper myStepper(stepsPerRevolution,9,10,11,12);
  14.  
  15. // CONFIGURAÇÃO SERVOS
  16. VarSpeedServo garra;
  17. VarSpeedServo base;
  18. VarSpeedServo frente;
  19. VarSpeedServo altura;
  20. const int basePin2 = 2;
  21. const int frentePin3 = 3;
  22. const int alturaPin7 = 7;
  23. const int garraPin8 = 8;
  24.  
  25. struct ControleServo{
  26. int posicaoAtual;
  27. int posicaoInicial;
  28. int maximo;
  29. int minimo;
  30. };
  31. struct ControleUltrasonico{
  32. float posicaoAtual;
  33. float minimo;
  34. float maximo;
  35.  
  36. };
  37.  
  38. ControleUltrasonico ultrasonico ={3.5,3.5,15.2};
  39.  
  40. ControleServo servoGarra ={15,15,20,2}; // Minimo e maximo definidos
  41. ControleServo servoBase ={120,120,140,0}; // Minimo e maximo nao definidos
  42. ControleServo servoAltura={120,120,150,30}; // Não foi definido o minimo e o máximo
  43. ControleServo servoFrente={110,110,20,150}; // Não foi definido o minimo e o máximo
  44.  
  45. // FALTA CONTROLAR OS SERVOS PARA LIMITAR PARA SE MOVER AO MAXIMO E MINIMO , QUESTAO DE SEGURANÇA
  46.  
  47. struct ControleCiclo{
  48. int X1;
  49. int Y1;
  50. int Z1;
  51. int X2;
  52. int Y2;
  53. int Z2;
  54. int X3;
  55. int Y3;
  56. int Z3;
  57. int X4;
  58. int Y4;
  59. int Z4;
  60. int tamanho; // Número de passos do Ciclo incluem passos de abrir garra e fechar garra
  61. };
  62. ControleCiclo ciclo1 ={120,40,30,0,0,0,0,0,0,0,0,0};
  63.  
  64. char mydata = 0;
  65. bool direcao;
  66. unsigned long timeini,timefim;
  67. //------------------------------------------------SETUP------------------------------------------------
  68. void setup() {
  69. // initialize the digital pin as an output.
  70. myStepper.setSpeed(40);
  71. base.attach(basePin2);
  72. frente.attach(frentePin3);
  73. altura.attach(alturaPin7);
  74. garra.attach(garraPin8);
  75. altura.write(servoAltura.posicaoInicial,10);
  76. base.write(servoBase.posicaoInicial,10);
  77. garra.write(servoGarra.posicaoInicial,10);
  78. Serial.begin(9600);
  79. delay(500);
  80. }
  81.  
  82. //-------------------------------------------FUNÇÕES UTILIZADAS NO LOOP-------------------------------
  83. //***********************************FUNÇÃO PARA LER PORTA SERIAL****************************************
  84. String leStringSerial() {
  85. String conteudo = "";
  86. char caractere;
  87. // Enquanto receber algo pela serial
  88. while (Serial.available() > 0) {
  89. // Lê byte da serial
  90. caractere = Serial.read();
  91. // Ignora caractere de quebra de linha
  92. if (caractere != '\n') {
  93. // Concatena valores
  94. conteudo.concat(caractere);
  95. }
  96. // Aguarda buffer serial ler próximo caractere
  97. delay(10);
  98. }
  99. return conteudo;
  100. }
  101.  
  102. void leUltrasonico(ControleUltrasonico *ultra){
  103.  
  104. long microsec = ultrasonic.timing();
  105. ultra->posicaoAtual = ultrasonic.convert(microsec, Ultrasonic::CM);
  106. Serial.print("U");
  107. Serial.println(ultra->posicaoAtual);
  108. }
  109.  
  110. void escreveSerial(){
  111. Serial.print("B");
  112. Serial.println(servoBase.posicaoAtual);
  113. delay(50);
  114. Serial.print("G");
  115. Serial.println(servoGarra.posicaoAtual);
  116. delay(50);
  117. Serial.print("A");
  118. Serial.println(servoAltura.posicaoAtual);
  119. delay(50);
  120. Serial.print("F");
  121. Serial.println(servoFrente.posicaoAtual);
  122. delay(50);
  123. Serial.print("U");
  124. Serial.println(ultrasonico.posicaoAtual);
  125. }
  126.  
  127. void escreveSerialOK(String letrasModoFim,long tempo){
  128. Serial.print(letrasModoFim);
  129. Serial.println(tempo);
  130. }
  131. //************************************ FUNÇÃO MOVE SERVO PARA O MODO MANUAL ********************************************************
  132. int moveServo(VarSpeedServo s, int graus, bool dir,ControleServo *servo) {
  133. if (dir==true){
  134. s.write(graus+servo->posicaoAtual,10);
  135. // servoBase.posicaoAtual=servoBase.posicaoAtual+graus;
  136. servo->posicaoAtual=servo->posicaoAtual+graus;
  137. }else {
  138. s.write(servo->posicaoAtual-graus,10); // Move o servo para posição atual mais o definido pela interface em py
  139. servo->posicaoAtual=servo->posicaoAtual-graus;
  140. }
  141. escreveSerial();
  142.  
  143. delay(50);
  144. }
  145.  
  146. int moveMotorDePasso(Stepper s,bool dir){
  147. if (dir==true){
  148. for(int i=0;i<10;i++){
  149. s.setSpeed(40);
  150. s.step(64);
  151. delay(1);
  152. // falta atualizar posicão atual to ultrasonico
  153. }
  154. }else {
  155. for(int i=0;i<10;i++){
  156. s.step(-64);
  157. s.setSpeed(-40);
  158. delay(1);
  159. }
  160. }
  161. }
  162.  
  163. void moverAutomaticoMotorDePasso(Stepper s,bool dir){
  164. if(dir==true){
  165. while(ultrasonico.posicaoAtual<ultrasonico.maximo){
  166. moveMotorDePasso(myStepper,true);
  167. leUltrasonico(&ultrasonico);
  168. }
  169. // Move para o ponto máximo
  170. }else{
  171. // Move para o ponto minimo
  172. while(ultrasonico.posicaoAtual>ultrasonico.minimo){
  173. moveMotorDePasso(myStepper,false);
  174. leUltrasonico(&ultrasonico);
  175. }
  176. }
  177. }
  178. //********************************** FUNÇÃO PARA MOVER OS SERVOS DE COORDENADAS PREDEFINIDAS PARA OUTRAS - PARA CICLOS E MODO AUTOMÁTICO************
  179. void MoverCoordenadas(int iniX, int iniY, int iniZ, int fimX,int fimY, int fimZ,ControleServo *sFrente,ControleServo *sBase,ControleServo *sAltura)
  180. {
  181.  
  182. int grauServo; //grau em que o servo deve estar (todos os servos usar esta variavel, um de cada vez)
  183. int varX = fimX-iniX; //variação entre o ponto inicial e ponto final que o servo responsavel em ir para frente e para traz tem
  184. int varY = fimY-iniY;
  185. int varZ = fimZ-iniZ;
  186. int deltaVar=0; // armazena a maior variacao
  187.  
  188. // pega a maior variacao para garantir o tempo de execucao do processo
  189. if(abs(varX)>=abs(varZ))
  190. {
  191. deltaVar=abs(varX);
  192. } else
  193. {
  194. deltaVar=abs(varZ);
  195. }
  196. if(deltaVar<abs(varY))
  197. {
  198. deltaVar=abs(varY);
  199. }
  200.  
  201. // movimentacao dos servos
  202. for(int i=0; i<deltaVar; i++)
  203. {
  204. grauServo = int(iniX+float(i*float(float(varX)/float(deltaVar))));
  205. frente.write(grauServo);
  206.  
  207. grauServo = int(iniY+float(i*float(float(varY)/float(deltaVar))));
  208. base.write(grauServo);
  209.  
  210. grauServo = int(iniZ+float(i*float(float(varZ)/float(deltaVar))));
  211. altura.write(grauServo);
  212.  
  213. delay(15);
  214. }
  215. // Atualiza a posição atual de todos servos
  216. sFrente->posicaoAtual = sFrente->posicaoAtual+(fimX-iniX);
  217. sBase->posicaoAtual = sBase->posicaoAtual+(fimY-iniY);
  218. sAltura->posicaoAtual = sAltura->posicaoAtual+(fimZ-iniZ);
  219. }
  220.  
  221. //************************************ FUNÇÃO FECHA/ABRE GARRA PARA MODO CICLOS ********************************************************
  222. void FechaAbreGarra(VarSpeedServo g,ControleServo *sGarra,bool dir){
  223. if (dir == false){
  224. g.write(sGarra->maximo,10);
  225. sGarra->posicaoAtual = sGarra->minimo;
  226.  
  227. }else{
  228. //int graus1 =sGarra->posicaoAtual - sGarra->minimo;
  229. g.write(sGarra->minimo,10);
  230. sGarra->posicaoAtual=sGarra->minimo;
  231. }
  232.  
  233. }
  234.  
  235. void MoveCiclo1(){
  236. // BASE -> QUANTO MENOR,MAIS PRO LADO DA ESTEIRA
  237. // ALTURA -> QUANTO MAIOR, MAIS PRA BAIXO VAI
  238. // FRENTE -> QUANTO MENOR, MAIS PRA FRENTE VAI
  239. MoverCoordenadas(servoFrente.posicaoAtual,servoBase.posicaoAtual,servoAltura.posicaoAtual,ciclo1.X1,ciclo1.Y1,ciclo1.Z1,&servoFrente,&servoBase,&servoAltura);
  240. escreveSerial();
  241. delay(100);
  242. // MoverCoordenadas(servoFrente.posicaoAtual,servoBase.posicaoAtual,servoAltura.posicaoAtual,ciclo1.X2,ciclo1.Y2,ciclo1.Z2,&servoFrente,&servoBase,&servoAltura);
  243. // delay(100);
  244. FechaAbreGarra(garra,&servoGarra,true); // Fecha a gara
  245. escreveSerial();
  246. delay(100);
  247. FechaAbreGarra(garra,&servoGarra,false);
  248. escreveSerial();
  249. // MoverCoordenadas(servoFrente.posicaoAtual,servoBase.posicaoAtual,servoAltura.posicaoAtual,servoFrente.posicaoInicial,servoBase.posicaoAtual,servoAltura.posicaoAtual,&servoFrente,&servoBase,&servoAltura);
  250. // delay(100);
  251. // FechaAbreGarra(garra,&servoGarra,0); // Abre a garra
  252. }
  253.  
  254. void MoveCiclo2(){
  255. // BASE -> QUANTO MENOR,MAIS PRO LADO DA ESTEIRA
  256. // ALTURA -> QUANTO MAIOR, MAIS PRA BAIXO VAI
  257. // FRENTE -> QUANTO MENOR, MAIS PRA FRENTE VAI
  258. MoverCoordenadas(servoFrente.posicaoAtual,servoBase.posicaoAtual,servoAltura.posicaoAtual,ciclo1.X1,ciclo1.Y1,ciclo1.Z1,&servoFrente,&servoBase,&servoAltura);
  259. escreveSerial();
  260. delay(100);
  261. // MoverCoordenadas(servoFrente.posicaoAtual,servoBase.posicaoAtual,servoAltura.posicaoAtual,ciclo1.X2,ciclo1.Y2,ciclo1.Z2,&servoFrente,&servoBase,&servoAltura);
  262. // delay(100);
  263. FechaAbreGarra(garra,&servoGarra,true); // Fecha a gara
  264. escreveSerial();
  265. delay(100);
  266. FechaAbreGarra(garra,&servoGarra,false);
  267. escreveSerial();
  268. // MoverCoordenadas(servoFrente.posicaoAtual,servoBase.posicaoAtual,servoAltura.posicaoAtual,servoFrente.posicaoInicial,servoBase.posicaoAtual,servoAltura.posicaoAtual,&servoFrente,&servoBase,&servoAltura);
  269. // delay(100);
  270. // FechaAbreGarra(garra,&servoGarra,0); // Abre a garra
  271. }
  272.  
  273. void MoveCiclo3(){
  274. // BASE -> QUANTO MENOR,MAIS PRO LADO DA ESTEIRA
  275. // ALTURA -> QUANTO MAIOR, MAIS PRA BAIXO VAI
  276. // FRENTE -> QUANTO MENOR, MAIS PRA FRENTE VAI
  277. MoverCoordenadas(servoFrente.posicaoAtual,servoBase.posicaoAtual,servoAltura.posicaoAtual,ciclo1.X1,ciclo1.Y1,ciclo1.Z1,&servoFrente,&servoBase,&servoAltura);
  278. escreveSerial();
  279. delay(100);
  280. // MoverCoordenadas(servoFrente.posicaoAtual,servoBase.posicaoAtual,servoAltura.posicaoAtual,ciclo1.X2,ciclo1.Y2,ciclo1.Z2,&servoFrente,&servoBase,&servoAltura);
  281. // delay(100);
  282. FechaAbreGarra(garra,&servoGarra,true); // Fecha a gara
  283. escreveSerial();
  284. delay(100);
  285. FechaAbreGarra(garra,&servoGarra,false);
  286. escreveSerial();
  287. // MoverCoordenadas(servoFrente.posicaoAtual,servoBase.posicaoAtual,servoAltura.posicaoAtual,servoFrente.posicaoInicial,servoBase.posicaoAtual,servoAltura.posicaoAtual,&servoFrente,&servoBase,&servoAltura);
  288. // delay(100);
  289. // FechaAbreGarra(garra,&servoGarra,0); // Abre a garra
  290. }
  291.  
  292. void MoveCiclo4(){
  293. // BASE -> QUANTO MENOR,MAIS PRO LADO DA ESTEIRA
  294. // ALTURA -> QUANTO MAIOR, MAIS PRA BAIXO VAI
  295. // FRENTE -> QUANTO MENOR, MAIS PRA FRENTE VAI
  296. MoverCoordenadas(servoFrente.posicaoAtual,servoBase.posicaoAtual,servoAltura.posicaoAtual,ciclo1.X1,ciclo1.Y1,ciclo1.Z1,&servoFrente,&servoBase,&servoAltura);
  297. escreveSerial();
  298. delay(100);
  299. // MoverCoordenadas(servoFrente.posicaoAtual,servoBase.posicaoAtual,servoAltura.posicaoAtual,ciclo1.X2,ciclo1.Y2,ciclo1.Z2,&servoFrente,&servoBase,&servoAltura);
  300. // delay(100);
  301. FechaAbreGarra(garra,&servoGarra,true); // Fecha a gara
  302. escreveSerial();
  303. delay(100);
  304. FechaAbreGarra(garra,&servoGarra,false);
  305. escreveSerial();
  306. // MoverCoordenadas(servoFrente.posicaoAtual,servoBase.posicaoAtual,servoAltura.posicaoAtual,servoFrente.posicaoInicial,servoBase.posicaoAtual,servoAltura.posicaoAtual,&servoFrente,&servoBase,&servoAltura);
  307. // delay(100);
  308. // FechaAbreGarra(garra,&servoGarra,0); // Abre a garra
  309. }
  310.  
  311. //------------------------------------------------LOOP--------------------------------------------------------
  312. void loop(){
  313. // Vai ler o serial sempre!!!
  314. String mydata = leStringSerial();
  315. String mydata1,mydata2;
  316. String automaticoCil1,automaticoCil2,automaticoCil3,automaticoCil4;
  317. int graus;
  318.  
  319. // Primeiro if : comando de operação mais graus entre 0 e 9
  320. // Segundo if : comando de operação mais graus a partir de 10
  321. // Terceiro else: comando de operação para automático mais Ciclos que irão executar no automático
  322. if(mydata.length()<=5){
  323. // Subdivide a sstring recebida pelo serial
  324. mydata1 = mydata.substring(0, 3);
  325. mydata2 = mydata.substring(3);
  326. graus = mydata2.toInt();
  327. }else if(mydata.length() == 6){
  328. mydata1 = mydata.substring(0,3);
  329. mydata2 = mydata.substring(3,5);
  330. graus = mydata2.toInt();
  331. }else{
  332. mydata1=mydata.substring(0,1);
  333. automaticoCil1=mydata.substring(1,3);
  334. automaticoCil2=mydata.substring(3,5);
  335. automaticoCil3=mydata.substring(5,7);
  336. automaticoCil4=mydata.substring(7,9);
  337. }
  338.  
  339. delay(2);
  340.  
  341. //------------------------------------------------ MODO MANUAL------------------------------------------------
  342.  
  343. //*********************************** MODO MANUAL - M01-M02 - SERVO BASE****************************************
  344. if (mydata1 == "M00" || mydata1 == "M01" ) {
  345. if (mydata1 == "M00") {
  346. direcao = true;
  347. moveServo(base, graus, direcao,&servoBase);
  348.  
  349. } else {
  350. direcao = false;
  351. moveServo(base, graus, direcao,&servoBase);
  352. }
  353. }
  354.  
  355. //*********************************** MODO MANUAL - M10-M11 - SERVO FRENTE****************************************
  356. if (mydata1 == "M10" || mydata1 == "M11" ) {
  357. if (mydata1 == "M10") {
  358. direcao = false;
  359. moveServo(frente, graus, direcao,&servoFrente);
  360.  
  361. } else {
  362. direcao = true;
  363. moveServo(frente, graus, direcao,&servoFrente);
  364. }
  365. }
  366.  
  367.  
  368. //*********************************** MODO MANUAL - M20-M21 - SERVO ALTURA****************************************
  369. if (mydata1 == "M20" || mydata1 == "M21" ) {
  370. if (mydata1 == "M20") {
  371. direcao = true;
  372. moveServo(altura, graus, direcao,&servoAltura);
  373.  
  374. } else {
  375. direcao = false;
  376. moveServo(altura, graus, direcao,&servoAltura);
  377. }
  378. }
  379. //*********************************** MODO MANUAL - M30-M31 - SERVO GARRA****************************************
  380. // Direção = true Fecha a garra
  381. // Direção = false Abre a garra
  382. // Garra 0 graus aberta, 25 fechada
  383. if (mydata1 == "M30" || mydata1 == "M31" ) {
  384. if (mydata1 == "M30") {
  385. // Fecha a garra
  386. FechaAbreGarra(garra,&servoGarra,true);
  387.  
  388. } else {
  389. FechaAbreGarra(garra,&servoGarra,false);
  390. }
  391. }
  392. //*********************************** MODO MANUAL - M40-M41 - MOTOR DE PASSO ESTEIRA****************************************
  393.  
  394. // Preciso atualizar a distancia do pallet enquanto estiver movimentando
  395. if(mydata1=="M40" || mydata1=="M41"){
  396. if (mydata1 == "M40"){
  397. moveMotorDePasso(myStepper,true) ;
  398. }else{
  399. moveMotorDePasso(myStepper,false);
  400. }
  401. }
  402.  
  403.  
  404. //---------------------------------------------MODO CICLOS---------------------------------------------------------------
  405. //*********************************************MODO CICLOS - CICLO 1************************************************************
  406. // PARA EXECUTAR É PRECISO QUE O ROBÔ ESTEJA REPOSICIONADO PARA A ORIGEM, ESTÁ CONDIÇÃO SERÁ TESTADA PELA INTERFACE !!!
  407. if (mydata1 == "C10"){
  408. timeini=millis()/1000;
  409. MoveCiclo1();
  410. timefim=millis()/1000;
  411. escreveSerialOK("CK",timefim-timeini);
  412. delay(5);
  413. }
  414.  
  415. //*********************************************MODO CICLOS - CICLO 2************************************************************
  416. // PARA EXECUTAR É PRECISO QUE O ROBÔ ESTEJA REPOSICIONADO PARA A ORIGEM, ESTÁ CONDIÇÃO SERÁ TESTADA PELA INTERFACE !!!
  417. if (mydata1 == "C20"){
  418. timeini=millis()/1000;
  419. MoveCiclo2();
  420. timefim=millis()/1000;
  421. escreveSerialOK("CK",timefim-timeini);
  422. delay(5);
  423. }
  424.  
  425. //*********************************************MODO CICLOS - CICLO 3************************************************************
  426. // PARA EXECUTAR É PRECISO QUE O ROBÔ ESTEJA REPOSICIONADO PARA A ORIGEM, ESTÁ CONDIÇÃO SERÁ TESTADA PELA INTERFACE !!!
  427. if (mydata1 == "C30"){
  428. timeini=millis()/1000;
  429. MoveCiclo3();
  430. timefim=millis()/1000;
  431. escreveSerialOK("CK",timefim-timeini);
  432. delay(50);
  433. }
  434.  
  435.  
  436. //*********************************************MODO CICLOS - CICLO 4************************************************************
  437. // PARA EXECUTAR É PRECISO QUE O ROBÔ ESTEJA REPOSICIONADO PARA A ORIGEM, ESTÁ CONDIÇÃO SERÁ TESTADA PELA INTERFACE !!!
  438. if (mydata1 == "C40"){
  439. timeini=millis()/1000;
  440. MoveCiclo4();
  441. timefim=millis()/1000;
  442. escreveSerialOK("CK",timefim-timeini);
  443. delay(5);
  444. }
  445.  
  446. ///-----------------------------------MODO REPOSICIONAR-----------------------------------------------------------
  447. if (mydata1 == "RE0"){
  448. garra.write(servoGarra.posicaoInicial,10);
  449. frente.write(servoFrente.posicaoInicial,10);
  450. altura.write(servoAltura.posicaoInicial,10);
  451. base.write(servoBase.posicaoInicial,10);
  452. moverAutomaticoMotorDePasso(myStepper,false);
  453. escreveSerialOK('R');
  454. }
  455.  
  456. ///-----------------------------------MODO AUTOMÁTICO-----------------------------------------------------------
  457. // OS CICLOS QUE ENTRAM NO MODO AUTOMÁTICO SERÁ DE ACORDO COM O COMANDO ENVIADO PELA SERIAL PROVENIENTE DA INTERFACE.
  458. // É POSSIVEL OPTAR POR QUAIS CICLOS O USUÁRIO DESEJA QUE ENTRE EM AUTOMÁTICO
  459. // ANTES DE CHAMAR A FUNÇÃO PARA INICIAR O CICLO,DEVE-SE AVANÇAR A ESTEIRA ATÉ O SEU PONTO MÁXIMO.
  460. // APÓS O TERMÍNO DEVE-SE VOLTAR A ESTEIRA ATÉ O PONTO MÍNIMO
  461. if (mydata1 == "A"){
  462. timeini=millis()/1000;
  463. if(automaticoCil1=="C1"){
  464. moverAutomaticoMotorDePasso(myStepper,true);
  465. MoveCiclo1();
  466. moverAutomaticoMotorDePasso(myStepper,false);
  467. }
  468. if(automaticoCil2=="C2"){
  469. moverAutomaticoMotorDePasso(myStepper,true);
  470. MoveCiclo2();
  471. moverAutomaticoMotorDePasso(myStepper,false);
  472. }
  473. if(automaticoCil3=="C3"){
  474. moverAutomaticoMotorDePasso(myStepper,true);
  475. MoveCiclo3();
  476. moverAutomaticoMotorDePasso(myStepper,false);
  477. }
  478. if(automaticoCil4=="C4"){
  479. moverAutomaticoMotorDePasso(myStepper,true);
  480. MoveCiclo4();
  481. moverAutomaticoMotorDePasso(myStepper,false);
  482. }
  483. timefim=millis()/1000;
  484. escreveSerialOK("AK",timefim-timeini);
  485.  
  486. }
  487.  
  488. //------------------------------------------- LEITURA ULTRASÔNICO-----------------------------------------------------------
  489. leUltrasonico(&ultrasonico);
  490.  
  491. //-------------------------------------------- ESCREVE SERIAL TODO LOOP-----------------------------------------------------
  492. escreveSerial();
  493.  
  494. }
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!