API tools faq
paste
Advertisement
Guest User

Untitled

a guest
May 21st, 2018
91
0
Never
Add comment
Not a member of Pastebin yet? Sign Up, it unlocks many cool features!
text 12.40 KB | None | 0 0
raw download clone embed print report
  1. #include <SoftwareSerial.h>
  2. #include <LCD03.h>
  3. #include <Wire.h>
  4. #include "Time.h"
  5. #include "TimeLib.h"
  6. #include <EEPROM.h>
  7. #include <Sleep_n0m1.h>
  8.  
  9. //MIPROGRAMA
  10. #define CMD (byte)0x00 // MD49 command address of 0
  11. #define GET_SPEED1 0x21 //returns the current requested speed of motor 1
  12. #define GET_SPEED2 0x22 //returns the current requested speed of motor 2
  13. #define GET_ENC1 0x23 //motor 1 encoder count, 4 bytes returned high byte first (signed)
  14. #define GET_ENC2 0x24 //motor 2 encoder count, 4 bytes returned high byte first (signed)
  15. #define GET_ENCS 0x25 //returns 8 bytes - encoder1 count, encoder2 count
  16. #define GET_VOLT 0x26 //returns the input battery voltage level
  17. #define GET_CUR1 0x27 //returns the current drawn by motor 1
  18. #define GET_CUR2 0x28 //returns the current drawn by motor 2
  19. #define GET_VER 0x29 //returns the MD49 software version
  20. #define GET_ACE 0x2A //returns the current acceleration level
  21. #define GET_MOD 0x2B //returns the currently selected mode
  22. #define GET_VI 0x2C //returns battery volts, motor1 current and then motor2 current
  23. #define GET_ERR 0x2D //returns a byte within which the bits indicate errors on the MD49
  24. #define SET_SPEED1 0x31 //set new speed1
  25. #define SET_SPEED2 0x32 //set new speed2 or turn
  26. #define SET_ACCEL 0x33 //set new acceleration
  27. #define SET_MOD 0x34 //set the mode
  28. #define RESET_ENC 0x35 //zero both of the encoder counts
  29. #define DISABLE_REG 0x36 //power output not changed by encoder feedback
  30. #define ENABLE_REG 0x37 //power output is regulated by encoder feedback
  31. #define DISABLE_TOUT 0x38 //MD49 will continuously output with no regular commands
  32. #define ENABLE_TOUT 0x39 //MD49 output will stop after 2 segundos without communication
  33. #define DELAY_MSECS 50
  34. #define M_PI 3.14159265358979323846
  35. #define TICSV 980
  36. #define RADIO 0.06
  37. #define LCD05 0x63 // LCD05 address
  38.  
  39.  
  40. SoftwareSerial motors = SoftwareSerial(0x02, 0x03); // Creates a serial port for the motors
  41. volatile byte enc1a, enc1b, enc1c, enc1d;
  42. volatile byte enc2a, enc2b, enc2c, enc2d;
  43. volatile int32_t en1=0, enAnt1=0;
  44. volatile int32_t en2=0, enAnt2=0;
  45. volatile int32_t tf1=0, tb1=0, tf2=0, tb2=0, tk1=0,tk2=0;
  46. volatile float velocidad1=0, velocidad2=0;
  47. const int32_t minimum = -2147483648;
  48. const int32_t maximum = 2147483647;
  49. const int ticspv = 980; //tics por vuelta
  50. LCD03 lcd;
  51. int muestra = 0; //para contar hasta un seg.
  52. int entero = 0; //para acotar valor de la velocidad
  53. int cero = 0; //para corregir la hora
  54. Sleep sleep; //Sleep mode
  55. int tiempo =0; //Variable para el sleep mode
  56. unsigned long sleepTime = 5000; //Sleep time (ms)
  57. volatile unsigned int seconds = 57; //segundos iniciales
  58. volatile unsigned int minutes = 59; //minutos iniciales
  59. volatile unsigned int hours = 23; //hora inicial
  60. String command = "";
  61. int horas, minutos, segundos; //variables recogidas de la EEPROM
  62. int eeAddress=0; //Direccion EEPROM
  63.  
  64. void setup(){
  65.  
  66. lcd.begin(16,2);
  67. lcd.backlight(); // Turn on the backlight
  68. // Write to the LCD
  69. lcd.print("Starting...");
  70.  
  71. EEPROM.put( eeAddress, hours ); //Grabamos el valor
  72. eeAddress += sizeof(int); //Obtener la siguiente posicion para escribir
  73. EEPROM.put( eeAddress, minutes ); //Grabamos el valor
  74. eeAddress += sizeof(int); //Obtener la siguiente posicion para escribir
  75. EEPROM.put( eeAddress, seconds ); //Grabamos el valor
  76.  
  77. if(eeAddress >= EEPROM.length()) eeAddress = 0; //Comprobar que no hay desbordamiento
  78.  
  79. EEPROM.get( 0, horas);
  80. EEPROM.get( 0+sizeof(int), minutos);
  81. EEPROM.get( 0+sizeof(int)+sizeof(int), segundos);
  82.  
  83. setTime(horas,minutos,segundos,21,05,18);
  84. Wire.begin();
  85. Serial.begin(9600);
  86. motors.begin(9600);
  87. motors.write(CMD); // command byte
  88. motors.write(0x34);
  89. //delay(2);
  90. motors.write(1); //modo 1
  91. //delay(2);
  92. motors.write(CMD); // command byte
  93. motors.write(0x35);
  94. //delay(2);
  95. setSpeed1(100); //velocidad inicial 1
  96. setSpeed2(50); //velocidad inicial 2
  97.  
  98.  
  99. TCCR1A = 0;// set entire TCCR1A register to 0
  100. TCCR1B = 0;// same for TCCR1B
  101. TCNT1 = 0;//initialize counter value to 0
  102. TCCR1B |= (1<<(WGM12));
  103. //TCCR1B |= (1<<(WGM13));
  104. OCR1A = 3124; // cada 50 ms
  105. TCCR1B |= (1<<(CS12)); //prescaler 256
  106. TIMSK1 |= (1<<(OCIE1A)); //por desbordamiento
  107. delay(1500);
  108. lcd.clear();
  109. }
  110.  
  111. void loop(){
  112.  
  113. leerEncoders();
  114. printTime();
  115. printVel();
  116. if(Serial.available()>0){
  117. command=Serial.readStringUntil('\n');
  118. if(command.substring(0,6)=="duerme"){
  119. String numero=command.substring(6,command.length());
  120. tiempo=numero.toInt();
  121. duerme(tiempo);
  122. }
  123.  
  124. }
  125. }
  126. ISR (TIMER1_COMPA_vect){
  127. muestra ++;
  128. if(muestra >=19){
  129. update_clock();
  130. muestra=0;
  131.  
  132. }
  133. //leerEncoders();
  134. }
  135. // Función Sleep
  136. void duerme(int sleepTime ){
  137.  
  138. Serial.print("Me voy a dormir durante ");
  139. Serial.print(sleepTime);
  140. Serial.println(" milisegundos");
  141. sleep.pwrDownMode();
  142. //sleepTime = tiempo * 1000;
  143. sleep.sleepDelay(sleepTime);
  144.  
  145. setSpeed1(100); //velocidad inicial 1
  146. setSpeed2(50); //velocidad inicial 2
  147. Serial.print("Revivo!");
  148. }
  149.  
  150. //Establecer velocidad1
  151. void setSpeed1(int velocidad){
  152. motors.write(CMD);
  153. motors.write(SET_SPEED1);
  154. motors.write(velocidad);
  155. }
  156. //Establecer velocidad2
  157. void setSpeed2(int velocidad){
  158. motors.write(CMD);
  159. motors.write(SET_SPEED2);
  160. motors.write(velocidad);
  161. }
  162. //Establecer aceleración 1
  163. void setAcel(int acel){
  164. motors.write(CMD);
  165. motors.write(SET_ACCEL);
  166. motors.write(acel);
  167. }
  168. //Establecer modo
  169. void setMode(int mode){
  170. motors.write(CMD);
  171. motors.write(SET_MOD);
  172. motors.write(mode);
  173. }
  174. //Obtener Encoder1
  175. int32_t getEncoder1(){
  176. int32_t en1=0;
  177. byte enc1a, enc1b, enc1c, enc1d;
  178. motors.write(CMD);
  179. motors.write(GET_ENC1);
  180. delay(2);
  181. if(motors.available() > 3)
  182. {
  183. enc1a = motors.read();
  184. enc1b = motors.read();
  185. enc1c = motors.read();
  186. enc1d = motors.read();
  187. }
  188. en1=(enc1a << 24) | (enc1b <<16 ) | (enc1c <<8) | enc1d;
  189. return en1;
  190. }
  191.  
  192. int getSpeed1(){
  193. int speed=0;
  194. motors.write(CMD);
  195. motors.write(GET_SPEED1);
  196. if(motors.available() > 0)
  197. {
  198. speed = motors.read();
  199. }
  200. return speed;
  201. }
  202.  
  203. int getSpeed2(){
  204. int speed=0;
  205. motors.write(CMD);
  206. motors.write(GET_SPEED2);
  207. delay(2);
  208. if(motors.available() > 0)
  209. {
  210. speed = motors.read();
  211. }
  212. return speed;
  213. }
  214.  
  215. int32_t getEncoder2(){
  216. int32_t en2=0;
  217. byte enc2a, enc2b, enc2c, enc2d;
  218. motors.write(CMD);
  219. motors.write(GET_ENC2);
  220. delay(2);
  221. if(motors.available() > 3)
  222. {
  223. enc2a = motors.read();
  224. enc2b = motors.read();
  225. enc2c = motors.read();
  226. enc2d = motors.read();
  227. }
  228. en2=(enc2a << 24) | (enc2b <<16 ) | (enc2c <<8) | enc2d;
  229. return en2;
  230. }
  231.  
  232. byte getVoltaje(){
  233. byte v=0;
  234. motors.write(CMD);
  235. motors.write(GET_VOLT);
  236. if(Serial.available()>0){
  237. v=Serial.read();
  238. }
  239.  
  240. return v;
  241. }
  242.  
  243. byte getCurrent1(){
  244. byte c=0;
  245. motors.write(CMD);
  246. motors.write(GET_CUR1);
  247. if(Serial.available()>0){
  248. c=Serial.read();
  249. }
  250. return c;
  251. }
  252.  
  253. byte getCurrent2(){
  254. byte c=0;
  255. motors.write(CMD);
  256. motors.write(GET_CUR2);
  257. if(Serial.available()>0){
  258. c=Serial.read();
  259. }
  260. return c;
  261. }
  262.  
  263. byte getVersion(){
  264. byte ver=0;
  265. motors.write(CMD);
  266. motors.write(GET_VER);
  267. if(Serial.available()>0){
  268. ver=Serial.read();
  269. }
  270. return ver;
  271. }
  272.  
  273. byte getAceleracion(){
  274. byte ac=0;
  275. motors.write(CMD);
  276. motors.write(GET_ACE);
  277. if(Serial.available()>0){
  278. ac=Serial.read();
  279. }
  280. return ac;
  281. }
  282.  
  283. byte getMode() {
  284. byte m=0;
  285. motors.write(CMD);
  286. motors.write(GET_MOD);
  287. if(Serial.available()>0){
  288. m=Serial.read();
  289. }
  290. return m;
  291. }
  292.  
  293. byte getError(){
  294. byte err=0;
  295. motors.write(CMD);
  296. motors.write(GET_ERR);
  297. if(Serial.available()>0){
  298. err=Serial.read();
  299. }
  300. return err;
  301. }
  302.  
  303. void resetEncoders(){
  304. motors.write(CMD);
  305. motors.write(RESET_ENC);
  306. }
  307.  
  308. void disableRegulator(){
  309. motors.write(CMD);
  310. motors.write(DISABLE_REG);
  311. }
  312.  
  313. void enableRegulator(){
  314. motors.write(CMD);
  315. motors.write(ENABLE_REG);
  316. }
  317.  
  318. void disableTimeout(){
  319. motors.write(CMD);
  320. motors.write(DISABLE_TOUT);
  321. }
  322.  
  323. void enableTimeout(){
  324. motors.write(CMD);
  325. motors.write(ENABLE_TOUT);
  326. }
  327.  
  328.  
  329.  
  330.  
  331. void leerEncoders(){
  332. // TIMSK1 |= (0<< OCIE1A);
  333. motors.write(CMD);
  334.  
  335. motors.write(GET_ENC1); // Recieve encoder 1 value
  336. delay(2);
  337.  
  338. if(motors.available() > 3)
  339. {
  340. enc1a = motors.read();
  341. enc1b = motors.read();
  342. enc1c = motors.read();
  343. enc1d = motors.read();
  344. }
  345. motors.write(CMD);
  346.  
  347. motors.write(GET_ENC2); // Recieve encoder 2 value
  348. delay(2);
  349.  
  350. if(motors.available() > 3)
  351. {
  352. enc2a = motors.read();
  353. enc2b = motors.read();
  354. enc2c = motors.read();
  355. enc2d = motors.read();
  356. }
  357.  
  358. /*Serial.println(enc1a);
  359. Serial.println(enc1b);
  360. Serial.println(enc1c);
  361. Serial.println(enc1d);*/
  362. en1 = (static_cast<uint32_t>(enc1a) << 24) +
  363. (static_cast<uint32_t>(enc1b)<<16) +
  364. (static_cast<uint32_t>(enc1c)<<8) +
  365. static_cast<uint32_t>(enc1d);
  366. en2 = (static_cast<uint32_t>(enc2a) << 24) +
  367. (static_cast<uint32_t>(enc2b)<<16) +
  368. (static_cast<uint32_t>(enc2c)<<8) +
  369. static_cast<uint32_t>(enc2d);
  370. /*
  371. Serial.print("enc1: [");
  372. Serial.print(enc1a,HEX); Serial.print(",");
  373. Serial.print(enc1b,HEX); Serial.print(",");
  374. Serial.print(enc1c,HEX); Serial.print(",");
  375. Serial.print(enc1d,HEX);
  376. Serial.println("]");
  377. Serial.print("enc1: "); Serial.print(en1); Serial.print(" (0x"); Serial.print(en1,HEX); Serial.println(")");
  378. Serial.print("enc2: [");
  379. Serial.print(enc2a,HEX); Serial.print(",");
  380. Serial.print(enc2b,HEX); Serial.print(",");
  381. Serial.print(enc2c,HEX); Serial.print(",");
  382. Serial.print(enc2d,HEX);
  383. Serial.println("]");
  384. Serial.print("enc2: "); Serial.print(en2); Serial.print(" (0x"); Serial.print(en2,HEX); Serial.println(")");
  385. */
  386. //TIMSK1 |= (1<< OCIE1A);
  387.  
  388.  
  389.  
  390. //FORWARD
  391. if(en1>=enAnt1){
  392. tf1=en1-enAnt1;
  393. }else{
  394. tf1=maximum-minimum+en1-enAnt1;
  395. }
  396.  
  397. if(en2>=enAnt2){
  398. tf2=en2-enAnt2;
  399. }else{
  400. tf2=maximum-minimum+en2-enAnt2;
  401. }
  402.  
  403. //BACKWARD
  404. if(en1<=enAnt1){
  405. tb1=enAnt1-en1;
  406. }else{
  407. tb1=maximum-minimum+enAnt1-en1;
  408. }
  409.  
  410. if(en2<=enAnt2){
  411. tb2=enAnt2-en2;
  412. }else{
  413. tb2=maximum-minimum+enAnt2-en1;
  414. }
  415.  
  416. //OBTENER TICKS POR PERIODO
  417. if(tb1<tf1){
  418. tk1=-tb1;
  419. }else{
  420. tk1=tf1;
  421. }
  422.  
  423. if(tb2<tf2){
  424. tk2=-tb2;
  425. }else{
  426. tk2=tf2;
  427. }
  428.  
  429. velocidad1=((tk1*2*M_PI/TICSV)/0.05)*RADIO;
  430. velocidad2=((tk2*2*M_PI/TICSV)/0.05)*RADIO;
  431.  
  432.  
  433. enAnt1=en1;
  434. enAnt2=en2;
  435. // Serial.println(velocidad1);
  436.  
  437. //Serial.println(velocidad2);
  438. }
  439.  
  440. void printTime(){
  441. time_t t = now();
  442.  
  443.  
  444.  
  445. lcd.home();
  446. lcd.print(day(t));
  447. lcd.print(+ "/") ;
  448. lcd.print(month(t));
  449. lcd.print(+ "/") ;
  450. lcd.print(year(t) - 2000);
  451. //lcd.print( " ") ;
  452.  
  453. lcd.setCursor(8, 0);
  454. if (horas<10){
  455. lcd.print(cero);
  456. }
  457. lcd.print(horas);
  458. lcd.setCursor(10,0);
  459. lcd.print(":");
  460. lcd.setCursor(11,0);
  461. if (minutos<10){
  462. lcd.print(cero);
  463. }
  464. lcd.print(minutos);
  465. lcd.setCursor(13,0);
  466. lcd.print(":");
  467. lcd.setCursor(14,0);
  468. if (segundos<10){
  469. lcd.print(cero);
  470. }
  471. lcd.print(segundos);
  472.  
  473.  
  474.  
  475. }
  476. void printVel(){
  477. delay(300);
  478. lcd.setCursor(0,1);
  479. lcd.print("v1:");
  480. entero = velocidad1*100;
  481. lcd.print(entero/100);
  482. lcd.setCursor(8,1);
  483. lcd.print("v2:");
  484. entero = velocidad2*100;
  485. lcd.print(entero/100);
  486.  
  487.  
  488. }
  489.  
  490. void update_clock()
  491. {
  492. segundos++;
  493. if (segundos == 60)
  494. {
  495. segundos = 0;
  496. minutos++;
  497. }
  498. if(minutos==60)
  499. {
  500. minutos=0;
  501. horas++;
  502. }
  503. if(horas>23)
  504. {
  505. horas=0;
  506. }
  507.  
  508.  
  509. }
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!